MPC for Path Following Problems of Wheeled Mobile Robots⁎

MPC for Path Following Problems of Wheeled Mobile Robots⁎

6th IFAC Conference on Nonlinear Model Predictive Control 6th IFAC Conference on Nonlinear Model Predictive Control Madison, WI, USA, August 19-22, 20...

611KB Sizes 8 Downloads 99 Views

6th IFAC Conference on Nonlinear Model Predictive Control 6th IFAC Conference on Nonlinear Model Predictive Control Madison, WI, USA, August 19-22, 2018 6th IFAC Conference on Nonlinear Model Predictive Control Available online at www.sciencedirect.com Madison, WI, USA, August 19-22, 2018 6th IFAC Conference on Nonlinear Model Predictive Control Madison, WI, USA, August 19-22, 2018 Madison, WI, USA, August 19-22, 2018

ScienceDirect

IFAC PapersOnLine 51-20 (2018) 247–252

MPC for Path Following Problems of MPC of MPC for for Path Path Following Following Problems Problems of ⋆ ⋆⋆ Robots MPC Wheeled for Path Mobile Following Problems of Wheeled Mobile Robots Wheeled Mobile Robots ⋆ Wheeled Mobile Robots∗∗ ∗∗ ∗ Shuyou Yu ∗,∗∗ ∗,∗∗ Yang Guo ∗∗ Lingyu Meng ∗∗ Ting Qu ∗

∗∗ Ting Qu ∗ Shuyou Yu ∗,∗∗ Yang Guo ∗∗ Lingyu ∗∗ ∗ ∗,∗∗ Meng Shuyou Yu ∗,∗∗ YangHong Guo ∗∗ Lingyu Chen ∗,∗∗ ∗,∗∗ Meng ∗∗ Ting Qu ∗ Hong Chen ∗,∗∗ ∗∗ ∗,∗∗ Shuyou Yu YangHong Guo Chen Lingyu Meng Ting Qu Hong Chen ∗,∗∗ ∗ ∗ State Key Laboratory of Automotive Simulation and Control, Jilin ∗ ∗ State Key Laboratory of Automotive Simulation and Control, Jilin State Key Laboratory of Automotive Simulation and Control, Jilin University, Changchun China University, Changchun Simulation China 130012 130012 ∗ State Key (e-mail: Laboratory of Automotive and Control, Jilin University, Changchun China 130012 {shuyou,quting,chenh}@jlu.edu.cn) (e-mail: {shuyou,quting,chenh}@jlu.edu.cn) ∗∗ University, Changchun China 130012 (e-mail: {shuyou,quting,chenh}@jlu.edu.cn) ∗∗ ∗∗ Department of Control Science and Engineering, Jilin University, of Science and Engineering, ∗∗ Department (e-mail: {shuyou,quting,chenh}@jlu.edu.cn) Department of Control Control Science and Engineering, Jilin Jilin University, University, Changchun China 130012 (e-mail: {861460929,609006272}@qq.com) Changchun China 130012 Science (e-mail: and {861460929,609006272}@qq.com) ∗∗ Department of Control Engineering, Jilin University, Changchun China 130012 (e-mail: {861460929,609006272}@qq.com) Changchun China 130012 (e-mail: {861460929,609006272}@qq.com) Abstract: Abstract: In In this this paper paper disturbance disturbance observer observer based based model model predictive predictive control control for for path path following following Abstract: In this paper disturbance observer based model ispredictive control for path following problems of wheeled mobile robots with input disturbances proposed. A nonlinear disturbance problems of wheeled mobile robots with input disturbances is proposed. A nonlinear disturbance Abstract: this paper disturbance observer basedand model predictive for path following problems wheeled mobile robotsthe with input disturbances proposed.control Athe nonlinear disturbance observer isof In designed to estimate disturbances, to is compensate influence of disturobserver is designed to estimate the disturbances, and to compensate the influence of disturproblems of wheeled mobile robots with input disturbances is proposed. A nonlinear disturbance observer is designed to estimate the disturbances, and to compensate the influence of disturbances. Nominal model predictive control scheme with guaranteed asymptotic convergence is bances. Nominal model predictive control scheme with guaranteed asymptotic convergence is observer is designed toinput estimate the disturbances, and to compensate the influence of disturbances. Nominal model predictive control scheme with guaranteed asymptotic convergence is adopted to deal with constraints of wheeled mobile robots. Simulation example shows adopted to deal with input constraints of wheeled mobile robots. Simulation example shows bances. Nominal model predictive control scheme with guaranteed asymptotic convergence is adopted to deal with input constraints of wheeled mobile robots. Simulation example shows that proposed scheme can drive wheeled mobile robots to aa given path that the the to proposed scheme can drive the the of wheeled mobile robots to follow follow given path despite despite adopted deal with input constraints wheeled mobile robots. Simulation example shows that the proposed scheme can input drive disturbances. the wheeled mobile robots to follow a given path despite the presence of slowly varying the of varying that the proposed scheme can input drive disturbances. the wheeled mobile robots to follow a given path despite the presence presence of slowly slowly varying input disturbances. © 2018, IFAC of (International Federation Automatic Control) Hosting by Elsevier Ltd. All rights reserved. the presence slowly varying input ofdisturbances. Keywords: Wheeled mobile robot, path following, disturbance observer, model predictive Keywords: Wheeled mobile robot, path Keywords: Wheeled mobile robot, path following, following, disturbance disturbance observer, observer, model model predictive predictive control control Keywords: Wheeled mobile robot, path following, disturbance observer, model predictive control control1. INTRODUCTION schemes 1. INTRODUCTION schemes are are proposed proposed in in (Sun (Sun et et al., al., 2017a) 2017a) for for tracking tracking 1. INTRODUCTION schemes are proposed in (Sun et al., 2017a) for tracking unicycle robots with input constraints and bounded disunicycle are robots with input constraints and bounded dis1. INTRODUCTION schemes proposed in (Sun et al., 2017a) for tracking unicycle robots with input constraints andtobounded disturbances, where the bound is supposed be small. A Path following problems of a wheeled mobile robot is wherewith the input boundconstraints is supposed tobounded be small.disA Path following problems of a wheeled mobile robot is turbances, unicycle robots and wherecontrol the bound is supposed to betracking small. of A Path following problems of aofwheeled mobile robot is turbances, model predictive scheme for trajectory a control task that consists following a given path model predictive control scheme for trajectory tracking of a control task that consists of following a given path where the robots bound is proposed supposed to (Nascimento betracking small. of A Path following problems of aof wheeled mobile robot is turbances, model predictive control scheme for trajectory aparameterized control taskbythat consists following a given path nonholonomic mobiel is in its arc length or curvature (Jarzebowska, parameterized bythat its arc length of or curvature (Jarzebowska, nonholonomic mobiel robots is proposed in (Nascimento model predictive control scheme for trajectory tracking of a control task consists following a given path nonholonomic mobiel robots is proposed in (Nascimento parameterized by its arc length or curvature (Jarzebowska, et al., 2018) where a modified cost function is adopted 2012). Compared Compared to to the the tracking tracking control control problem, problem, usually usually et al., 2018) where a modified cost function is adopted 2012). mobiel is proposed in (Nascimento parameterized by its arcwheeled length ormobile curvature (Jarzebowska, et al., minimizes 2018) where a robots modified cost function is adopted 2012). Compared tothe the tracking control problem, which the between the and it is is assumed assumed that robot that usually follows nonholonomic which minimizes the adistance distance between the robot robotis pose pose and it that the wheeled mobile robot that follows et al., 2018) where modified cost function adopted 2012). Compared to the tracking control problem, usually which minimizes the distance between the robot to pose and it is path assumed that the wheeled mobile robot that follows the given global trajectory coordinate. In order reduce the moves forward, and the time information is not the given global trajectory coordinate. In order to reduce the path moves forward, and the time information is not which minimizes theburden, distance between the robotpredictive pose and it is path assumed that the wheeled mobile robotthere that follows the given global trajectory coordinate. Inmodel order to reduce the moves forward, and is, the time information is not computational event-based a control demand yet. That in general, are no computational burden, coordinate. event-basedInmodel predictive a control demand yet. That is, intime general, there are no the the given global trajectory order to reduce the path moves forward, and the information is not computational burden, event-based model predictive atemporal control demand yet. That is, in general, there are no tracking control of nonholonomic systems with coupled specifications. control of burden, nonholonomic systems withpredictive coupled temporal specifications. the computational event-based model a control specifications. demand yet. That is, in general, there are no tracking tracking control ofand nonholonomic systems with coupled temporal input constraints bounded disturbances is proposed input constraints and bounded disturbances is proposed In the the early early days of of path path following following control control of of wheeled wheeled tracking control ofand nonholonomic systems with coupled temporal specifications. input constraints bounded disturbances is proposed (Eqtami et al., 2013; Sun et al., 2018). In days (Eqtami et al., 2013; Sun et al., 2018). In the robots early days of path following control of wheeled mobile (WMR), scheme based on Lyapunov theory input constraints and bounded disturbances is proposed (Eqtami et al., 2013; Sun et al., 2018). mobile robots (WMR), scheme based on Lyapunov theory In the robots early days oflocal path following control of wheeled mobile (WMR), scheme based Lyapunov theory Uncertainty (disturbance or perturbation) exists is used used to design design control lawon (Kanayama et al., et al., 2013; Sun et 2018). Uncertainty (disturbance oral., perturbation) exists widely widely is to aa local control law (Kanayama et al., (Eqtami mobile robots (WMR), scheme based on Lyapunov theory Uncertainty (disturbance or might perturbation) exists perforwidely is usedand to design a such local as control law linearization (Kanayama et al., in industrial systems, which cause system 1990), schemes feedback (Luca in industrial systems, which might cause system perfor1990), and schemes such as feedback linearization (Luca Uncertainty (disturbance orinstability. perturbation) exists the widely is used to design a such local control law linearization (Kanayama et al., in industrial systems, which might cause system perfor1990), and schemes as feedback (Luca mance degradation or even To reduce inand Benedetto, 1993; d’Andrea Novel et al., 1995), backdegradation or which even instability. To system reduce the inand Benedetto, 1993;such d’Andrea Novel linearization et al., 1995), (Luca back- mance in industrial systems, might cause perfor1990), and schemes as feedback mance degradation or even instability. To reduce the inand Benedetto, 1993; d’Andrea Novel et al., 1995), backfluence of uncertainty is a key point of controller design. stepping (Jiang and Nijmeijer, 1997) are used to design fluence of uncertainty is a key point of controller design. stepping (Jiang and Nijmeijer, 1997) are used to design mance degradation or even instability. To reduce the inand Benedetto, d’Andrea Novel et al., 1995), backfluence of control uncertainty is a(Dixon key point of controller stepping (Jiang 1993; and Nijmeijer, 1997) arethe used to design Adaptive method et al., 2001; Xindesign. et al., global (regional) control laws. Although, existing conglobal (regional) control laws. Although, the existing con- Adaptive control method (Dixon et al., 2001; Xin et al., fluence of uncertainty is a key point of controller design. stepping (Jiang and Nijmeijer, 1997) are used to design control method (Dixon et and al., Drakunov, 2001; Xin et al., global Although, the existing con- Adaptive 2016), sliding mode control (Bloch 1995; straints(regional) make it it control difficultlaws. to achieve achieve better performance sliding mode control (Bloch Drakunov, 1995; straints make difficult to better performance Adaptive control method (Dixon et and al., 2001; Xincontrol et al., global (regional) laws. Although, the existing con- 2016), 2016), sliding mode control (Bloch and Drakunov, 1995; straints make mobile it control difficult to follow achieve better performance Yu et al., 2014a; Xu and Chen, 2016) and robust when wheeled robots the desired (reference) et al., 2014a; Xu control and Chen, 2016) robust control when wheeled mobile robots follow thebetter desiredperformance (reference) Yu 2016), sliding mode (Bloch andand Drakunov, 1995; straints make input it difficult to follow achieve Yu et al., 2014a; Xu and Chen, 2016) and robustare control when wheeled mobile robots the desired (reference) (Koubaa et al., 2013; Fateh and Arab, 2015) used path, neither constraints nor state constraints are et al., 2013; Fateh and Arab, are used path, wheeled neither input constraints nor state constraints are (Koubaa et al., 2014a; Xu and Chen, 2016) and2015) robust control when mobileconstraints robots follow desired (reference) (Koubaa et al., 2013; Fateh and Arab, 2015) are used path, input northe state constraints are Yu to deal with uncertainties and to improve the following taken neither into account. account. to deal with uncertainties and to improve the following taken into et 2013; Fateh and 2015) areuncerused path, neither input constraints nor state constraints are (Koubaa to deal with uncertainties and to Arab, improve thethe following taken into account. accuracy of al., path following problems. While accuracy of path following problems. While the uncerModel predictive control (MPC), also known as recedto deal with uncertainties and to improve the following taken into account. accuracy of path following problems. While the uncertainty is measurable, or the uncertainty is unmeasurable Model predictive control (MPC), also known as reced- tainty is measurable, or the uncertainty is unmeasurable Model predictive control (MPC), known control as receding horizon horizon control (RHC), is an analso advanced s- accuracy ofestimated path following problems. While the uncertainty is measurable, or the uncertainty is unmeasurable but can the other variables, ing control (RHC), is advanced control sModel predictive control (MPC), also known as recedcanisbe be estimated from from theuncertainty other measurable measurable variables, ing horizon control (RHC), is anprocess advanced control s- but trategy widely used in industrial control. Modtainty measurable, or the is unmeasurable but can be estimated from the other measurable variables, the influence of the uncertainty can be compensated or trategy widely used in industrial process control. Moding horizon control iscananprocess advanced control s- the influence of the uncertainty canmeasurable be compensated or trategy widely used (RHC), in industrial control. Model predictive predictive control schemes deal with constrained but can be (Chen estimated from the Disturbance other variables, the influence of the uncertainty can be compensated or eliminated et al., 2016). observer based el control schemes can deal with constrained trategy widelycontrol used in industrial process control. Mod- eliminated (Chen et al., 2016). Disturbance observer based el predictive schemes canboth dealrecursive with constrained control problems, and guarantee feasibilithe influence of the uncertainty can be compensated or eliminated (Chen et al., 2016). Disturbance observer based explicit nonlinear model predictive control is designed for control problems, and guarantee both recursive feasibiliel control schemes can dealrecursive with nonlinear model predictive control observer is designed for control and guarantee both feasibility predictive of the theproblems, involved optimization problem and constrained stability of explicit eliminated (Chen et al., 2016). Disturbance based explicit nonlinear model predictive control is designed for the drone’s flight characteristics (Liu, 2011), where neity of involved optimization problem and stability of control and guarantee both recursive feasibilidrone’s flight model characteristics (Liu, 2011), where neity theproblems, involved optimization and stability of the theofclosed-loop closed-loop system. Recedingproblem horizon control for path path explicit nonlinear predictive control is of designed for the drone’s flight characteristics (Liu, 2011), where neither state constraints nor input constraints the the system. Receding horizon control for ty of the involved optimization problem and stability of ther state constraints nor input constraints of the drone drone the closed-loop system. Receding horizon control for path following problems of wheeled mobile robot is proposed the drone’s flight characteristics (Liu, 2011), where neither state constraints nor input constraints of the drone considered. By combing the disturbance observer and following problems of wheeled mobile robot is proposed the closed-loop system. Receding horizon control for2017) path are are considered. By the disturbance observer and following problems of Yu wheeled proposed in (Faulwasser, (Faulwasser, 2012; et al., al.,mobile 2015; robot Liu etis al., al., state constraints nor input constraints of thecontrol drone are considered. By combing combing the model disturbance observer and MPC, aa disturbance rejection predictive in 2012; Yu et 2015; Liu et 2017) ther following problems of wheeled mobile robot is proposed MPC, disturbance rejection model predictive control in (Faulwasser, 2012; Yu is etconsidered. al., 2015; Two Liu et al., 2017) where only nominal model robust MPC are considered. By combing the disturbance observer and MPC, afor disturbance rejection model predictive control scheme tracking nonholonomic vehicles with coupled where only nominal model is considered. Two robust MPC in (Faulwasser, 2012; Yu is etconsidered. al., 2015; Two Liu et al., 2017) for tracking nonholonomic vehicles with coupled where only nominal model robust MPC scheme MPC, a disturbance rejection model predictive control schemeconstraints for tracking nonholonomic vehicles with coupled and disturbances proposed (Sun et ⋆ where onlyisnominal considered. robust MPC input input constraints andnonholonomic disturbances is isvehicles proposed (Sun et al., al., The work supportedmodel by the is National NaturalTwo Science Foundation ⋆ scheme for tracking with coupled work is supported by the National Natural Science Foundation input constraints and disturbances ison proposed (Sun et al., 2017b), where only the disturbance the linear velocity ⋆ The of China for isfinancial support within the Natural projectsScience No. 61573165, No. The work supported by the National Foundation 2017b), where only the disturbance on the linear velocity of China for financial support within the projects No. 61573165, No. input constraints and disturbances proposed (Sun et al., ⋆ The 2017b), where only the disturbanceison the linear velocity is considered. 6171101085 and No. 61520106008. of China for is financial support the Natural projectsScience No. 61573165, No. work supported by thewithin National Foundation is considered. 6171101085 and No. 61520106008. 2017b), where only the disturbance on the linear velocity is considered. 6171101085 No. 61520106008. of China for and financial support within the projects No. 61573165, No. considered. 6171101085 No. 61520106008. 2405-8963 ©and 2018, IFAC (International Federation of Automatic Control) is Hosting by Elsevier Ltd. All rights reserved.

Copyright © 2018 IFAC 281 Copyright © under 2018 IFAC 281 Control. Peer review responsibility of International Federation of Automatic Copyright © 2018 IFAC 281 10.1016/j.ifacol.2018.11.021 Copyright © 2018 IFAC 281

2018 IFAC NMPC 248 Madison, WI, USA, August 19-22, 2018

Shuyou Yu et al. / IFAC PapersOnLine 51-20 (2018) 247–252

i.e., β = 0. Then, the kinematics equation of wheeled mobile robot is simplified as   � � � � z˙ v cos θ cos θ 0 y˙  = v sin θ = sin θ 0 u (2) ω 0 1 θ˙ T

where u = [v w] is the control input, v = 2r (ωr +ωl ) cos δ, r ω is the angle velocity of the yaw angle and ω = 2b (ωr −ωl ).

Fig. 1. Schematic diagram of the wheeled mobile robot In this paper, disturbance observer based model predictive control scheme for path following problems of wheeled mobile robots is studied, where input constraints of the system are taken into account. Nonlinear disturbance observer is used to estimate and to compensate the influence of the input disturbances. Thus, the following accuracy of the wheeled mobile robot can be improved. 2. PATH FOLLOWING PROBLEM OF WHEELED MOBILE ROBOTS A unicycle robot has a front castor and two rear driving wheels. The schematic diagram of the wheeled mobile robot is shown in Fig.1, and the symbols are described in Table 1. Table 1. Description of symbols Meaning Track between front wheels Vertical distance between centroid and front wheel Radius of the wheel Instantaneous center of robot Distance between instantaneous center and the front wheel Distance between instantaneous center and the centroid Resultant velocity of the centroid Side slip angle Yaw angle Steering angle Velocity of the left wheel Velocity of the right wheel

Singularities play a significant role in the design and control of the robot manipulators (Donelan, 2007). A singularity ia a point within the robot’s workspace where the robot’s Jacobin matrix loses rank. The drawbacks of singularities are (1) Loss of freedom. A drop in rank reduces the dimension of the image which represents a loss of instantaneous motion of the wheeled mobile robot. (2) Loss of control. Near a singularity, the Jacobin matrix is ill-conditioned and the control algorithm fails. Eq.(2) implies that the side slip angle β ≡ 0 which will avoid the singularities and benefit the stability of the wheeled mobile robot. Suppose that there is a robot moving along the reference path, then the position and orientation of the “virtual” mobile robot represent the ideal state of the wheeled mobile robot. Denote (zR , yR , θR )T as the reference state, and describe the kinematic model of the “virtual” wheeled mobile robot as   � � z˙R vR cos θR y˙ R  = vR sin θR (3) wR θ˙R where zR and yR denote the position of the robot, vR is the magnitude of the robot translational velocity, θR denotes the robot moving direction and wR is the angular velocity of θR .

Symbol 2b d r O

Denote (xe , ye , θe )T as the error state which represents the deviation of the current position to the reference. To control the robot (2) to track the “virtual” robot (3), an error state can be defined as � � � �� � ze cos θ sin θ 0 zR − z ye = − sin θ cos θ 0 yR − y (4) 0 0 1 θR − θ θe � �� �

ρf ρ v β θ δ wl wf

:G

in which the linear operator G represents a coordinate transformation matrix.

The motion state of the wheeled mobile robot is described by its position (z, y) and its orientation θ, where (z, y) is the midpoint of the rear axis of the wheeled mobile robot. The kinematics equation of wheeled mobile robot is (Gu and Hu, 2006)   � � z˙ v cos(β + θ) y˙  = v sin(β + θ) (1) r(ωr − ωl )/2b θ˙ � � ρ sin δ−d with β = arctan fρf cos δ , where v is the magnitude of the robot translational velocity and w is the angular velocity of θ.

Then, the error dynamic is   � � z˙e wye − v + vR cos θe y˙ e  = −wze + vR sin θe wR − w θ˙e

(5)

Denote

u1e =vR cos θe − v u2e =wR − w, then the dynamic model of the error system is z˙e = wR ye − u2e ye + u1e y˙ e = −wR ze + u2e ze + vR sin θe θ˙e = u2e .

(6)

The error state (ze , ye , θe )T is close to the equilibrium while v and w approach vR and wR . Thus, the path

Choose steering angle δ such that ρf sin δ − d = 0, 282

2018 IFAC NMPC Madison, WI, USA, August 19-22, 2018

Shuyou Yu et al. / IFAC PapersOnLine 51-20 (2018) 247–252

249

3.1 Disturbance observer Considering the input disturbances, system (7) can be rewritten as x˙ p = f (xp ) + g(xp )(up + d) (9) T

where d = [dv dw ] and 

Fig. 2. Composite controller structure following problems of the wheeled mobile robot to a given path can be reduced to the regulation problem of the error systems (5). Denote xp = [ze ye θe ]

 c(s)vR ye f (xp ) = vR sin θe − c(s)vR ze , 0   1 −ye g(xp ) = 0 ze . 0 1

The adopted nonlinear disturbance observer is dˆ =ξ + h(xp )

T

T

up = [u1e u2e ] , the system (6) can be rewritten as x˙ p = β(xp , up ) (7) where β(·, ·) is twice-continuous differentiable on xp and up , and β(0, 0) = 0. An autonomous unicycle robot has some forward speed but zero instantaneous lateral motion, i.e., a unicycle robot is a non-holonomic system. Furthermore, the velocity v and the angular velocity w are restricted such that   v ∈U (8) w where U is a compact set.

Denote c(s) as the path curvature at the given point in the reference path. For simplicity, suppose that the reference speed vR is given, then wR = c(s)vR . The control objective of the wheeled mobile robot is: Given a reference path, T find control laws [v w] ∈ U such that the wheeled mobile robot follows the reference path, i.e., find suitable control T laws [v w] to drive the errors ze , ye and θe to zero. 3. DISTURBANCE OBSERVER BASED ROBUST MODEL PREDICTIVE CONTROL The wheeled mobile robot will generate a side slip phenomenon considering the influence of the ground environment when it performs a path following task. The side slip phenomenon can be treated as an input disturbance which poses a challenge to the control of the wheeled mobile robot: it may significantly degrade the following performance and may even cause instability if the influence is not taken into account in the system design. The composite controller structure is illustrated in Fig.2. The disturbance observer provides an estimate of disturbance. An optimization problem with nominal model and tightened constraints is used in MPC to predict the system behavior and to drive the wheeled mobile robot to track the reference trajectory. Note that disturbance observer based control is in principle a robust control scheme. Disturbance observer can estimate and compensate the system disturbance without extra sensors (extra complexities). 283

ξ˙ = − l(xp )g(xp )ξ − l(xp )(f (xp ) + g(xp )up + g(xp )h(xp ))

(10)

T

where dˆ = [dv dw ] is the estimate of disturbances, ξ is the state of the nonlinear disturbance observer, h(·) is a nonlinear function to be determined. The gain of the nonlinear disturbance observer satisfies that ∂h(xp ) l(xp ) = (11) ∂xp Assumption 1. The derivative of the disturbance is bound˙ ≤ ε, where ε > 0 is a ed. That is, for any t ≥ 0, �d� constant scalar. Theorem 1. Suppose that the considered disturbances are input disturbances and satisfy Assumption 1. Choose l(xp ) such that −l(xp )g(xp ) is asymptotically stable, then the estimate dˆ of the disturbance observer (10) can asymptotically track the input disturbances d. Furthermore, �d − ˆ ∞ is proportional to ε. d� Proof. Denote ed := d − dˆ as the estimate error. The dynamics of the estimate error is ˙ e˙ d =d˙ − dˆ ∂h(xp ) (12) + d˙ = − ξ˙ − ∂xp ˙ = − l(xp )h(xp )ed + d. Thus, the estimate error is input-to-state stable and �d − ˆ ∞ is proportional to ε, since −l(xp )g(xp ) is asymptotid� ˙ ≤ ε (Khalil, 2002). cally stable and �d� Corollary 2. Considering the system (9), and choosing   10 0 l(xe ) = , 00 1

the output of the disturbance observer (10) can asymptotically track the disturbance if the input disturbance satisfies Assumption 1.    1 0 0 Proof. Since l(xp ) = and h(xp ) = l(xp )dxp + C 0 0 1 with matrix, the eigenvalues of −l(ze )g(xp ) =   C a constant 1 −ye are all located on the left half of complex plane. − 0 1 Due to Theorem 1, the conclusion is come to.

2018 IFAC NMPC 250 Madison, WI, USA, August 19-22, 2018

Shuyou Yu et al. / IFAC PapersOnLine 51-20 (2018) 247–252



T 0 0 0 Note that, for simplicity, C = is chosen in the 0 0 0 simulation example.

1.5 1

Suppose that �ed � ≤ ξ, and choose U0 = U ⊖ ξ, where the operator ⊖ denotes the Pontryagin difference of two sets A ⊆ Rn and B ⊆ Rn .

y[m]

0.5

3.2 Constrained model predictive control

0 −0.5 −1

The optimization problem is formulated as follows: Problem 3. minimize J(up (·))

−1.5 −2

−1.5

−1

−0.5

−0

0.5

1

1.5

2

x[m]

up (·)

Fig. 3. Desired eight-shaped curve (solid line) and the actual trajectory (dashed line) of the wheeled mobile robot: with MPC

subject to x˙ p (τ, xp (t)) = f (xp (τ, xp (t))) + g(xp (τ, xp (t)))u(τ ), xp (t, xp (t)) = xp (t), u(τ ) ∈ U0 , τ ∈ [t, t + Tp ], xp (t + Tp , x(t)) ∈ Ω,

4. SIMULATIONS In order to verify the effectiveness of the proposed scheme, a simulation experiment is carried out in Matlab. The physical parameters of the robot is as follows: r = 8cm, b = 23cm and d = 75cm.

where J(u(·)) := �xp (t + Tp , xp (t))�2P  t+Tp   �xp (τ, xp (t))�2Q + �up (τ )�2R dτ + t

is the cost functional, Tp is the prediction horizon, Q ∈ Rn3 ×n3 and R ∈ Rn2 ×n2 are positive definite state  and v T Qv input weighting matrices. Note that �v�Q = with Q∈ Rn×n and Q > 0 for a vector v ∈ Rn , and v cos θe (τ ) − u1e (τ ) . u(τ ) = R c(s)vR − u2e (τ ) The positive definite matrix P ∈ Rn3 ×n3 is the terminal penalty matrix, and E (x) := �x�2P is the terminal penalty function. The terminal set Ω := {x ∈ Rn3 | xT P x ≤ α} is a level set of the terminal penalty function. The term xp (·, xp (t)) represents the predicted state trajectory starting from the initial state xp (t) under the control u(·). In order to guarantee feasibility and convergence, P and Ω have to satisfy terminal conditions, see (Chen and Allg¨ower, 1998) and (Mayne et al., 2000). Remark 4. Only nominal model is adopted to predict the system dynamics in Problem 3. Remark 5. Similar to (Yu et al., 2015), the initial state of the reference path can be chosen as a new determined variable in Problem 3. Since ed keeps small and only input constraints are considered here, similar to (Yu et al., 2014b), both recursive feasibility of the optimization problem and asymptotic convergence of the system (9) to the origin can be guaranteed. Thus, the wheeled mobile robot can follow asymptotically the reference path. 3.3 Composite control law The overall control input is ˆ u(t) = u(t)∗ − d(t) (13) where u(t) is the first segment of control input obtained by the online optimization of model predictive control. ∗

284

The speed constraint is −1 ≤ v ≤ 3m/s, the angular speed constraint is −3.5 ≤ ω ≤ 3.5rad/s, the magnitude of the translational velocity of the “virtual” wheeled mobile robot is vR = 0.7. The cost function is F (xp , up ) = xTp Qxp + uTp Rup (14) where the weighting matrices are Q = 0.4I3 and R = 0.5I2 with Ij ∈ Rj×j identity matrix. Consider the input disturbances  0.5(1 − e−0.05(t−12) ), dv = 0, and dw = 0.

t ∈ [12, ∞) t ∈ [0, 12)

(15)

The prediction horizon is 10 and the sampling time is 0.02s. The terminal penalty matrix is   4.3483 0 0 0 4.7593 4.3374 P = (16) 0 4.3374 23.3629   and the terminal set is Ω = xp ∈ R3 | xTp P xp ≤ 10 . 4.1 Eight-shaped curve tracking

The reference trajectory is an eight-shaped trajectory zR = 1.8 sin(θR ) (17) yR = 1.2 sin(2θR ). The initial position of the mobile robot is (z0 , y0 , θ0 )T = (−0.4, −0.4, π/2)T . The actual trajectory of the wheeled mobile robot is shown in Fig. 3 as dashed line, and the reference trajectory is shown as solid line, where model predictive control for path following problems (Yu et al., 2015; Liu et al., 2017) is adopted. It shows that the actual trajectory of the wheeled mobile robot deviates from the reference trajectory and the deviation is increasing while the input disturbances (15) acts on. In order to reduce the influence of input disturbances and improve

2018 IFAC NMPC Madison, WI, USA, August 19-22, 2018

Shuyou Yu et al. / IFAC PapersOnLine 51-20 (2018) 247–252

251

1.5

v[m/s]

1.5 1

y[m]

0.5

0.5 0

w[rad/s]

0 −0.5 −1 −1.5 −2

−1.5

−1

−0.5

0

0.5

1

1.5

2

x[m]

0

500

0

500

1000

1500

1000

1500

4 2 0 −2 −4

t[s]

Fig. 4. Desired eight-shaped curve (solid line) and the actual trajectory (dashed line) of the wheeled mobile robot: with disturbance observer based MPC

Fig. 6. Eight-shaped curve: Control curve with disturbance observer based MPC 0.6

1

xe[m]

1

0.5

0.5

0 −0.5

0

5

10

15

20

25

0.4

30

0.3

d

ye[m]

0.5

0.2

0 −0.5

0.1 0

5

10

15

20

25

30

0 −0.1

θe[rad]

5 0

−0.2

−5 −10

0

5

10

15

20

25

0

500

1000

1500

t[s]

30

t[s]

Fig. 5. Eight-shaped curve: error curve with disturbance observer based MPC the following accuracy, disturbance observer based MPC is adopted. Fig.4 shows the comparison between the actual trajectory and the desired trajectory of the wheeled mobile robot. Although there exist slowly varying and bounded disturbances, the wheeled mobile robot with disturbance observer based MPC can follow the desired path. Fig.5 shows the error curve of the wheeled mobile robot. Since the disturbance observer can estimate and compensate the input disturbances, the error will asymptotically converge to zero. The evolution of the control input of the wheeled mobile robot is shown in Fig.6, which satisfies the input constraints. Fig. 7 shows the output of the disturbance observer. On one hand, the disturbance observer can estimate the real disturbances; on the other hand, the estimate error does not asymptotically converge to zero since the error system (7) is state-dependent. 5. CONCLUSION Disturbance observer based model predictive control for path following problems of wheeled mobile robots was proposed in this paper. While there is no disturbance at all, model predictive control can guarantee the satisfaction of input constraints, and drive the wheeled mobile robot to the desired path. While there are input disturbances, in particular, slowly varying disturbances, nonlinear disturbance observer can estimate the disturbances and compensate the influence of it through a “feedback”. Simulation 285

Fig. 7. Eight-shaped curve: the estimated disturbances result showed that the proposed control strategy can guarantee the convergence of the wheeled mobile robot to the reference path with respect to input disturbances. REFERENCES Bloch, A.M. and Drakunov, S. (1995). Tracking in nonholonomic dynamic system via sliding modes. In Proc. 34th IEEE Conf. Decision Contr., 2103–2106. New Orleans, LA. Chen, H. and Allg¨ower, F. (1998). A quasi-infinite horizon nonlinear model predictive control scheme with guaranteed stability. Automatica, 34(10), 1205–1217. Chen, W.H., Yang, J., Guo, L., and Li, S. (2016). Disturbance-observer-based control and related methods - An overview. IEEE Transactions on Industrial Electronics, 63(2), 1083–1095. d’Andrea Novel, B., Campion, G., and Bastin, G. (1995). Control of nonholonomic wheeled mobile robots by state feedback linearization. The International Journal of Robotics Research, 14(6), 543–559. Dixon, W.E., Dawson, D.M., and Zergeroglu, E. (2001). Adaptive tracking control of a wheeled mobile robot via an uncalibrated camera system. IEEE Transactions on Systems, Man, and Cybernetics - Part B: Cybernetics, 31(3), 341–352. Donelan, P.S. (2007). Singularities of robot manipulators. In Singularity Theory, 189–217. Eqtami, A., Heshmati-alamdari, S., Dimarogonas, D.V., and Kyriakopolulos, K.J. (2013). Self-triggered mod-

2018 IFAC NMPC 252 Madison, WI, USA, August 19-22, 2018

Shuyou Yu et al. / IFAC PapersOnLine 51-20 (2018) 247–252

el predictive control for nonholonomic systems. In 2013 European Control of Conference, 638–643. Zurich, Switchland. Fateh, M.M. and Arab, A. (2015). Robust control of a wheeled mobile robot by voltage control strategy. Nonlinear Dynamics, 79(1), 335–348. Faulwasser, T. (2012). Optimization-based solutions to constrained trajectory-tracking and path-following problems. Ph.D. Thesis, Otto-von-Guericke University Magdeburg. Gu, D. and Hu, H.S. (2006). Receding horizon tracking control of wheel mobile robots. IEEE Trans. Contr. Syst. Technology, 14(4), 743–749. Jarzebowska, E. (2012). Model-based tracking contnrol of nonlinear systems. CRC Press, Taylor & Francis Group, Boca Raton. Jiang, Z.P. and Nijmeijer, H. (1997). Tracking control of mo-bile robots: A case study in backstepping. Automatica, 33, 1393–1399. Kanayama, Y.J., Kimura, Y., Miyazaki, F., and Noguchi, T. (1990). A stable tracking control method for an autonomous mobile robot. In IEEE International Conference on Robotics and Automation, 384–389. Khalil, H.K. (2002). Nonlinear Systems (third edition). Prentice Hall, New York. Koubaa, Y., Boukattaya, M., and Damak, T. (2013). Robust control of wheeled mobile robot in presence of disturbances and uncertainties. In 14th International Conference on Sciences and Techniques of Automatic Control & Computer Engineering - STA’2013, 274–280. Sousse, Tunisia. Liu, C. (2011). Advanced control for miniature helicopters: modeling, design and flight test. Loughborough University, UK. Liu, Y., Yu, S., Guo, Y., Gao, B., and Chen, H. (2017). Receding horizon control for path following problems of wheeled mobile robots. Control Theory & Applications, 34(4), 424–432. Luca, A.D. and Benedetto, M.D.D. (1993). Control of nonholonomic systems via dynamic compensation. Ky-

286

bernetika, 29(6), 593–608. Mayne, D.Q., Rawlings, J.B., Rao, C.V., and Scokaert, P.O.M. (2000). Constrained model predictive control: stability and optimality. Automatica, 36(6), 789–814. Nascimento, T.P., Dorea, C.E.T., and Goncalves, L.M.G. (2018). Nonlinear model predictive control for trajectory tracking of nonholonomic mobile robots: A modified approach. International Journa of Advanced Robotic Systems, January-February, 1–14. Sun, Z., Dai, L., Liu, K., Xia, Y., and Johansson, K.H. (2017a). Robust MPC for tracking constrained unicycle robots with additve disturbances. Automatica, 90, 172– 184. Sun, Z., Dai, L., Xia, Y., and Liu, K. (2018). Event-based model predictive control of nonholonomic systems with coupled input constraints and bounded disturbances. IEEE Trans. Automat. Contr., 63(2), 608–615. Sun, Z., Xia, Y., Dai, L., Liu, K., and Ma, D. (2017b). Disturbance rejection MPC for tracking of wheeled mobile robot. IEEE/ASME Transations on Mechatronics, 22(6), 2576–2587. Xin, L., Wang, Q., She, J., and Li, Y. (2016). Robust adaptive tracking control of wheeled mobile robot. Robotics and Autonomous Systems, 78, 36–48. Xu, K. and Chen, M. (2016). Control of trajectory tracking of mobile robots based on disturbance observer. Journal of Applied Sciences - Electronics and Information Engineering, 34(2), 177–189. Yu, J., Chen, M., and Jiang, C. (2014a). Adaptive sliding mode control for nonlinear uncertain systems based on disturbance observer. Control Theory & Applications, 31(8), 993–999. Yu, S., Li, X., Chen, H., and Allg¨ower, F. (2015). Nonlinear model predictive control for path following problems. Int. J. of Robust and Nonlinear Control, 25(8), 1168– 1182. Yu, S., Reble, M., Chen, H., and Allg¨ower, F. (2014b). Inherent robustness properties of quasi-infinite horizon nonlinear model predictive control. Automatica, 50(9), 2269–2280.