Non‐linear model predictive control for visual servoing systems incorporating iterative linear quadratic Gaussian

IET Control Theory & ApplicationsVolume 14, Issue 14 p. 1989-1994 Brief PaperFree Access Non-linear model predictive control for visual servoing systems incorporating iterative linear quadratic Gaussian Jinhui Wu, Jinhui Wu orcid.org/0000-0002-1012-0319 Department of Automation, Zhejiang University of Technology, Hangzhou, People's Republic of China Zhejiang Provincial United Key Laboratory of Embedded Systems, Hangzhou, 310023 People's Republic of ChinaSearch for more papers by this authorZhehao Jin, Zhehao Jin Department of Automation, Zhejiang University of Technology, Hangzhou, People's Republic of China Zhejiang Provincial United Key Laboratory of Embedded Systems, Hangzhou, 310023 People's Republic of ChinaSearch for more papers by this authorAndong Liu, Corresponding Author Andong Liu lad@zjut.edu.cn orcid.org/0000-0003-2445-8457 Department of Automation, Zhejiang University of Technology, Hangzhou, People's Republic of China Zhejiang Provincial United Key Laboratory of Embedded Systems, Hangzhou, 310023 People's Republic of ChinaSearch for more papers by this authorLi Yu, Li Yu Department of Automation, Zhejiang University of Technology, Hangzhou, People's Republic of China Zhejiang Provincial United Key Laboratory of Embedded Systems, Hangzhou, 310023 People's Republic of ChinaSearch for more papers by this author Jinhui Wu, Jinhui Wu orcid.org/0000-0002-1012-0319 Department of Automation, Zhejiang University of Technology, Hangzhou, People's Republic of China Zhejiang Provincial United Key Laboratory of Embedded Systems, Hangzhou, 310023 People's Republic of ChinaSearch for more papers by this authorZhehao Jin, Zhehao Jin Department of Automation, Zhejiang University of Technology, Hangzhou, People's Republic of China Zhejiang Provincial United Key Laboratory of Embedded Systems, Hangzhou, 310023 People's Republic of ChinaSearch for more papers by this authorAndong Liu, Corresponding Author Andong Liu lad@zjut.edu.cn orcid.org/0000-0003-2445-8457 Department of Automation, Zhejiang University of Technology, Hangzhou, People's Republic of China Zhejiang Provincial United Key Laboratory of Embedded Systems, Hangzhou, 310023 People's Republic of ChinaSearch for more papers by this authorLi Yu, Li Yu Department of Automation, Zhejiang University of Technology, Hangzhou, People's Republic of China Zhejiang Provincial United Key Laboratory of Embedded Systems, Hangzhou, 310023 People's Republic of ChinaSearch for more papers by this author First published: 18 August 2020 https://doi.org/10.1049/iet-cta.2019.1399Citations: 1AboutSectionsPDF ToolsRequest permissionExport citationAdd to favoritesTrack citation ShareShare Give accessShare full text accessShare full-text accessPlease review our Terms and Conditions of Use and check box below to share full-text version of article.I have read and accept the Wiley Online Library Terms and Conditions of UseShareable LinkUse the link below to share a full-text version of this article with your friends and colleagues. Learn more.Copy URL Share a linkShare onFacebookTwitterLinked InRedditWechat Abstract This study proposes a modified non-linear model predictive control (NMPC) to deal with the point stabilisation for the image-based visual servoing (IBVS) of a wheeled mobile robot by using the iterative linear quadratic Gaussian (iLQG) algorithm. Firstly, an IBVS non-linear system is modelled by taking the image coordinates and angle into account simultaneously. By setting a loss function, a model predictive control minimisation problem with an input constraint is formulated. Then, an iLQG-based NMPC strategy is proposed to obtain the optimal solution where a dual-gradient descent is used to deal with the input constraint and a specific Lorentzian ρ-function is added into the loss function to reduce the static error. Finally, numerical simulations and several comparisons are demonstrated to show the effectiveness of the proposed algorithm. 1 Introduction In recent years, as the working environment becomes more and more complex, the perception ability of the robot is becoming more important. As an essential part of intelligent robots, vision has attracted more attention. Visual servoing system is an intelligent system that controls robots to complete tasks by visual signals so as to enhance the extensibility and adaptivity of robots [1]. According to different structures, visual servoing methods can be classified into position-based visual servo [2], image-based visual servo (IBVS) [3-6] and hybrid visual servo [7]. Due to the small computational burden of IBVS, this method has rapidly implemented for the last decade. In the past two decades, a lot of methods have been proposed to deal with visual servoing control problems, such as adaptive control and sliding mode control [8-10]. However, these methods are difficult to handle the constrained problems which normally occur in mobile robots. MPC, also known as receding horizon control, is one of the effective ways to solve constrained control problems, and has been widely used in the field of mobile robots [11-13]. For example, in [11], MPC was used to solve the constrained problem of the non-holonomic mobile robot with a linear cascade model. However, due to the inherently non-linear of IBVS, linear MPC will lead to the performance degradation and even causes the task failure. Under this circumstance, non-linear MPC (NMPC) is just suitable to deal with this problem. In [14], a state feedback predictive control was investigated for six-dimensional non-linear hydro-turbine governing system. In [15], a distributed MPC approach was proposed to complete the formation control for multiple mobile robots with external disturbance and synchronisation constraints. In [16], a distributed economic MPC strategy was designed for non-linear multiagent system to handle the cooperative formation problem with transmission delay. In [17], a Lyapunov-based MPC method was proposed to enhance the performance of trajectory tracking for dynamic underwater vehicle systems. In [18], a collective neural network, including two recurrent neural networks, was proposed to solve the NMPC optimisation problem for a discrete time-invariant non-linear system. However, a key problem of the aforementioned methods is that they have difficulty in formulating and calculating the predicted extension equations if the prediction step is large. Iterative linear quadratic Gaussian (iLPG) was firstly derived for non-linear stochastic systems to eliminate the influence of noises [19-21]. It is a trajectory optimisation method which uses first derivatives of dynamics to optimise the cost-to-go function in a backward and forward manner. Hence, this method can circumvent deriving the MPC prediction equations. Recently, iLQG-based MPC has been proposed in [22] to deal with the stabilisation problem for the non-holonomic four-wheeled vehicle system with the existence of the computational delay. The non-holonomic system was modelled as a chained form and the loss function was chosen as a quadratic form. However, if the quadratic loss function is used for non-linear systems, the system may trap in local optima and result in static errors. Motivated by the above discussions, this paper focuses on employing an iLQG-based NMPC algorithm to solve the non-linear stabilisation task with consideration of angles. The main contributions of this paper are summarised as follows: (i) a modified NMPC based on iLQG is proposed to guide the constrained non-linear IBVS system to stay at a desired posture without formulating the complex MPC prediction equations; (ii) a Lorentzian -function rather than a quadratic one is chosen as the the performance index of MPC to reduce the impact of static errors; and (iii) the result of asymptotical stability for the proposed iLQG-based NMPC method is presented. 2 Model description The considered visual servoing system is depicted in Fig. 1, where , and denote the frameworks of world, robot and camera, respectively. is the coordinate of feature point with respect to the camera coordinate system, represent the linear velocity and angular velocity of the robot and camera, respectively. Fig. 1Open in figure viewerPowerPoint Visual servoing system for the wheeled mobile robot Based on the camera model, the relationship between pixel coordinate of features and is shown as follows [23]: (1) where is the offset between the origin of image coordinates and pixel coordinates, is the focal length of the camera. Differentiating (1), one has (2) where x and y are the image coordinates of x -axis and y -axis, respectively. As can be seen in Fig. 1, without loss of generality, we here assume that the movement of the robot is limited in the plane. Hence, the velocity vector of the robot in the robot coordinate can be defined as and in the camera coordinate can be defined as . According to the physical relationship between the angular velocity and the linear velocity shown in Fig. 2, we have (3) where , and denotes the radius of rotation. , , , , and denote the components of velocity of the mobile robot in the x -axis, y -axis, z -axis, respectively. Differentiating (3), one has (4) that is (5) Then, substituting (5) into (2), a non-linear IBVS model for the mobile robot can be derived as follows: (6) Since , we can rewrite (6) in a more compact manner: (7) where the state vector , image Jacobian matrix system input With the aim of driving the robot to a unique position in the world framework, the angle of the robot should also be considered. Combining with the fact that , as a result, the model can be further derived as follows: (8) where By setting as the desired image coordinates and angle, the error vector is defined as follows: The discrete error model of system (8) is also derived with sampling period T : (9) where . Fig. 2Open in figure viewerPowerPoint 3D relationship between the angular velocity and the linear velocity In summary, the goal of this paper is to design a controller for system (9) to drive the robot to reach the target posture such that . 3 iLQG-based NMPC This section focuses on designing an iLQG-based NMPC control scheme to achieve the desired posture. As the main objective of MPC is to minimise the performance index, we define the following performance index at time step t : (10) where is the running cost and is the terminal cost. In general, the quadratic items and are chosen to penalise the system, where and are weight matrices and is the terminal weight matrix. However, they are inadequate because the non-linearity is prone to fall into the local optimality and lead to static errors. Some approaches have been employed to deal with this situation, such as particle swarm optimisation [24] and approximation methods [25]. Inspired by Levine et al. [26], we here choose a non-linear loss function where the third item of (or the second item of ) is called the Lorentzian -function. and are the corresponding positive definite weight matrices which trade off the penalties [between and log ], and is the terminal weight matrix. The relationship between the error state and the values of this -function is shown in Fig. 3. As we can see in Fig. 2, the Lorentzian -function is similar to the negative normal distribution, and the parameter r is to determine its minimum and its width of distribution. Remark 1.It should be pointed out that our main concern is that the non-linear task (10) cannot be stabilised. If the quadratic items and are used, these items will become small when the robot enters into the vicinity of the desired position. Since we have to consider the desired angle, this loss function cannot sufficiently penalise small errors and will be prone to fall into the local optimum (this phenomenon can been seen in the 'Simulation' part). On the contrary, the Lorentzian -function still have a negative curvature when the robot is close to the target. This character encourages the algorithm to converge on the target more precisely when the robot is already close to the target. Fig. 3Open in figure viewerPowerPoint Lorentzian -function In addition, to prevent violating the physical constraint of the real robot system, the following constraint is given: (11) In order to handle the input constraint, the following modified performance index is further derived by introducing the dual variables and : (12) where (13) Various updating rules of Lagrange multipliers have been discussed in [27]. In this paper, the following rules are given: (14) (15) where represents the updating rate. iLQG is a kind of recursive optimisation algorithm. To show the entire process of the proposed algorithm for system (9) and the performance index (12), we first suppose that there exists a trajectory and a value function (16) During the time interval , the backward propagation of the value function is derived as (17) The key to success for the iLQG algorithm lies in the calculation of the perturbation between t th trajectory and ()th trajectory. Hence, the following function is considered: (18) According to the Taylor series expansion, we expand (18) as the following second-order form: (19) where and means the value function at time step . Based on the Karush–Kuhn–Tucker conditions, the optimal can be obtained as follows: (20) Substituting (20) into (19), we have (21) (22) By calculating the values of , , and from time t to , the backward propagation is completed, and the learning process is finished. When the learning process is finished, a forward propagation should be performed to generate a new trajectory by using the obtained control sequence , where , , the detailed forward updating laws are shown as follows: (23) Finally, the optimal trajectory can be eventually obtained by performing the backward and forward propagations repeatedly. The proposed algorithm is summarised in Algorithm 1 (see Fig. 4). Remark 2.Notice that in [19], the iLQG has been derived as a simpler variant of differential dynamic programming algorithm which has the quadratic convergence properties. Hence, we can guarantee that optimal solution of iLQG in (20) can be attained. Fig. 4Open in figure viewerPowerPoint Algorithm 1: the proposed iLQG-based NMPC algorithm According to Remark 2, iLQG allows us to obtain the optimal error state and the corresponding input , and we can rewrite the performance index (10) as the optimal one , that is . According to the optimum theory and (24) we have (25) By using the similar method in [28], a suitable terminal weigh matrix for can be designed to guarantee the following inequality: (26) As a result, the following inequality holds for all t. Based on the above discussion, the result of asymptotical stability for the proposed iLQG-based NMPC method can be stated as the following theorem. Theorem 1.If there exists the terminal weigh matrix that satisfies inequality (26), system (9) along with performance index (10) will be asymptotically stable and will be a Lyapunov function. 4 Simulation In this section, some simulations and comparisons are given to substantiate the advantage of the proposed iLQG-based NMPC algorithm. During the whole simulation, the state is defined as . The initial state is and the initial velocity is . The target posture is . The camera parameters are given as , , , and the camera height . The constraint of input is . The prediction step of MPC in each iLQG step is and the parameters in Algorithm 1 are set as . The parameters of the proposed loss functions are given as , , , and . As the parameters , and can influence the amplitude of oscillation and the control accuracy, we further give to show the characteristic of the proposed algorithm. Meanwhile, the quadratic items with weight matrices and are given and the results are shown in Fig. 5, where blue dash-dot line, red dotted line and green dashed line represent the results of , and the quadratic items, respectively. It can be seen that the trends are totally different at the end of the process. Fig. 5a shows the trajectories in the image plane. When the quadratic items are used to perform this task, the control process is smooth. However, the static error cannot be dealt with under this situation. The proposed loss functions can tackle the static error well although the oscillation occurs when close to the desired position. In Fig. 5b, the paths of the robot in the world plane are plotted and the quadratic items stay near which is far from the desired coordinates. When the proposed loss functions are given with , the robot can to some extent reduce the error in a way of moving forward and backward. Eventually, red dotted line () can reach the desired position. Fig. 5c demonstrates the change of . We can see that the value of stops changing after approximately 40 steps, which can explain directly why the quadratic items cannot drive the robot to the desired posture. When the proposed loss functions are added, the value of begins to oscillate after approximately 50 steps. Although the blue one also cannot converge to the desired angle, this pair of loss functions enhance the ability to reduce the static error. The suitable pair of loss functions (shown in the Fig. 5c with red dotted line) can complete the stabilisation task with the tolerant static error. The above analysis proves the advantage of the proposed iLQG-based NMPC with the specific Lorentzian -function. Fig. 5Open in figure viewerPowerPoint Comparison with different loss functions In order to show the necessity of the combination of the MPC and the strong impact of the prediction step, we choose three different cases as , and , and the results are shown in Fig. 6. Figs. 6a and b imply that the trend of the pure iLQG (i.e. ) is not smooth and unable to reach the desired position in 150 steps. Moreover, the angle even degenerates after 30 steps, falling down from to , which leads to the linear change both in the image layer and in the world layer. The difference between and is that the robot can be closer to the desired posture when . When , the robot approaches the desired posture after 50 steps. Then, it starts to oscillate in order to reach the desired posture and finally completes the task after 120 steps. Based on these three cases, we can conclude that it is necessary to combine the MPC into iLQG to solve the IBVS stabilisation task for wheeled mobile robot systems. Fig. 6Open in figure viewerPowerPoint Comparison with different N It is well known that MPC is a powerful control scheme which increases the control accuracy by increasing the prediction steps in a recursive manner, but the computational burden will become heavier. We further calculate the running time when in Table 1 by using 50-time Monte Carlo test, which contains average one-step running times and average final errors after operating 150 loops. We can see that when the prediction step increases from 0 to 20, the error deceases immediately. However, when the prediction step increases from 20 to 30, the running time increases from to but the error only decreases from to . These data imply that small prediction steps have short running times but poor control accuracy. Also, if N becomes larger, the static error will remain basically unchanged while the running time will increase rapidly. Table 1. Running times with different prediction steps Prediction steps Running time, s e, mm, mm, deg 0 0.058 ( ) 10 0.401 ( ) 20 0.809 ( ) 30 1.235 ( ) As illustrated by the above comparisons of control accuracy, computational burden under different performance indexes and different prediction steps, the proposed iLQG-based NMPC is capable of dealing with the non-linear stabilisation with suitable prediction steps and parameters of the Lorentzian -function. Although the added Lorentzian -function may cause oscillation when the robot is close to the desired posture, it can enhance the performance of the IBVS wheeled mobile robot system in terms of avoiding the local optimality and reducing the static error effectively. 5 Conclusion In this paper, the NMPC controller was designed by using the iLQG algorithm for the wheeled mobile robot IBVS system to complete the point stabilisation task. Based on the camera perspective projection, the error model of the system was constructed as a non-linear form where the angle was also considered to guide the robot to the unique posture under the world layer. By defining an MPC performance index, the constrained MPC minimisation was derived. In order to deal with the local optimality of the NMPC, an iLQG-based NMPC strategy with the specific Lorentzian -function was proposed to obtain the optimal solution. Finally, numerical simulations and comparisons with different prediction steps and different performance indices illustrated the advantage of the proposed algorithm. 6 Acknowledgments The authors thank the anonymous reviewers for their valuable suggestions to improve the quality of this PAPER. This work was supported partly by the National Natural Science Foundation of China under Grants 61973275, the NSFC-Zhejiang Joint Foundation for the Integration of Industrialization and Informatization under Grant U1709213, the Key R&D Foundation of Zhejiang under Grant 2020C01109, the Talent Project of Zhejiang Association for Science and Technology under Grant 2018YCGC018. 7 References 1Chaumette F. Hutchinson S.: 'Visual servo control. I. basic approaches', IEEE Robot. Autom. Mag., 2006, 13, (4), pp. 82– 90CrossrefWeb of Science®Google Scholar 2Shu T. Gharaaty S. Xie W. et al.: 'Dynamic path tracking of industrial robots with high accuracy using photogrammetry sensor', IEEE/ASME Trans. Mechatronics, 2018, 23, (2), pp. 1159– 1170CrossrefWeb of Science®Google Scholar 3Gao J. Proctor A. Shi Y. et al.: 'Hierarchical model predictive image-based visual servoing of underwater vehicles with adaptive neural network dynamic control', IEEE Trans. Cybern., 2016, 26, (10), pp. 2323– 2334CrossrefWeb of Science®Google Scholar 4Liang X. Wang H. Liu Y. et al.: 'Image-based position control of mobile robots with a completely unknown fixed camera', IEEE Trans. Autom. Control, 2018, 63, (9), pp. 3016– 3023CrossrefWeb of Science®Google Scholar 5Wu J. Chen X. Liu A. et al.: 'Predictive control for visual servoing control of cyber physical systems with packet loss', Peer-to-Peer Netw. Appl., 2019, 12, pp. 1774– 1784CrossrefWeb of Science®Google Scholar 6Liang X. Wang H. Chen W. et al.: 'Adaptive image-based trajectory tracking control of wheeled mobile robots with an uncalibrated fixed camera', IEEE Trans. Control Syst. Technol., 2015, 23, (6), pp. 2266– 2282CrossrefWeb of Science®Google Scholar 7He Z. Wu C. Zhang S. et al.: 'Moment-based 2 1/2D visual servoing for texture-less planar part grasping', IEEE Trans. Ind. Electron., 2019, 66, (10), pp. 7821– 7830CrossrefWeb of Science®Google Scholar 8Chen J. Jia B. Zhang K.: 'Trifocal tensor-based adaptive visual trajectory tracking control of mobile robots', IEEE Trans. Cybern., 2016, 47, (11), pp. 3784– 3798CrossrefPubMedWeb of Science®Google Scholar 9Yi Y. Chen D.: 'Disturbance observer-based backstepping sliding mode fault-tolerant control for the hydro-turbine governing system with dead-zone input', ISA Trans., 2019, 88, pp. 127– 141CrossrefPubMedWeb of Science®Google Scholar 10Yi Y. Chen D. Xie Q.: 'Controllability of nonlinear fractional order integrodifferential system with input delay', Math. Methods Appl. Sci., 2019, 42, (11), pp. 3799– 3817Wiley Online LibraryWeb of Science®Google Scholar 11Ke F. Li Z. Xiao H. et al.: 'Visual servoing of constrained mobile robots based on model predictive control', IEEE Trans. Syst. Man Cybern. Syst., 2017, 47, (7), pp. 1428– 1438CrossrefWeb of Science®Google Scholar 12Liu A. Zhang W. Chen M. et al.: 'Moving horizon estimation for mobile robots with multirate sampling', IEEE Trans. Ind. Electron., 2017, 64, (2), pp. 1457– 1467CrossrefWeb of Science®Google Scholar 13Lee S. Myung H.: 'Receding horizon particle swarm optimisation-based formation control with collision avoidance for non-holonomic mobile robots', IET Control Theory Applic., 2015, 9, (14), pp. 2075– 2083Wiley Online LibraryWeb of Science®Google Scholar 14Yi Y. Zhang Z. Chen D et al.: 'State feedback predictive control for nonlinear hydro-turbine governing system', J. Vib. Control, 2018, 24, (21), pp. 4945– 4959Web of Science®Google Scholar 15Liu A. Zhang W. Yu L. et al.: 'Formation control of multiple mobile robots incorporating an extended state observer and distributed model predictive approach', IEEE Trans. Syst. Man Cybern. Syst., 2018, doi 10.1109/TSMC.2018.2855444 Web of Science®Google Scholar 16He D. Lu L. Luo R. et al.: 'Energy-efficient cooperative predictive control for multi-agent non-linear systems with transmission delay', IET Control Theory Applic., 2019, 13, (17), pp. 2730– 2737Wiley Online LibraryWeb of Science®Google Scholar 17Shen C. Shi Y. Buckham B.: 'Trajectory tracking control of an autonomous underwater vehicle using Lyapunov-based model predictive control', IEEE Trans. Ind. Electron., 2017, 65, (7), pp. 5796– 5805CrossrefWeb of Science®Google Scholar 18Yan Z. Wang J.: 'Nonlinear model predictive control based on collective neurodynamic optimization', IEEE Trans. Neural Netw. Learning Syst., 2015, 26, (4), pp. 840– 850CrossrefPubMedWeb of Science®Google Scholar 19Todorov E. Li W.: ' A generalized iterative LQG method for locally-optimal feedback control of constrained nonlinear stochastic systems'. Proc. of American Control Conf., Portland, OR, USA, 2005, pp. 300– 306CrossrefGoogle Scholar 20Kaczynski P. Socha L.: 'Iterative procedures in application of the LQG approach to control problems for polynomial stochastic systems', IEEE Trans. Autom. Control, 2010, 55, (8),pp. 1875– 1881CrossrefWeb of Science®Google Scholar 21Park B. Oh S. Lee J.: 'Stochastic iterative learning model predictive control based on stochastic approximation', IFAC-Papers On Line, 2019, 52, (1), pp. 604– 609CrossrefGoogle Scholar 22Qian Y Ushio T.: ' Receding horizon control with iLQG method considering computational delay and its application to nonholonomic systems'. IEEE Conf. on Control Technology and Applications, Kohala Coast, Hawaii, USA, 2017, pp. 644– 649CrossrefGoogle Scholar 23Ke F. Li Z. Yang C.: 'Robust tube-based predictive control for visual servoing of constrained differential-drive mobile robots', IEEE Trans. Ind. Electron., 2018, 65, (4), pp. 3437– 3446CrossrefWeb of Science®Google Scholar 24Eberhart R. Shi Y.: ' Particle swarm optimization: developments, applications and resources'. Proc. of Congress on Evolutionary Computation, Seoul, South Korea, 2001, pp. 81– 86CrossrefGoogle Scholar 25Pan Y. Wang J.: 'Model predictive control of unknown nonlinear dynamical systems based on recurrent neural networks', IEEE Trans. Ind. Electron., 2012, 59, (8), pp. 3089– 3101CrossrefWeb of Science®Google Scholar 26Levine S. Wagener N. Abbeel P.: ' Learning contact-rich manipulation skills with guided policy search'. IEEE Int. Conf. on Robotics and Automation, Seattle, Washington, USA, 2015, pp. 156– 163CrossrefGoogle Scholar 27Boyd S. Vandenberghe L.: ' Convex optimization' ( Cambridge University Press, UK, 2004) CrossrefGoogle Scholar 28Liu A. Zhang W. Yu L.: 'Robust predictive trajectory tracking control for mobile robots with intermittent measurement and quantization', IEEE Trans. Ind. Electron., 2019, doi: https://doi.org/10.1109/TIE.2019.2962424Web of Science®Google Scholar Citing Literature Volume14, Issue14September 2020Pages 1989-1994 FiguresReferencesRelatedInformation