# Robust Optimal Navigation Using Nonlinear Model Predictive Control Method Combined with Recurrent Fuzzy Neural Network

Authors: Qidan Zhu; Yu Han; Chengtao Cai; Yao Xiao
Journal: Mathematical Problems in Engineering
 (2018)

---

## Abstract

This paper presents a novel navigation strategy of robot to achieve reaching target and obstacle avoidance in unknown dynamic environment. Considering possible generation of uncertainty, disturbances brought to system are separated into two parts, i.e., bounded part and unbounded part. A dual-layer closed-loop control system is then designed to deal with two kinds of disturbances, respectively. In order to realize global optimization of navigation, recurrent fuzzy neural network is used to predict optimal motion of robot for its ability of processing nonlinearity and learning. Extended Kalman filter method is used to train RFNN online. Moving horizon technique is used for RFNN motion planner to guarantee optimization in dynamic environment. Then, model predictive control is designed against bounded disturbances to drive robot to track predicted trajectories and limit robot’s position in a tube with good robustness. A novel iterative online learning method is also proposed to estimate intrinsic error of system using online data that makes system adaptive. Feasibility and stability of proposed method are analyzed. By examining our navigation method on mobile robot, effectiveness is proved in both simulation and hardware experiments. Robustness and optimization of proposed navigation method can be guaranteed in dynamic environment.


---

## Body

## 1. Introduction

During past decades, navigation which can fully reflect artificial intelligence and automatic ability of robot has been an attractive topic [1]. Autonomous navigation is realized in kinds of robots like ground mobile vehicles, unmanned underwater vehicle (UUV), unmanned aircraft, etc. [2–5]. These robots with navigation technology have been applied popularly in various fields such as industry, public service places, transportation, and military [6]. Generally, navigation structure is made up of three main components, perception, decision, and control. Different sensors, such as lidar, inertial measurement unit, GPS, and visual camera, have been used for this purpose generating a series of solutions. This is beyond this paper’s scope. The main problems that this paper prefers to solve are decision and control under assumption of known perception capability.Navigation of robot in objective world is complex for the reasons of existing various uncertainty and nonlinearity of system [7, 8]. Disturbance or perturbation caused by uncertainty brings challenge to design navigation system. Many inevitable factors lead to uncertainty like dynamic circumstances, sensor noises, and model error [9]. They are coupled and impact together on system. In order to achieve real-time navigation and guarantee safety of robot taking obstacles into account, nonlinear optimal program of robot motion needs to be carried out [10]. However, to make system stable and robust under this condition, control strategy can be tedious and time-consuming. So, effective method should be designed to deal with optimal robust problem of the nonlinear system.Fuzzy logic control is well suited to control robot for its accurate calculation capability and inference capability under uncertainty [11]. Many researchers have implemented this method to deal with navigation of robot. Wang [12] proposed a real-time fuzzy logic control in unknown environment to achieve obstacle avoidance. However, fuzzy logic based method is too sample to deal with complex problem and the structure is less of flexibility. Neural network based approaches have been widely employed in closed-loop control for their strong nonlinear approximation and self-learning capability [13]. Many researches have been done on feedforward multilayer perception neural network for classification, recognition, and function approximation. In [14], recurrent neural network is used to solve optimal navigation problem of multirobot. By constructing formation and using penalty method, navigation becomes convex optimization problem. Recently, fuzzy logic and recurrent neural network structure are combined to form a new structure, i.e., recurrent fuzzy neural network (RFNN). RFNN combines both advantages of neural network and fuzzy logic and earns good nonlinear control capability [15]. In [16], interval type 2 fuzzy neural network is used to control robot. By tuning parameters of structure with genetic algorithm, stable navigation is realized. However, optimization of navigation is not guaranteed and disturbances are not considered in their method.Model predictive control (MPC) has been successfully applied to process industries during past decades for its online optimization programming ability [17]. At each time-step, MPC obtains a finite sequence of control by solving a constrained, discrete-time, optimal control problem over prediction horizon. Current control variable of the sequence is regarded as control law to be applied to the system. By solving the optimal control problem within one sampling interval, MPC can effectively manage MIMO system and hard constraint system. Robust nonlinear MPC (RNMPC) is an active area of research and can easily handle constraint satisfaction against dynamic uncertainty [18]. If a controlled system is ISS with respect to bounded uncertainties, it has a stability margin and is therefore stable with respect to unmodeled dynamics [19]. Many researches have been published using MPC method realizing optimal navigation [20]. In [21], a dual-layer architecture is proposed using MPC and maximum likelihood estimation method to deal with navigation in uncertainty environment. In contrast to our method, it is a kind of probabilistic framework based method. In [22], MPC is also developed to micro air vehicle system generating real-time optimal trajectory in unknown and low-sunlight environments. In [23], autopilot controlling the nonlinear yaw dynamics of an unmanned surface vehicle is designed using MPC combined with multilayer neural network.Inspired by methods mentioned above, a dual-layer control system is designed in this paper. Considering nonlinear nonconvex property of navigation in dynamic environment, RFNN based planner is used to program optimal trajectory of robot. Many methods can be used to train RFNN like backpropagation (BP), genetic algorithm, evolutionary strategy, and particle swarm optimization [24]. Extended Kalman filter (EKF) has attracted many researchers for its characteristics of fast convergence, little limitation in training data, and good accuracy in mathematics process [25]. So, in this paper EKF is used to train RFNN to achieve optimal navigation. A kind of nonlinear robust MPC is then designed to generate actual control command of robot to drive robot to track predictions. Considering that there exists intrinsic error between known model and actual model of navigation system of robot, online estimation method needs to be designed. Learning based controller is more adaptive in practical application [26]. By using online data, accurate approximation of the true system model can be constructed. These learning methods mainly consist of Gaussian process and iterative learning methods [27]. In this paper, a specific application based iterative learning method is designed to update system model.The rest of the paper is structured as follows. Section2 introduces description of problem settled in this paper. Section 3 describes the working of proposed navigation strategy. Section 4 proves stability and feasibility of proposed method. Section 5 illustrates experiments and results and finally Section 6 concludes the proposed scheme.


## 2. Problem Description

In this paper, navigation of robot is realized in unknown environment and the system is nonlinear with constraints. Robust and optimal navigation should be achieved considering dynamic environment and disturbances.Consider the following discrete nonlinear model of robot system:(1)xk+1=fxk,uk+wk1yk=hxk,uk+wk2,where uk∈Rn1 is control input and xk,xk+1∈Rn2 are system states at time k and k+1, respectively. yk∈Rn3 is measured output at time k and wk1∈Rn2,wk2∈Rn3 are process and measurement disturbances, respectively. f(∙) and h(∙) are nonlinear functions where f(∙) is model of system and h(∙) is model of observation. Realistic physical plants are strictly proper, so output function h(xk,uk) can be simplified as h(xk).In (1), w1 is model error caused disturbance while w2 mainly contains sensor noise and dynamic environment caused disturbance. Among these uncertainties, dynamic environment changes in random leading to unboundedness of disturbance. On the other hand, model error and sensor noise are bounded and normally are Gaussian that can be estimated. To design useful control method decomposition and combination are used for disturbances as(2)w2=w′2+w′′2.Feedback controller is usually designed using y to generate control input u, so (1) changes as(3)xk+1=fxk,uk+wk1+wk′′′2yk=hxk+wk′2,where w′′′=f(C(w′′)). C(∙) represents controller. Then, disturbances are separated into two parts, bounded disturbance represented as Δ1k=wk1+wk′′′2 and unbounded disturbance represented as Δ2k=wk′2.To achieve navigation, suitable controller should be designed. Considering disturbances divided into two items, we separate strategy into two phases. Real-time program of optimal trajectory is designed at first and robust control of robot is then designed to track the optimal trajectory. In program phase, recurrent fuzzy neural network with moving horizon is designed to plan optimal motion to get robust performance consideringΔ2 and can be illustrated as C1. After Δ2 is limited, robust model predictive control method is designed in tracking period to deal with Δ1 and can be illustrated as C2. Δ1 is decoupled from Δ2, so Δ2 is not considered during tracking. Then, a controller with dual-layer structure is formed to deal with two kinds of disturbances, respectively. Structure is shown in Figure 1. By separating controller into two parts, bounded and unbounded disturbances are suitably dealt with so that optimal robust performance is realized. Further introduction of control system is in next section.Figure 1
Control system considering disturbances.


## 3. Control System Design

To realize robust optimal navigation of robot, rational framework of control system is designed in this section. Robust nonlinear model predictive controller and recurrent fuzzy neural network based planner are combined to make up the whole control system. Flow diagram is shown in Figure3 which is extension of Figure 2. Figure 3 emphasizes each part’s function in navigation strategy and is consistent with Figure 2 which mainly explains relationship of uncertainty, controller, and nominal system.Figure 2
Block diagram of control system.Figure 3
Structure of RFNN.The system is made up of nonlinear planner, robust tracker, and local plant. As mentioned in Section2, there exist kinds of uncertainty bringing disturbances to robot. In order to improve the performance of robot avoiding obstacles and reaching the target, feedback loops that can be used to construct constraints of system are added. Extended Kalman filter is used to train RFNN online to program optimal motion of plant. Iterative online leaning method is used to make system adaptive to disturbance improving performance of MPC.


### 3.1. RFNN Based Nonlinear Program

Objective function and constraints should be set to realize motion program of navigation. Considering (3), nominal state-space function of robot is represented as(4)zk+1=fzk,vk+C′yk=hzk,where Δ1 discussed later keeps invariant in this department and is represented as C′. Objective function reflecting shortest distance can be used:(5)minJ=∑kdzk,where d(∙) is distance operator.RFNN structure used here contains five layers and is shown in Figure3. In order to achieve obstacle avoidance and reach target, output of system is chosen as input of RFNN, i.e., s′=y∈Rn3, while v=s′′∈Rn1 is generated control command to local plant. Subscript k is omitted here for simplification. Each node in membership layer performs a membership function. Membership functions can be defined with Gaussian MF as(6)si1j11=exp-si1′-ci1j12σi1j12,where exp[∙] is exponential function. c, σ are the mean and standard deviation of Gaussian function, respectively. The values of them are initially set via expert experience before the strategy begins.As shown in Figure3, each input parameter has two membership values, so there are 2n3 fuzzy rules in fuzzy rule layer. The firing strength of each rule at current step is determined by outputs of membership layer through an AND operator. The result of each rule is calculated as(7)si22=∏j1si1j11.Moreover, a local internal feedback with real-time delay is added to each node of this layer to improve the robustness of the control system. The mathematical form is described by(8)si23k=1-γsi22+γsi23k-1,where γ represents the weight of self-feedback loop and is a constant; s3(k) is current output of this layer while s3(k-1) represents the last step output.Then, according to the definition of Takagi-Sugeno-Kang (TSK) fuzzy rules, the functions are combined linearly in consequent layer. TSK weight is defined as(9)wi3i4=ai3i41z1+ai3i42z2+⋯+ai3i4n3zn3,where w∈R2n3×n1, a1,…,an3∈R2n3×n1. For the purpose of simplifying calculation, it is assumed that a=a1=⋯=an. The output of this layer can be computed as follows:(10)si44=∑i3wi3i4si23.Activation function is set at each node of output layer to limit output forming output constraint:(11)si4′′=si441+ηi4si44,where η is constant and its value depends on practical application.In order to achieve motion program, extended Kalman filter method proposed in [28] is modified to train the weights of RFNN structure online. At step k, the EKF function is expressed as follows:(12)ak+1=ak-KkEKFekEKF,(13)ekEKF=ok-odk,(14)KkEKF=PkOkRkEKF+OkTPkOk-1,(15)Pk+1=QkEKF+I-KkEKFOkTPk,where KEKF is Kalman gain matrix. eEKF is estimation error. od is desired value of o which is observation vector. REKF is covariance matrix of measurement error. QkEKF is constant matrix and is set according to [28], P is covariance matrix of estimation error, and I is identity matrix. O is orderly derivative matrix of observation vector with respect to RFNN weights.Constraints of obstacles and target during navigation process are achieved by establishing observation vector in (13); Jacobian matrix is then calculated to predict weights of RFNN:(16)O=∂oT∂a.Target constraint is always prior. Only when obstacles threaten safety of robot, is obstacle constraint considered in (13). As initial weights are generated in random, by repeating calculation suboptimal trajectory can be got satisfying (4). Calculation time is t depending on repetition. To deal with uncertainty caused by dynamic environment, moving horizon is used forming online dynamic program. Tra1,…,Tran are trajectories generated in different horizon. tΔ is time between contiguous horizons. Time constraint should be satisfied:(17)t<tΔ,where tΔ depends on uncertainty. A new variable is set to judge disturbance using quadratic form as(18)Δ=okp-okrTQΔokp-okr,where okp and okr are observation vectors at each step of program trajectory under system (4) and real trajectory under system (3), respectively. QΔ is matrix of weights. Upper bound is set to get(19)Δ<Δbound,According to (4)~(19), global motion program is obtained with bounded Δ2. However, during practical process there exists Δ1 affecting performance of navigation. So, suitable control and online learning method are needed in tracking procedure to guarantee robustness of system and ensure effective estimation of disturbances.


### 3.2. Learning Based Robust Tracking

#### 3.2.1. Robust Nonlinear Model Predictive Control

Tube-based robust nonlinear model predictive control makes closed-loop trajectory, in the presence of disturbances, lie in a tube whose center is nominal trajectory. In this paper, nominal trajectory is generated by RFNN motion planner and then a tube-based approach is adopted to tracking system considering uncertainty caused by model and location error.Considering (3) and (4), robot system to be controlled is the following nonlinear state-space form with bounded disturbances and Δ2k is not included:(20)xk+1=fxk,uk+Δ1k,where f(∙) is twice continuously differentiable.If cost function of MPC is Lipschitz continuous; there exist two invariant sublevel sets ofX such that the smaller set is robustly asymptotically stable for controlled system with a region for attracting the larger set [29]. Planned state and control input of navigation system in Section 3.1 are chosen as nominal state and control represented as Z, V respectively. The controller aims at steering robot with disturbances close to index prediction and keeping actual trajectory of robot in a tube with good robustness. However, nominal state and control provided above are finite and the whole trajectory keeps changing as introduced in (17) ~ (19). To obtain effective and tightened reference sets, the following procedure is used. An object is defined as(21)T=Tra1,tΔ1,Tra2,tΔ2,…,Tral,tΔl,…,representing trajectories and update interval in different horizon. Only the first tΔ step of Tra is valid motion. And interpolation is applied to get tightened Z and V.Considering deviation of state and control between actual system and nominal system, cost function to be minimized is defined as(22)Jx,u,k,l=x-zlTQx-zl+u-vlTRu-vl+Jfx;zf,l,where Q∈Rn2×n2 and R∈Rn1×n1 are positive definite. zl∈Z and vl∈V are reference trajectory and control action, respectively. Jf(∙) is terminal cost function and zf is terminal state that is changing due to (21).Optimal control problem can be illustrated as(23)minuJx,u,k,l.The solution of (23) is u∗={u0∗,u1∗,…,uN-1∗} and associated state trajectory is x∗=x0∗,x1∗,...,xN-1∗. κ=u0∗ is applied to uncertainty system, i.e., the first element in the sequence. If x=zl, then u=vl, so that J(x,u,k,l)=0. Minimum value of cost function at each step of trajectory is represented as Jmin(x,k,l). Level set of cost function defines a neighborhood of nominal trajectory. A tube can be defined [29]:(24)Sd=x∣Jminx,k,l≤d.According to [30], stable condition exists:(25)minuJ′ffx,u;zf,l+x-zfTQx-zf+u-vfTRu-vf≤J′ffx,u;zf,l,where J′f(f(x,u);zf,l)=(f(x,u)-zf)TQf(f(x,u)-zf) and x∈x∣J′f(x;zf,l)≤α. Qf is positive definite. Then Jf(∙) is determined as Jf(x;zf)=dJ′f(x;zf)/α. Method in [31] is used to solve the optimization problem.


#### 3.2.2. Iterative Online Learning

In actual situation there are disturbances caused by inaccurate model established a priori and invariant perception error. So, online learning method should be considered to estimate intrinsic part of disturbances online and make the control system effective. Asf(∙) is nonlinear in (20), disturbance Δ1 is no more Gaussian leading to the fact that GP based estimation method is not suitable here. So, a kind of PD iterative online learning method is designed in this section to estimate disturbances. The process is assumed to occur in an unknown environment and robot moves to goal avoiding obstacles in a predictive routine instead of a known iterative path. So, compared to traditional iterative method dynamic iterative route is used in our iterative learning algorithm. In order to learn compensation with dynamic route, suitable and effective iterative function should be designed as follows.Kinematic model of mobile robot is used here to introduce the procedure and detailed information is introduced in Section5.1. Then state-space system in (20) can be illustrated as(26)fx,u,w=x+Axu+Δ1,where x is state of system, u is control input, and Δ1 is disturbance need to be learned.Each iteration hask equations, but start and end points are different. The demonstration of choosing iteration route is illustrated intuitively in Figure 4. There are two trials end at points 1 and 2, respectively, during the process; start points can be got due to same steps k.Figure 4
Dynamic iterative route.In order to use this form of iterative route, a new variable is defined:σ=z-x. According to (26), k equations can be got:(27)σ2=z2-x2=z2-x1-A1u1-Δ1=z1-x1+A-1v1-A1u1-Δ1=σ1+A-1v1-A1u1-Δ1σ3=z3-x3=z2-x2+A-2v2-A2u2-Δ2=σ2+A-2v2-A2u2-Δ2=σ1+A-1v1-A1u1+A-2v2-A2u2-Δ1-Δ2⋮,where z, v, and A- are variables of references produced by RFNN in Section 3.1. In order to simplify representation, Δ1k is replaced by Δk above. Then the lifted form can be organized as(28)σK=σ0-BΔK,where K represents iteration and B is lower triangular matrix. Component of each item is shown as(29)σK=σ2σ3⋮σk+1,σ0=σ1+A-1v1-A1u1σ1+A-1v1-A1u1+A-2v2-A2u2⋮σ1+⋯+A-kvk-Akuk,ΔK=Δ1Δ2⋮Δk.Although the system illustrated by (26) is nonlinear, it is Lipschitz continuous. According to [32], linear method can deal with this kind of problem well. Compared to nominal discrete-time system introduced in [33], the initial conditions in (28) are different at each iteration. Many methods can be used to settle this problem such as initial state learning mechanism or smooth interpolation. As shown above, σ0 is made up of discrepancy at all steps in each trajectory. Suppose the first two iterations are convergent, then σ0 decreases in later iteration and is upper bounded. So, a simple forgetting factor λf is used to discount integration effect of iterative learning. Then a kind of PD-type iterative learner is used:(30)ΔK+1=λfΔK+λpRILeK+λdI-RILeK,where λp and λd are proportional and derivative learning gains, respectively. RIL is lower triangular Toeplitz matrix derived from column vector [0,I,0,…,0]T where 0 is matrix with 0 elements and I is n2×n2 unit matrix.For iterationK, error can be defined as(31)eK=σd-σK,where σd represents desired error between actual state and optimal predictive state and it is usually 0. Substituting (31) and (28) into (30) yields closed-loop iteration dynamics:(32)ΔK+1=I+λpRIL+λdI-RILBΔK+λpRIL+λdI-RILσd-σ0.According to [33], iterative learning process is asymptotically stable, if(33)ρI+λpRIL+λdI-RILB<1,where ρ(∙) is spectral radius operator.


## 3.1. RFNN Based Nonlinear Program

Objective function and constraints should be set to realize motion program of navigation. Considering (3), nominal state-space function of robot is represented as(4)zk+1=fzk,vk+C′yk=hzk,where Δ1 discussed later keeps invariant in this department and is represented as C′. Objective function reflecting shortest distance can be used:(5)minJ=∑kdzk,where d(∙) is distance operator.RFNN structure used here contains five layers and is shown in Figure3. In order to achieve obstacle avoidance and reach target, output of system is chosen as input of RFNN, i.e., s′=y∈Rn3, while v=s′′∈Rn1 is generated control command to local plant. Subscript k is omitted here for simplification. Each node in membership layer performs a membership function. Membership functions can be defined with Gaussian MF as(6)si1j11=exp-si1′-ci1j12σi1j12,where exp[∙] is exponential function. c, σ are the mean and standard deviation of Gaussian function, respectively. The values of them are initially set via expert experience before the strategy begins.As shown in Figure3, each input parameter has two membership values, so there are 2n3 fuzzy rules in fuzzy rule layer. The firing strength of each rule at current step is determined by outputs of membership layer through an AND operator. The result of each rule is calculated as(7)si22=∏j1si1j11.Moreover, a local internal feedback with real-time delay is added to each node of this layer to improve the robustness of the control system. The mathematical form is described by(8)si23k=1-γsi22+γsi23k-1,where γ represents the weight of self-feedback loop and is a constant; s3(k) is current output of this layer while s3(k-1) represents the last step output.Then, according to the definition of Takagi-Sugeno-Kang (TSK) fuzzy rules, the functions are combined linearly in consequent layer. TSK weight is defined as(9)wi3i4=ai3i41z1+ai3i42z2+⋯+ai3i4n3zn3,where w∈R2n3×n1, a1,…,an3∈R2n3×n1. For the purpose of simplifying calculation, it is assumed that a=a1=⋯=an. The output of this layer can be computed as follows:(10)si44=∑i3wi3i4si23.Activation function is set at each node of output layer to limit output forming output constraint:(11)si4′′=si441+ηi4si44,where η is constant and its value depends on practical application.In order to achieve motion program, extended Kalman filter method proposed in [28] is modified to train the weights of RFNN structure online. At step k, the EKF function is expressed as follows:(12)ak+1=ak-KkEKFekEKF,(13)ekEKF=ok-odk,(14)KkEKF=PkOkRkEKF+OkTPkOk-1,(15)Pk+1=QkEKF+I-KkEKFOkTPk,where KEKF is Kalman gain matrix. eEKF is estimation error. od is desired value of o which is observation vector. REKF is covariance matrix of measurement error. QkEKF is constant matrix and is set according to [28], P is covariance matrix of estimation error, and I is identity matrix. O is orderly derivative matrix of observation vector with respect to RFNN weights.Constraints of obstacles and target during navigation process are achieved by establishing observation vector in (13); Jacobian matrix is then calculated to predict weights of RFNN:(16)O=∂oT∂a.Target constraint is always prior. Only when obstacles threaten safety of robot, is obstacle constraint considered in (13). As initial weights are generated in random, by repeating calculation suboptimal trajectory can be got satisfying (4). Calculation time is t depending on repetition. To deal with uncertainty caused by dynamic environment, moving horizon is used forming online dynamic program. Tra1,…,Tran are trajectories generated in different horizon. tΔ is time between contiguous horizons. Time constraint should be satisfied:(17)t<tΔ,where tΔ depends on uncertainty. A new variable is set to judge disturbance using quadratic form as(18)Δ=okp-okrTQΔokp-okr,where okp and okr are observation vectors at each step of program trajectory under system (4) and real trajectory under system (3), respectively. QΔ is matrix of weights. Upper bound is set to get(19)Δ<Δbound,According to (4)~(19), global motion program is obtained with bounded Δ2. However, during practical process there exists Δ1 affecting performance of navigation. So, suitable control and online learning method are needed in tracking procedure to guarantee robustness of system and ensure effective estimation of disturbances.


## 3.2. Learning Based Robust Tracking

### 3.2.1. Robust Nonlinear Model Predictive Control

Tube-based robust nonlinear model predictive control makes closed-loop trajectory, in the presence of disturbances, lie in a tube whose center is nominal trajectory. In this paper, nominal trajectory is generated by RFNN motion planner and then a tube-based approach is adopted to tracking system considering uncertainty caused by model and location error.Considering (3) and (4), robot system to be controlled is the following nonlinear state-space form with bounded disturbances and Δ2k is not included:(20)xk+1=fxk,uk+Δ1k,where f(∙) is twice continuously differentiable.If cost function of MPC is Lipschitz continuous; there exist two invariant sublevel sets ofX such that the smaller set is robustly asymptotically stable for controlled system with a region for attracting the larger set [29]. Planned state and control input of navigation system in Section 3.1 are chosen as nominal state and control represented as Z, V respectively. The controller aims at steering robot with disturbances close to index prediction and keeping actual trajectory of robot in a tube with good robustness. However, nominal state and control provided above are finite and the whole trajectory keeps changing as introduced in (17) ~ (19). To obtain effective and tightened reference sets, the following procedure is used. An object is defined as(21)T=Tra1,tΔ1,Tra2,tΔ2,…,Tral,tΔl,…,representing trajectories and update interval in different horizon. Only the first tΔ step of Tra is valid motion. And interpolation is applied to get tightened Z and V.Considering deviation of state and control between actual system and nominal system, cost function to be minimized is defined as(22)Jx,u,k,l=x-zlTQx-zl+u-vlTRu-vl+Jfx;zf,l,where Q∈Rn2×n2 and R∈Rn1×n1 are positive definite. zl∈Z and vl∈V are reference trajectory and control action, respectively. Jf(∙) is terminal cost function and zf is terminal state that is changing due to (21).Optimal control problem can be illustrated as(23)minuJx,u,k,l.The solution of (23) is u∗={u0∗,u1∗,…,uN-1∗} and associated state trajectory is x∗=x0∗,x1∗,...,xN-1∗. κ=u0∗ is applied to uncertainty system, i.e., the first element in the sequence. If x=zl, then u=vl, so that J(x,u,k,l)=0. Minimum value of cost function at each step of trajectory is represented as Jmin(x,k,l). Level set of cost function defines a neighborhood of nominal trajectory. A tube can be defined [29]:(24)Sd=x∣Jminx,k,l≤d.According to [30], stable condition exists:(25)minuJ′ffx,u;zf,l+x-zfTQx-zf+u-vfTRu-vf≤J′ffx,u;zf,l,where J′f(f(x,u);zf,l)=(f(x,u)-zf)TQf(f(x,u)-zf) and x∈x∣J′f(x;zf,l)≤α. Qf is positive definite. Then Jf(∙) is determined as Jf(x;zf)=dJ′f(x;zf)/α. Method in [31] is used to solve the optimization problem.


### 3.2.2. Iterative Online Learning

In actual situation there are disturbances caused by inaccurate model established a priori and invariant perception error. So, online learning method should be considered to estimate intrinsic part of disturbances online and make the control system effective. Asf(∙) is nonlinear in (20), disturbance Δ1 is no more Gaussian leading to the fact that GP based estimation method is not suitable here. So, a kind of PD iterative online learning method is designed in this section to estimate disturbances. The process is assumed to occur in an unknown environment and robot moves to goal avoiding obstacles in a predictive routine instead of a known iterative path. So, compared to traditional iterative method dynamic iterative route is used in our iterative learning algorithm. In order to learn compensation with dynamic route, suitable and effective iterative function should be designed as follows.Kinematic model of mobile robot is used here to introduce the procedure and detailed information is introduced in Section5.1. Then state-space system in (20) can be illustrated as(26)fx,u,w=x+Axu+Δ1,where x is state of system, u is control input, and Δ1 is disturbance need to be learned.Each iteration hask equations, but start and end points are different. The demonstration of choosing iteration route is illustrated intuitively in Figure 4. There are two trials end at points 1 and 2, respectively, during the process; start points can be got due to same steps k.Figure 4
Dynamic iterative route.In order to use this form of iterative route, a new variable is defined:σ=z-x. According to (26), k equations can be got:(27)σ2=z2-x2=z2-x1-A1u1-Δ1=z1-x1+A-1v1-A1u1-Δ1=σ1+A-1v1-A1u1-Δ1σ3=z3-x3=z2-x2+A-2v2-A2u2-Δ2=σ2+A-2v2-A2u2-Δ2=σ1+A-1v1-A1u1+A-2v2-A2u2-Δ1-Δ2⋮,where z, v, and A- are variables of references produced by RFNN in Section 3.1. In order to simplify representation, Δ1k is replaced by Δk above. Then the lifted form can be organized as(28)σK=σ0-BΔK,where K represents iteration and B is lower triangular matrix. Component of each item is shown as(29)σK=σ2σ3⋮σk+1,σ0=σ1+A-1v1-A1u1σ1+A-1v1-A1u1+A-2v2-A2u2⋮σ1+⋯+A-kvk-Akuk,ΔK=Δ1Δ2⋮Δk.Although the system illustrated by (26) is nonlinear, it is Lipschitz continuous. According to [32], linear method can deal with this kind of problem well. Compared to nominal discrete-time system introduced in [33], the initial conditions in (28) are different at each iteration. Many methods can be used to settle this problem such as initial state learning mechanism or smooth interpolation. As shown above, σ0 is made up of discrepancy at all steps in each trajectory. Suppose the first two iterations are convergent, then σ0 decreases in later iteration and is upper bounded. So, a simple forgetting factor λf is used to discount integration effect of iterative learning. Then a kind of PD-type iterative learner is used:(30)ΔK+1=λfΔK+λpRILeK+λdI-RILeK,where λp and λd are proportional and derivative learning gains, respectively. RIL is lower triangular Toeplitz matrix derived from column vector [0,I,0,…,0]T where 0 is matrix with 0 elements and I is n2×n2 unit matrix.For iterationK, error can be defined as(31)eK=σd-σK,where σd represents desired error between actual state and optimal predictive state and it is usually 0. Substituting (31) and (28) into (30) yields closed-loop iteration dynamics:(32)ΔK+1=I+λpRIL+λdI-RILBΔK+λpRIL+λdI-RILσd-σ0.According to [33], iterative learning process is asymptotically stable, if(33)ρI+λpRIL+λdI-RILB<1,where ρ(∙) is spectral radius operator.


## 3.2.1. Robust Nonlinear Model Predictive Control

Tube-based robust nonlinear model predictive control makes closed-loop trajectory, in the presence of disturbances, lie in a tube whose center is nominal trajectory. In this paper, nominal trajectory is generated by RFNN motion planner and then a tube-based approach is adopted to tracking system considering uncertainty caused by model and location error.Considering (3) and (4), robot system to be controlled is the following nonlinear state-space form with bounded disturbances and Δ2k is not included:(20)xk+1=fxk,uk+Δ1k,where f(∙) is twice continuously differentiable.If cost function of MPC is Lipschitz continuous; there exist two invariant sublevel sets ofX such that the smaller set is robustly asymptotically stable for controlled system with a region for attracting the larger set [29]. Planned state and control input of navigation system in Section 3.1 are chosen as nominal state and control represented as Z, V respectively. The controller aims at steering robot with disturbances close to index prediction and keeping actual trajectory of robot in a tube with good robustness. However, nominal state and control provided above are finite and the whole trajectory keeps changing as introduced in (17) ~ (19). To obtain effective and tightened reference sets, the following procedure is used. An object is defined as(21)T=Tra1,tΔ1,Tra2,tΔ2,…,Tral,tΔl,…,representing trajectories and update interval in different horizon. Only the first tΔ step of Tra is valid motion. And interpolation is applied to get tightened Z and V.Considering deviation of state and control between actual system and nominal system, cost function to be minimized is defined as(22)Jx,u,k,l=x-zlTQx-zl+u-vlTRu-vl+Jfx;zf,l,where Q∈Rn2×n2 and R∈Rn1×n1 are positive definite. zl∈Z and vl∈V are reference trajectory and control action, respectively. Jf(∙) is terminal cost function and zf is terminal state that is changing due to (21).Optimal control problem can be illustrated as(23)minuJx,u,k,l.The solution of (23) is u∗={u0∗,u1∗,…,uN-1∗} and associated state trajectory is x∗=x0∗,x1∗,...,xN-1∗. κ=u0∗ is applied to uncertainty system, i.e., the first element in the sequence. If x=zl, then u=vl, so that J(x,u,k,l)=0. Minimum value of cost function at each step of trajectory is represented as Jmin(x,k,l). Level set of cost function defines a neighborhood of nominal trajectory. A tube can be defined [29]:(24)Sd=x∣Jminx,k,l≤d.According to [30], stable condition exists:(25)minuJ′ffx,u;zf,l+x-zfTQx-zf+u-vfTRu-vf≤J′ffx,u;zf,l,where J′f(f(x,u);zf,l)=(f(x,u)-zf)TQf(f(x,u)-zf) and x∈x∣J′f(x;zf,l)≤α. Qf is positive definite. Then Jf(∙) is determined as Jf(x;zf)=dJ′f(x;zf)/α. Method in [31] is used to solve the optimization problem.


## 3.2.2. Iterative Online Learning

In actual situation there are disturbances caused by inaccurate model established a priori and invariant perception error. So, online learning method should be considered to estimate intrinsic part of disturbances online and make the control system effective. Asf(∙) is nonlinear in (20), disturbance Δ1 is no more Gaussian leading to the fact that GP based estimation method is not suitable here. So, a kind of PD iterative online learning method is designed in this section to estimate disturbances. The process is assumed to occur in an unknown environment and robot moves to goal avoiding obstacles in a predictive routine instead of a known iterative path. So, compared to traditional iterative method dynamic iterative route is used in our iterative learning algorithm. In order to learn compensation with dynamic route, suitable and effective iterative function should be designed as follows.Kinematic model of mobile robot is used here to introduce the procedure and detailed information is introduced in Section5.1. Then state-space system in (20) can be illustrated as(26)fx,u,w=x+Axu+Δ1,where x is state of system, u is control input, and Δ1 is disturbance need to be learned.Each iteration hask equations, but start and end points are different. The demonstration of choosing iteration route is illustrated intuitively in Figure 4. There are two trials end at points 1 and 2, respectively, during the process; start points can be got due to same steps k.Figure 4
Dynamic iterative route.In order to use this form of iterative route, a new variable is defined:σ=z-x. According to (26), k equations can be got:(27)σ2=z2-x2=z2-x1-A1u1-Δ1=z1-x1+A-1v1-A1u1-Δ1=σ1+A-1v1-A1u1-Δ1σ3=z3-x3=z2-x2+A-2v2-A2u2-Δ2=σ2+A-2v2-A2u2-Δ2=σ1+A-1v1-A1u1+A-2v2-A2u2-Δ1-Δ2⋮,where z, v, and A- are variables of references produced by RFNN in Section 3.1. In order to simplify representation, Δ1k is replaced by Δk above. Then the lifted form can be organized as(28)σK=σ0-BΔK,where K represents iteration and B is lower triangular matrix. Component of each item is shown as(29)σK=σ2σ3⋮σk+1,σ0=σ1+A-1v1-A1u1σ1+A-1v1-A1u1+A-2v2-A2u2⋮σ1+⋯+A-kvk-Akuk,ΔK=Δ1Δ2⋮Δk.Although the system illustrated by (26) is nonlinear, it is Lipschitz continuous. According to [32], linear method can deal with this kind of problem well. Compared to nominal discrete-time system introduced in [33], the initial conditions in (28) are different at each iteration. Many methods can be used to settle this problem such as initial state learning mechanism or smooth interpolation. As shown above, σ0 is made up of discrepancy at all steps in each trajectory. Suppose the first two iterations are convergent, then σ0 decreases in later iteration and is upper bounded. So, a simple forgetting factor λf is used to discount integration effect of iterative learning. Then a kind of PD-type iterative learner is used:(30)ΔK+1=λfΔK+λpRILeK+λdI-RILeK,where λp and λd are proportional and derivative learning gains, respectively. RIL is lower triangular Toeplitz matrix derived from column vector [0,I,0,…,0]T where 0 is matrix with 0 elements and I is n2×n2 unit matrix.For iterationK, error can be defined as(31)eK=σd-σK,where σd represents desired error between actual state and optimal predictive state and it is usually 0. Substituting (31) and (28) into (30) yields closed-loop iteration dynamics:(32)ΔK+1=I+λpRIL+λdI-RILBΔK+λpRIL+λdI-RILσd-σ0.According to [33], iterative learning process is asymptotically stable, if(33)ρI+λpRIL+λdI-RILB<1,where ρ(∙) is spectral radius operator.


## 4. Stability Analysis

According to Figure2, the control system is made up of two parts. So, in order to analyze stability of the system, each part’s stability should be guaranteed.


### 4.1. Convergence of Recurrent Fuzzy Neural Network

In this paper, a training method based on EKF is used as shown in Section3.1. Appropriate parameters should be designed. Then, by iteratively calculating (12) ~ (15) trajectories are generated. By evaluating these trajectories, suboptimal trajectory is chosen.Observation vector,ok, is first expanded at final stable weights ad as(34)ok=Oad+ak-ad∂o∂a+ξk,where ξk is first-order approximation residue and O(∙) is mapping function from weights to observation. Error of weights can be defined: eak=ak-ad.Then, Lyapunov function can be chosen as(35)Ek=eakTPkTeak.The difference can be calculated as(36)ΔEk=Ek+1-Ek=eak+1TPk+1-1eak+1-eakTPk-1eak<eak+1-eakTPk-1eak-eak+1TPk+1-QkEKF-1PkOkEKFHk-1ξk,where Hk=RkEKF+OkTPkOk. Then following inequation can be got according to [28](37)ΔEk<3ξk2rk-ek2ok/m+rk,where ok is the trace of OkTPkOk, Rk=rkI. I is m×m identity matrix and rk is a positive real number. For details of inference formula refer to [28].As mentioned above,O and R determine convergence of EKF training process. O is derived from observation vector o. In order to guarantee effectiveness of recurrent fuzzy neural network structure, o should be constructed reasonable. According to (6) ~ (16),(38)O=∂o∂z∂z∂s′′∂s′′∂a,where ∂z/∂s′′ illustrates plant information, ∂s′′/∂a illustrates RFNN structure information, and ∂o/∂z illustrates relation between observation and plant’s state. Jacobian matrix, O, reflects performance of chosen weights to system. Then R is designed considering O to realize convergence of strategy.In (37), system is convergent if ΔEk<0 and then rk should be set as(39)rk>3okξk2mek2-3ξk2,where each element of ξk is normal distribution as ξik~N(0,rk); then(40)3okm<rk<ek264m,with 99.99%  ξk being bounded.Andr(k) adaption law is got:(41)rk=12ek264m+3okm,which can make control system’s error convergent, i.e., bounded ek.As mentioned in Section3.1, initial weights are generated in random leading to predictive trajectories being different with different initial weights. Initial weights are represented as(42)a01a02⋯a0l⋯,where each element corresponds to an independent trajectory produced by EKF training method. Some method can be chosen to learn initial weights, but a simple method of iteration is used to economize computation cost and guarantee real-time program ability. Considering practical condition, evaluation of optimal trajectory is different especially in dynamic navigation problem. With evaluation criteria, a0 is chosen to obtain suboptimal trajectory.


### 4.2. Stability and Robustness of Nonlinear Model Predictive Control

By using RFNN based planner, a set of trajectories are obtained in (21) to approximate optimal path in dynamic environment. MPC method is then used to limit states of robot in a tube around the optimal path under disturbance Δ1.For each trajectory inT, suppose there is d′>d and bounded area around trajectory is Sd′={x∣Jmin(x,k,l)≤d′}. Let x∗=z denotes generated state of nominal system; i.e., xk+1=f(xk,uk)+C, with input u∗=v. Since f(∙) is Lipschitz continuous, xk∗-zlk≤Lx-zl0 for all x∈Sd′+Δ1. According to (22) and (25), there exists d1 such that(43)Jminx,k,l≤Jx,u∗,k,l≤d1x-zl02.Stability condition is established in (25); then(44)Jminx,k+1,l≤Jminx,k,l-x-zl0TQx-zl0-κk-vl0TRκk-vl0.There is also lower bound due to weight matrix Q, so(45)Jminx,k,l≥d2x-zl02.Combine (43) ~ (45), and(46)Jminx,k+1,l≤1-d2d1Jminx,k,l.In practice, the surroundings where robot needs to achieve navigation can be entirely or partially detected by sensors. Suppose the change of surroundings is continuous and not too fast relative to robot’s motion. Then disturbance Δ1 is bounded. Let Δ-=maxΔ∣Δ∈Δ1. According to [29], for all x∈Sd′ and xk+1=f(xk,uk)+Δ1, there is a constant d3 such that(47)Jminx,k+1,l≤1-d2d1Jminx,k,l+d3Δ1≤1-d2d1Jminx,k,l+d3Δ-.If d2/d1Jmin(x,k,l)≥d3Δ-, (47) becomes(48)Jminx,k+1,l≤Jminx,k,l≤c1c3Δ-c2.So, there exists constant d=c1c3Δ-/c2 that limits actual path of robot in a tube of optimal trajectory generated by RFNN.After analysis and design of the overall system, whole strategy of navigation is illustrated as Algorithm1.Algorithm 1:Navigation strategy.
Initialization: x0=0, obtain initial position of target and obstacles, xtarget,xobstacle. setc,σ,QΔ,γ,η,Δbound,Q,R,d/αQf,λf,λp,λd while  x-xtarget>β  do Prediction: Predict robot motion according to (4) ~ (16) and (33) ~ (39). Generate trajectory inT as well as referential states and control commands, i.e. z,v. while  Δ<Δbound  do Tracking: Generate actual control command, u, according to (21) ~ (30). Sensor: Measure distance and angel information among target, obstacles and robot, i.e. y. Location: Estimate state of robot, obstacle and target, i.e. x,xtarget,xobstacle. Update Δ,x,xtarget,xobstacle.


## 4.1. Convergence of Recurrent Fuzzy Neural Network

In this paper, a training method based on EKF is used as shown in Section3.1. Appropriate parameters should be designed. Then, by iteratively calculating (12) ~ (15) trajectories are generated. By evaluating these trajectories, suboptimal trajectory is chosen.Observation vector,ok, is first expanded at final stable weights ad as(34)ok=Oad+ak-ad∂o∂a+ξk,where ξk is first-order approximation residue and O(∙) is mapping function from weights to observation. Error of weights can be defined: eak=ak-ad.Then, Lyapunov function can be chosen as(35)Ek=eakTPkTeak.The difference can be calculated as(36)ΔEk=Ek+1-Ek=eak+1TPk+1-1eak+1-eakTPk-1eak<eak+1-eakTPk-1eak-eak+1TPk+1-QkEKF-1PkOkEKFHk-1ξk,where Hk=RkEKF+OkTPkOk. Then following inequation can be got according to [28](37)ΔEk<3ξk2rk-ek2ok/m+rk,where ok is the trace of OkTPkOk, Rk=rkI. I is m×m identity matrix and rk is a positive real number. For details of inference formula refer to [28].As mentioned above,O and R determine convergence of EKF training process. O is derived from observation vector o. In order to guarantee effectiveness of recurrent fuzzy neural network structure, o should be constructed reasonable. According to (6) ~ (16),(38)O=∂o∂z∂z∂s′′∂s′′∂a,where ∂z/∂s′′ illustrates plant information, ∂s′′/∂a illustrates RFNN structure information, and ∂o/∂z illustrates relation between observation and plant’s state. Jacobian matrix, O, reflects performance of chosen weights to system. Then R is designed considering O to realize convergence of strategy.In (37), system is convergent if ΔEk<0 and then rk should be set as(39)rk>3okξk2mek2-3ξk2,where each element of ξk is normal distribution as ξik~N(0,rk); then(40)3okm<rk<ek264m,with 99.99%  ξk being bounded.Andr(k) adaption law is got:(41)rk=12ek264m+3okm,which can make control system’s error convergent, i.e., bounded ek.As mentioned in Section3.1, initial weights are generated in random leading to predictive trajectories being different with different initial weights. Initial weights are represented as(42)a01a02⋯a0l⋯,where each element corresponds to an independent trajectory produced by EKF training method. Some method can be chosen to learn initial weights, but a simple method of iteration is used to economize computation cost and guarantee real-time program ability. Considering practical condition, evaluation of optimal trajectory is different especially in dynamic navigation problem. With evaluation criteria, a0 is chosen to obtain suboptimal trajectory.


## 4.2. Stability and Robustness of Nonlinear Model Predictive Control

By using RFNN based planner, a set of trajectories are obtained in (21) to approximate optimal path in dynamic environment. MPC method is then used to limit states of robot in a tube around the optimal path under disturbance Δ1.For each trajectory inT, suppose there is d′>d and bounded area around trajectory is Sd′={x∣Jmin(x,k,l)≤d′}. Let x∗=z denotes generated state of nominal system; i.e., xk+1=f(xk,uk)+C, with input u∗=v. Since f(∙) is Lipschitz continuous, xk∗-zlk≤Lx-zl0 for all x∈Sd′+Δ1. According to (22) and (25), there exists d1 such that(43)Jminx,k,l≤Jx,u∗,k,l≤d1x-zl02.Stability condition is established in (25); then(44)Jminx,k+1,l≤Jminx,k,l-x-zl0TQx-zl0-κk-vl0TRκk-vl0.There is also lower bound due to weight matrix Q, so(45)Jminx,k,l≥d2x-zl02.Combine (43) ~ (45), and(46)Jminx,k+1,l≤1-d2d1Jminx,k,l.In practice, the surroundings where robot needs to achieve navigation can be entirely or partially detected by sensors. Suppose the change of surroundings is continuous and not too fast relative to robot’s motion. Then disturbance Δ1 is bounded. Let Δ-=maxΔ∣Δ∈Δ1. According to [29], for all x∈Sd′ and xk+1=f(xk,uk)+Δ1, there is a constant d3 such that(47)Jminx,k+1,l≤1-d2d1Jminx,k,l+d3Δ1≤1-d2d1Jminx,k,l+d3Δ-.If d2/d1Jmin(x,k,l)≥d3Δ-, (47) becomes(48)Jminx,k+1,l≤Jminx,k,l≤c1c3Δ-c2.So, there exists constant d=c1c3Δ-/c2 that limits actual path of robot in a tube of optimal trajectory generated by RFNN.After analysis and design of the overall system, whole strategy of navigation is illustrated as Algorithm1.Algorithm 1:Navigation strategy.
Initialization: x0=0, obtain initial position of target and obstacles, xtarget,xobstacle. setc,σ,QΔ,γ,η,Δbound,Q,R,d/αQf,λf,λp,λd while  x-xtarget>β  do Prediction: Predict robot motion according to (4) ~ (16) and (33) ~ (39). Generate trajectory inT as well as referential states and control commands, i.e. z,v. while  Δ<Δbound  do Tracking: Generate actual control command, u, according to (21) ~ (30). Sensor: Measure distance and angel information among target, obstacles and robot, i.e. y. Location: Estimate state of robot, obstacle and target, i.e. x,xtarget,xobstacle. Update Δ,x,xtarget,xobstacle.


## 5. Experiments

### 5.1. Model of Mobile Robot

In order to approve effectiveness of our control system, a kind of differentially driven mobile robot system model in [9] is used here. The robot model in unknown environment with obstacles and target is illustrated as in Figure 5.Figure 5
Model of mobile robot in unknown environment.Robot is motivated by right and left wheels,[vr,vl]T. Linear and angular velocities are(49)v=vr+vl2,(50)ω=vr-vl2b,(51)R=bvr+vlvr-vl,Difference equation is then illustrated as(52)xk+1=xk+Δtcos⁡θk0sin⁡θk001uk,where u=(v,ω)T. xk,xk+1 are robot’s postures at k and k+1 steps, respectively, and are represented as x=[x,y,θ]T.Distance and angle of target and nearest obstacle corresponding to robot are chosen as measurement, so output is represented as(53)yk=dgk,θgk,dok,θokT.As shown in Figure 5, a sparse representation of obstacles is used to train RFNN. Circles with center on obstacle represent dangerous area. Radius of circle depends on EKF learning method and estimation error. Location of obstacle is usually not accurate leading to error of centers’ position shown in figure. Distance between O1 and O2 depends on b. Although RFNN structure is convergent, robot will vibrate at terminal position due to feedback control principle. When robot reaches area determined by β, a brake control is used.


### 5.2. Performance in Static Condition with Bounded Disturbance

Simulation experiment is carried out using Matlab. Activity space is limited in10m×10m. There exist obstacles and target. Mobile robot needs to reach target and avoid obstacles. Constant parameters to be initialized are listed in Table 1. The weight matrices are set with 25 : 15 : 1 ratio weighting position-tracking error and linear and angular velocity control input in (22). The weight QΔ is switched between two sets due to position corresponding to target and obstacles.Table 1
Initialization of parameters.
b 0.2m Δ b o u n d 0.5 c 0 0 - π 2 - π 2 10 5 π 2 π 2 λ p 0.1 σ 2.5 1 1 1 λ d 3 η 0.3 0.3 ⋯ λ f 0.9 γ 0.3 β 0.5Proposed method is first examined in static environment. As environment is static, only one prediction of optimal motion of robot is generated and the weights of RFNN are shown in Figure6. There is no drastic variety underlying smoothness of process. Gaussian noise with mean zero and variance of 0.03 is injected to system named as disturbance 1. And 20 trajectories are generated under random disturbance. Performance is shown as in Figure 7. All trajectories are bounded in a band. Each trajectory of robot can realize reaching adjacent area of target and avoiding obstacles. Optimal and robust navigation is achieved. Position error curves of robot are shown in Figure 8(a), which are bounded in a narrow band. Figures 8(b) and 8(c) represent linear and angular velocities of robot, respectively. They are not narrow contrasted to position error for the reason that input commands should control robot against random disturbance and limit robot’s position in a narrow tube. Computation cost of generating optimal control command at each step is shown in Figure 9. Lines in red are average values.Figure 6
Weights of RFNN.Figure 7
Performance in static environment: 20 trajectories with disturbance 1.Figure 8
Position error and control inputs of robot with 20 trajectories.
(a)
Distance error of robot (b)
Linear velocity of robot (c)
Angular velocity of robotFigure 9
Computation cost at each step of 20 trajectories.


### 5.3. Performance of Online Iterative Learning Method

In order to examine online iterative learning method proposed, Gaussian noise with mean 0.1 and variance of 0.03 is injected to system named as disturbance 2. 20 trajectories are also generated under this random disturbance. Performance is shown as Figure10. For neat expression only one trajectory without iterative learning in magenta is shown in figure. This trajectory is dangerous and does not lie in stable tube. Trajectories with iterative leaning are illustrated in cyan and are same as trajectories under disturbance 1 underlying estimation method which is effect. Estimation of disturbance 2 is shown in Figure 11. Detailed information is shown in Table 2. There is not much difference of values in each item between two conditions. The largest bias at each point of 20 trajectories under two conditions is shown in Figure 12. The largest bias of trajectories under disturbance 2 is a little higher.Table 2
Evaluation of trajectories in static environment.
Prediction cost (s) Step Average distance (m) Average terminal error (m) Average bias (m) Trajectories under disturbance 1 4.13 63 9.82 0.36 0.061 Trajectories under disturbance 2 4.13 63 9.86 0.35 0.067Figure 10
Performance in static environment: 20 trajectories with disturbance 1.Figure 11
Estimation of disturbance 2.Figure 12
Largest bias at each step under two conditions.
(a)
Maximal bias under disturbance 1 (b)
Maximal bias under disturbance 2


### 5.4. Comparison with Other Methods

The main novelty of proposed method in this paper is that a motion planning and control method based navigation strategy is proposed to deal with uncertainty and guarantee robust optimization of the process. The dual-layer structure is designed to be feasible and effective by combining RFNN and MPC method. Many motion planning techniques are used to achieve navigation, like potential field method, sampling based planner, interpolation curve method, A∗, neural network, fuzzy logic, genetic algorithm, particle swarm optimization, etc. A∗ method is widely used in autonomous vehicles for its efficiency [34]. Furthermore, OPTI toolbox of Matlab includes many optimal solvers that can be used to design nonlinear planner. In order to introduce our method’s effectiveness, A∗ method together with OPTI based numerical optimization method is compared with ours. As control policy is considered in our method, PID method in [35] is used for two planners above forming complete navigation system just like ours. Performance is shown in Figure 13. A∗ method just plans positions of robot, leading to bad tracking performance under disturbance which is reflected by tracking error in Table 3 and the path in red with rectangle marks. The trajectory is not smooth contrasted to our method and OPTI and PID method. PID control method that lacks online learning mechanism fails to drive robot to target when disturbance in Figure 10 acted on system, which is reflected by red path with triangle marks. Compared to A∗  and PID based strategy, OPTI and PID method that plans motion performs better. But OPTI based planner takes up too much computation cost reflected in Table 3 leading to bad real-time planning ability. Detailed information of these methods’ performance is illustrated in Table 3. It is proved that our method can achieve good online navigation.Table 3
Comparison between our method and others.
Distance (m) Step Computation cost (s) Terminal error (m) Tracking error (m) OPTI & PID 10.132 60 37.891 0.289 0.048 A∗  & PID 1 10.389 62 0.512 0.402 0.059 A∗  & PID 2 6.209 37 0.512 3.889 0.231 OURS 10.090 62 3.142 0.251 0.040Figure 13
Comparison between our method and others.


### 5.5. Performance in Dynamic Condition

Proposed method is then examined in dynamic environment. Performance is shown in Figure14. Dynamic obstacles referred to in Figure 7 keep moving during process. Trajectories of obstacles are shown in Figures 14(a)–14(d). Target also keeps moving during the whole process. RFNN needs to update predictions depending on changes of surroundings limited by threshold settled above. And measured Δ varying with time is shown in Figure 15. Each state in Figures 14(a)–14(f) is corresponding to zero point of Δ in Figure 15. Six predictions are generated during the whole process. As seen in Table 4, the first four predictions are generated due to (17) ~ (19) and last two are generated due to runout of steps. Changes of RFNN weights are shown in Figure 16. Detailed information of different phases is shown in Table 4. Position error between robot and target is shown in Figure 17(a). Linear and angular velocity of robot is shown in Figures 17(b) and 17(c). As shown above, optimal and robust navigation is achieved in dynamic environment.Table 4
Evaluation of trajectories in dynamic environment.
Trajectory 1 2 3 4 5 6 Cost of prediction (s) 4.13 1.47 1.25 0.38 0.02 0.003 Predicted step 63 49 42 51 29 27 Actual step 9 8 37 42 29 27 Predicted distance (m) 9.72 5.11 4.93 2.08 2.06 1.54 Actual distance (m) 2.16 1.87 5.27 2.37 2.30 1.59 Predicted terminal error (m) 0.36 0.41 0.38 0.41 0.42 0.37 Actual terminal error (m) 7.45 5.40 1.87 2.55 1.92 0.37Figure 14
Trajectory of robot in dynamic environment.
(a)
k = 1 (b)
k = 10 (c)
k = 18 (d)
k = 55 (e)
k = 97 (f)
k = 126  (g)
k = 152Figure 15
Changes ofΔ.Figure 16
Changes of RFNN weights.Figure 17
Position error and control inputs of robot in dynamic environment.
(a)
Distance error of robot (b)
Linear velocity of robot (c)
Angular velocity of robot


### 5.6. Hardware Experiments on a Vehicle

In this section, we will validate navigation strategy presented in this paper on a differential driven vehicle. The onboard processor of vehicle is STM322F407 that is responsible for real-time control. All the online computation, including localization, motion planning, and optimal control command generation, is performed on a computer with Intel i5 2.3 GHz processor and 8 GB RAM. Commands are sent through a 2.4 GHz Bluetooth transmitter at an update rate of 50 Hz.The experiment is carried out in indoor environment as shown in Figure18. There are two moving tracked mobile robots between our robot and target. The robot needs to reach target and avoid collision with them. The software running on the computer is developed based on the ROS middleware. 2D map of surroundings is established using LIDAR carried on the front of robot. Adaptive Monte Carlo localization method is used to localize robot’s position. RFNN based motion planner is used for global optimization programming. NMPC based controller is used for robust optimal control. The performance is shown in Figure 19. The trajectories in red are optimal predictions of robot’s motion. Optimal prediction is updated due to moving obstacle. Figure 20 illustrates changes of Δ which is tanglesome contrasted to Figure 15. Trajectory in blue is actual motion of robot. Motion state is reflected in Figure 21. Computation cost and changes of weights are, respectively, shown in Figures 22 and 23. Comparison between hardware and simulation experiment in Section 5.5 is shown in Table 5. As prediction cost depends on distance between vehicle and target, simulation experiment takes up more computation than hardware experiment and prediction time is more. Average computation cost of both experiments at each step is about half a second. Average tracking error of hardware experiment is higher than simulation for the reasons of effective location method in actual situation. According to hardware experiment, our method’s effectiveness is proved.Table 5
Comparison between simulation and hardware experiment.
Predicted distance (m) Actual distance (m) Predicted step Total prediction cost (times) (s) Average computation cost (s) Total time taken (s) Average tracking error (m) Simulation 15.186 15.606 153 7.253(6) 0.551 102 0.230 Hardware 7.361 7.577 47 0.727(2) 0.539 65 0.156Figure 18
Experimental surroundings.Figure 19
Performance of vehicle. Trajectories in red are predictions, and trajectory in blue reflects actual positions of vehicle.
(a) (b) (c)Figure 20
Changes ofΔ.Figure 21
Motion condition of vehicle.
(a) (b)Figure 22
Computation cost.Figure 23
Changes of RFNN weights.


## 5.1. Model of Mobile Robot

In order to approve effectiveness of our control system, a kind of differentially driven mobile robot system model in [9] is used here. The robot model in unknown environment with obstacles and target is illustrated as in Figure 5.Figure 5
Model of mobile robot in unknown environment.Robot is motivated by right and left wheels,[vr,vl]T. Linear and angular velocities are(49)v=vr+vl2,(50)ω=vr-vl2b,(51)R=bvr+vlvr-vl,Difference equation is then illustrated as(52)xk+1=xk+Δtcos⁡θk0sin⁡θk001uk,where u=(v,ω)T. xk,xk+1 are robot’s postures at k and k+1 steps, respectively, and are represented as x=[x,y,θ]T.Distance and angle of target and nearest obstacle corresponding to robot are chosen as measurement, so output is represented as(53)yk=dgk,θgk,dok,θokT.As shown in Figure 5, a sparse representation of obstacles is used to train RFNN. Circles with center on obstacle represent dangerous area. Radius of circle depends on EKF learning method and estimation error. Location of obstacle is usually not accurate leading to error of centers’ position shown in figure. Distance between O1 and O2 depends on b. Although RFNN structure is convergent, robot will vibrate at terminal position due to feedback control principle. When robot reaches area determined by β, a brake control is used.


## 5.2. Performance in Static Condition with Bounded Disturbance

Simulation experiment is carried out using Matlab. Activity space is limited in10m×10m. There exist obstacles and target. Mobile robot needs to reach target and avoid obstacles. Constant parameters to be initialized are listed in Table 1. The weight matrices are set with 25 : 15 : 1 ratio weighting position-tracking error and linear and angular velocity control input in (22). The weight QΔ is switched between two sets due to position corresponding to target and obstacles.Table 1
Initialization of parameters.
b 0.2m Δ b o u n d 0.5 c 0 0 - π 2 - π 2 10 5 π 2 π 2 λ p 0.1 σ 2.5 1 1 1 λ d 3 η 0.3 0.3 ⋯ λ f 0.9 γ 0.3 β 0.5Proposed method is first examined in static environment. As environment is static, only one prediction of optimal motion of robot is generated and the weights of RFNN are shown in Figure6. There is no drastic variety underlying smoothness of process. Gaussian noise with mean zero and variance of 0.03 is injected to system named as disturbance 1. And 20 trajectories are generated under random disturbance. Performance is shown as in Figure 7. All trajectories are bounded in a band. Each trajectory of robot can realize reaching adjacent area of target and avoiding obstacles. Optimal and robust navigation is achieved. Position error curves of robot are shown in Figure 8(a), which are bounded in a narrow band. Figures 8(b) and 8(c) represent linear and angular velocities of robot, respectively. They are not narrow contrasted to position error for the reason that input commands should control robot against random disturbance and limit robot’s position in a narrow tube. Computation cost of generating optimal control command at each step is shown in Figure 9. Lines in red are average values.Figure 6
Weights of RFNN.Figure 7
Performance in static environment: 20 trajectories with disturbance 1.Figure 8
Position error and control inputs of robot with 20 trajectories.
(a)
Distance error of robot (b)
Linear velocity of robot (c)
Angular velocity of robotFigure 9
Computation cost at each step of 20 trajectories.


## 5.3. Performance of Online Iterative Learning Method

In order to examine online iterative learning method proposed, Gaussian noise with mean 0.1 and variance of 0.03 is injected to system named as disturbance 2. 20 trajectories are also generated under this random disturbance. Performance is shown as Figure10. For neat expression only one trajectory without iterative learning in magenta is shown in figure. This trajectory is dangerous and does not lie in stable tube. Trajectories with iterative leaning are illustrated in cyan and are same as trajectories under disturbance 1 underlying estimation method which is effect. Estimation of disturbance 2 is shown in Figure 11. Detailed information is shown in Table 2. There is not much difference of values in each item between two conditions. The largest bias at each point of 20 trajectories under two conditions is shown in Figure 12. The largest bias of trajectories under disturbance 2 is a little higher.Table 2
Evaluation of trajectories in static environment.
Prediction cost (s) Step Average distance (m) Average terminal error (m) Average bias (m) Trajectories under disturbance 1 4.13 63 9.82 0.36 0.061 Trajectories under disturbance 2 4.13 63 9.86 0.35 0.067Figure 10
Performance in static environment: 20 trajectories with disturbance 1.Figure 11
Estimation of disturbance 2.Figure 12
Largest bias at each step under two conditions.
(a)
Maximal bias under disturbance 1 (b)
Maximal bias under disturbance 2


## 5.4. Comparison with Other Methods

The main novelty of proposed method in this paper is that a motion planning and control method based navigation strategy is proposed to deal with uncertainty and guarantee robust optimization of the process. The dual-layer structure is designed to be feasible and effective by combining RFNN and MPC method. Many motion planning techniques are used to achieve navigation, like potential field method, sampling based planner, interpolation curve method, A∗, neural network, fuzzy logic, genetic algorithm, particle swarm optimization, etc. A∗ method is widely used in autonomous vehicles for its efficiency [34]. Furthermore, OPTI toolbox of Matlab includes many optimal solvers that can be used to design nonlinear planner. In order to introduce our method’s effectiveness, A∗ method together with OPTI based numerical optimization method is compared with ours. As control policy is considered in our method, PID method in [35] is used for two planners above forming complete navigation system just like ours. Performance is shown in Figure 13. A∗ method just plans positions of robot, leading to bad tracking performance under disturbance which is reflected by tracking error in Table 3 and the path in red with rectangle marks. The trajectory is not smooth contrasted to our method and OPTI and PID method. PID control method that lacks online learning mechanism fails to drive robot to target when disturbance in Figure 10 acted on system, which is reflected by red path with triangle marks. Compared to A∗  and PID based strategy, OPTI and PID method that plans motion performs better. But OPTI based planner takes up too much computation cost reflected in Table 3 leading to bad real-time planning ability. Detailed information of these methods’ performance is illustrated in Table 3. It is proved that our method can achieve good online navigation.Table 3
Comparison between our method and others.
Distance (m) Step Computation cost (s) Terminal error (m) Tracking error (m) OPTI & PID 10.132 60 37.891 0.289 0.048 A∗  & PID 1 10.389 62 0.512 0.402 0.059 A∗  & PID 2 6.209 37 0.512 3.889 0.231 OURS 10.090 62 3.142 0.251 0.040Figure 13
Comparison between our method and others.


## 5.5. Performance in Dynamic Condition

Proposed method is then examined in dynamic environment. Performance is shown in Figure14. Dynamic obstacles referred to in Figure 7 keep moving during process. Trajectories of obstacles are shown in Figures 14(a)–14(d). Target also keeps moving during the whole process. RFNN needs to update predictions depending on changes of surroundings limited by threshold settled above. And measured Δ varying with time is shown in Figure 15. Each state in Figures 14(a)–14(f) is corresponding to zero point of Δ in Figure 15. Six predictions are generated during the whole process. As seen in Table 4, the first four predictions are generated due to (17) ~ (19) and last two are generated due to runout of steps. Changes of RFNN weights are shown in Figure 16. Detailed information of different phases is shown in Table 4. Position error between robot and target is shown in Figure 17(a). Linear and angular velocity of robot is shown in Figures 17(b) and 17(c). As shown above, optimal and robust navigation is achieved in dynamic environment.Table 4
Evaluation of trajectories in dynamic environment.
Trajectory 1 2 3 4 5 6 Cost of prediction (s) 4.13 1.47 1.25 0.38 0.02 0.003 Predicted step 63 49 42 51 29 27 Actual step 9 8 37 42 29 27 Predicted distance (m) 9.72 5.11 4.93 2.08 2.06 1.54 Actual distance (m) 2.16 1.87 5.27 2.37 2.30 1.59 Predicted terminal error (m) 0.36 0.41 0.38 0.41 0.42 0.37 Actual terminal error (m) 7.45 5.40 1.87 2.55 1.92 0.37Figure 14
Trajectory of robot in dynamic environment.
(a)
k = 1 (b)
k = 10 (c)
k = 18 (d)
k = 55 (e)
k = 97 (f)
k = 126  (g)
k = 152Figure 15
Changes ofΔ.Figure 16
Changes of RFNN weights.Figure 17
Position error and control inputs of robot in dynamic environment.
(a)
Distance error of robot (b)
Linear velocity of robot (c)
Angular velocity of robot


## 5.6. Hardware Experiments on a Vehicle

In this section, we will validate navigation strategy presented in this paper on a differential driven vehicle. The onboard processor of vehicle is STM322F407 that is responsible for real-time control. All the online computation, including localization, motion planning, and optimal control command generation, is performed on a computer with Intel i5 2.3 GHz processor and 8 GB RAM. Commands are sent through a 2.4 GHz Bluetooth transmitter at an update rate of 50 Hz.The experiment is carried out in indoor environment as shown in Figure18. There are two moving tracked mobile robots between our robot and target. The robot needs to reach target and avoid collision with them. The software running on the computer is developed based on the ROS middleware. 2D map of surroundings is established using LIDAR carried on the front of robot. Adaptive Monte Carlo localization method is used to localize robot’s position. RFNN based motion planner is used for global optimization programming. NMPC based controller is used for robust optimal control. The performance is shown in Figure 19. The trajectories in red are optimal predictions of robot’s motion. Optimal prediction is updated due to moving obstacle. Figure 20 illustrates changes of Δ which is tanglesome contrasted to Figure 15. Trajectory in blue is actual motion of robot. Motion state is reflected in Figure 21. Computation cost and changes of weights are, respectively, shown in Figures 22 and 23. Comparison between hardware and simulation experiment in Section 5.5 is shown in Table 5. As prediction cost depends on distance between vehicle and target, simulation experiment takes up more computation than hardware experiment and prediction time is more. Average computation cost of both experiments at each step is about half a second. Average tracking error of hardware experiment is higher than simulation for the reasons of effective location method in actual situation. According to hardware experiment, our method’s effectiveness is proved.Table 5
Comparison between simulation and hardware experiment.
Predicted distance (m) Actual distance (m) Predicted step Total prediction cost (times) (s) Average computation cost (s) Total time taken (s) Average tracking error (m) Simulation 15.186 15.606 153 7.253(6) 0.551 102 0.230 Hardware 7.361 7.577 47 0.727(2) 0.539 65 0.156Figure 18
Experimental surroundings.Figure 19
Performance of vehicle. Trajectories in red are predictions, and trajectory in blue reflects actual positions of vehicle.
(a) (b) (c)Figure 20
Changes ofΔ.Figure 21
Motion condition of vehicle.
(a) (b)Figure 22
Computation cost.Figure 23
Changes of RFNN weights.


## 6. Conclusion

In this paper, a combined nonlinear model predictive control and recurrent fuzzy neural network approach is designed to achieve robust and optimal navigation in unknown and dynamic environment. Controller is separated into two parts depending on disturbances of system forming dual-layer structure. RFNN based dynamic program is used to predict optimal trajectory in unknown environment. NMPC is used to track optimal trajectories against location and model error. By applying our method to wheeled mobile robot, simulation experiment is carried out. According to analysis of experiment results, robustness and optimization of designed strategy are proved. Although proposed method is used in ground robot system, it is also applicable to UUV, UAV, space aircraft, etc. Because problem described in Section2 and working in Section 3 are not specific to a certain model, in our future work, perception ability mentioned in Section 1 will be considered in practical navigation system.



---

*Source: 8014019-2018-08-13.xml*