Optimal path planning of

Nonlinear Dyn(2014)78:1515–1543DOI10.1007/s11071-014-1532-9ORIGINAL PAPEROptimal path planning of a cable-suspended robot with moving boundary using optimal feedback linearization approachM.H.Korayem·H.Tourajizadeh·A.Zehfroosh·A.H.KorayemReceived:3February2014/Accepted:9June2014/Published online:1July2014©Springer Science+Business Media Dordrecht2014Abstract In this paper,the optimal path of a cable-suspended robot between two boundaries is assessed subject to its maximum load,while the initial and tar-get points are moving boundary.Considering the fact that the most important application of cable robots is load carrying between two boundaries,planning the optimal path in which the heaviest load can be car-ried is extremely applicable.A closed loop optimal path planning algorithm is proposed in this paper for non-linear dynamic of a cable robots based on opti-mal feedback linearization.This method not only pro-duces the optimal path of the end-effector,but also is robust to external disturbances and parametric uncer-tainties as a result of its closed loop nature.Moreover, considering the fact that in many automation applica-tions the target of load handling is a sort of moving boundary like conveyors,finding the optimal point of this boundary which produces the optimal path with the maximum dynamic load carrying capacity(DLCC) amongst the other points of the boundary is obviously a useful study.Therefore,an online solution,based on variational algorithm is proposed here to solve the moving boundary problem which is compatible with M.H.Korayem(B)·H.Tourajizadeh·A.Zehfroosh·A.H.KorayemRobotics Research Laboratory,Center of Excellencein Experimental Solid Mechanics and Dynamics,School of Mechanical Engineering,Iran Universityof Science and Technology,Tehran,Irane-mail:hkorayem@iust.ac.ir the presented closed loop optimal path planning.This method is developed for both the initial andfinal mov-ing boundaries.Finally maximum DLCC is obtained using an iterative method to check the optimality of the proposed method.The efficiency of the proposed algo-rithm is verified at the end by the aid of some simulation scenarios performed on a spatial six DOFs cable robot with six cables.The motors’torque,motors’speed,and resultant DLCCs are calculated for both simple optimal path and moving boundary cases and comparison of the results proves the mentioned claims based on superior-ity of the proposed algorithm of moving boundary in saving the energy and increasing the DLCC.All sim-ulation results are supported by conducting an experi-mental study on the cable robot of IUST(ICaSbot)for regulation movement of the end-effector with moving boundary.Keywords Optimal regulation·Moving boundary·Cable suspended robot·Feedback linearization control·Dynamic load carrying capacity1IntroductionCable robots are considered as a modified version of cranes in which carrying loads can be controlled bet-ter within its predefined trajectory or point to point movement.Therefore,finding the maximum load car-rying capacity and its optimal path is definitely a useful research for industrial environments.However,in order1516M.H.Korayem et al.to neutralize the destructive effect of load swinging, which is common for under constrained cable robots, closed loop optimal control seems to be necessary.As a result not only the stability of the end-effector is assured,but also the optimal path is tracked and so the heaviest load is carried along.To meet this goal a nonlinear controlling algorithm needs to be com-bined with a proper optimizing method.The objec-tive function of this paper should be defined based on required constraints related to maximizing the pay-load.Two dynamic constraints are supposed to be min-imized in this paper including allowable accuracy of the robot,and allowable torque of the motors while the constraint of positive tension of the cables is supposed to be satisfied since the robot is maneuvering within its allowable dynamic workspace.Moreover,another geo-metrical constraint for special applications of robots in automation process is considered here which let the system to have even more load carrying capac-ity.In many robotic applications like automation lines, the exact position of the boundary points of the end-effector are not definitely determined since the load can start or end its movements on a line or a curve which is the conveyor profile of the automation line. This line or curve is called moving boundary of the optimal path.The initial condition of the robot can also be moving boundary in some applications.It is desired that the robot chooses the optimal point of this initial orfinal moving boundaries which results in the maximum load carrying capacity.In many automation processes,the gripping and manipulating speed and the speed of placement of the particles on the conveyor is considerably less than the speed of the conveyor.More-over,since the speed of the conveyor is simply control-lable by the aid of an operator,it is always possible to provide a free space on the conveyor through which the particle can be placed in.Thus,it is always pos-sible to have a free space boundary on the conveyor as the start or end point of the robot’s end-effector for which the optimal point can be determined.Therefore, the particle is replaced with considerably less energy while no malfunctionality is occurred on the process of the automation since all of the points of the con-veyors eventually are led to a unique destination.The main purpose of this paper is tofind out the optimal path of the cable robot and its corresponding maxi-mum DLCC between a definite or moving initial point and a moving boundaryfinal point in a closed loop way.Some researchers have been performed in this area so far.The initial researches of optimizations related to the DLCC of the robots are conducted using open loop approaches like Hamilton–Jacobi–Bellman(HJB) method which were not robust to parametric uncertain-ties or external noises.For example,open loop opti-mal control of a spatial cable robot is done by Alp for a six DOFs cable robot with six cables[1]and by Williams for two samples of planar cable robots [2]in order to minimize the cables tension.Applica-tions of cable robots made researchers study differ-ent aspects of them such as dynamic modeling,con-trol strategies,load carrying capacity,accuracy and also designing more efficient cable robots.For exam-ple,Pott et al.introduce a new version of cable driven robot,IPAnema,that has more applications for indus-trial processes[3],or Merlet studied a redundantly actu-ated robot with eight cables[4].Moreover,stability of underconstrained cable driven robot is studied in [5].Accuracy of cable robot while carrying payload, which is one of their important application,is stud-ied and improved by Pott and co-workers[6].Opti-mizing the DLCC of a six cable robot is done by[7] using open loop variational approach.Iterative linear programming(ILP)method is employed by Korayem et al.[8]for the same cable robot in order to maxi-mize the payload of the system.Moreover,the men-tioned cable robot with elastic characteristics is opti-mized in[9]using open loop optimization approach. Considering the fact that in open loop approaches the robustness of the system to disturbances and uncertain-ties is not guaranteed,some algorithms were developed which planed the optimal path under the control of a closed loop system with different objective functions. For example feedback linearization method is used in [10]in order to optimal distributing of cables’tension for a constrained sample of a cable robot by Fang et al.Trevisani has also optimized a constrained sample of cable robot to achieve the minimum cables’tension [11].Optimization of tracking accuracy of a cable robot is performed in[12]using adaptive controller.PD con-troller is used in[13]in order to minimize the motors’torque.Maximizing the load capacity of cable robots as the desired objective function is considered in[14]for a predefined trajectory withflexible joints.Also DLCC of a crane is optimized in[15–17]for crane applica-tions using robust controller.State dependent Riccati equation(SDRE)is used in[18]to optimal control of aflexible cable transporter system while the stabilityOptimal path planning of a cable-suspended robot with moving boundary1517is guaranteed by robust H-infinity method.The same method(SDRE)is used by Korayem and Zehfroosh for optimal motion planning of cable robot in the pres-ence of obstacles and moving boundaries[19]which is nonlinear with high calculation process and is not proper for real time applications.Korayem and Toura-jizadeh focused on obstacle as a constraint of optimal path planning and proposed a proper strategy to avoid collision and combined this strategy with adaptive opti-mal feedback linearization approach[20].The fact that in some industrial environments the boundaries are not determined as a point,the idea of extra optimization of the robots was ignited in[21]for moving boundary cases but in an open loop way.However,something has not been done yet which is necessary to be considered.In this paper,the lack of [21]i.e.,its open loop nature,is improved.The opti-mal path of a spatial under constrained cable-suspended robot which acts under the control of a closed loop con-troller is assessed while its initial and target points are moving boundary and their related maximum DLCC are also calculated.This goal is satisfied in this paper by designing a closed loop optimal strategy combined of feedback linearization controlling method which is a proper controlling strategy for non-linear systems, together with optimizing tool of LQR(linear quadratic regulator)which is a strong optimizing algorithm of linear systems.Destructive effects of nonlinearity of the plant are neutralized by exact linearizing the sys-tem using feedback linearization method.Therefore, stability of the system is guaranteed by Lyapunov and optimality and robustness of the closed loop plant is provided by LQR which has an acceptable phase and gain margins.Thus,contrary to the open loop optimal path planning approaches,not only the optimal path is found in an online way,but also the procedure is robust against any kind of external disturbances and paramet-ric uncertainties since the optimal path is tracked using the actual position feedback of the end-effector.More-over,to solve the moving boundary problem in thefinal point,a geometrical based method is proposed accord-ing to variational approach,whichfinds the optimal point of any kind of moving boundaries consisting of straight line,curve or surface shaped boundaries for a closed loop system with a fast calculation capabil-ity which is suitable for online processes.Therefore,a complementary algorithm is attached to the designed optimal controller and a method is derived tofind the instantaneous optimal point of the moving boundary in an on line way.This point is fed to the controller as the online setpoint to minimize the consumed energy.Con-sidering the fact that the optimal path of the end-effector is not a straight line,the optimal point is not a constant target and varies during the regulation process.An iter-ative method is also proposed tofind the optimal point of moving boundary at the starting point.Moreover, the effect of gravity on the moving boundary is studied and the efficiency of the proposed moving boundary strategy at thefinal and initial points is compared.First of all,the non-linear formulation of parallel cable robot are represented and then feedback lineariza-tion is implemented which converts the dynamic into linear state space.Secondly,the proposed closed loop control algorithm is optimized based on LQR.After-ward,the employed iterative algorithm of calculating the maximum load carrying capacity of this system is portrayed based on an objective function consist-ing of end-effector’s error and motors’torque.Finally, the geometrical constraint of DLCC which isfinding the optimal point of a moving boundary is then real-ized by proposing a complementary variational based strategy forfinal point while an iterative method is derived for initial moving boundary.The efficiency of the designed controller and also the proposed algo-rithm of moving boundary problem are verified by the aid of a series of simulation studies done on a spa-tial six DOFs under constrained cable robot and com-paring the results between the simple system and the one which is equipped by moving boundary strategy. Four different categories of moving boundary problems are studied in this paper including straight line mov-ing boundary,curve and surface shaped samples and finally a generalized case in which both of initial and final points are moving boundaries.The superiority of the closed loop over open loop approaches,the effect of gravity and involved DOFs,differences of initial and final moving boundaries and the relation between the torque changes and the distance of thefixed setpoint to the optimal one which is calculated by the aid of moving boundary strategy is also investigated in this section.The related optimal DLCC is calculated then and the effect of the proposed algorithm on this objec-tive parameter is analyzed.DLCC is calculated using an iterative method with an analytic approach which is secure from numerical problems or local optimiza-tion syndrome.The effect of the proposed method on the torque and optimal path and eventually its related DLCC is investigated.At the end,all of the theoreti-1518M.H.Korayem et al.cal claims and simulation results are verified by com-paring them with experimental studies which are con-ducted on the Iran University of Science and Technol-ogy Cable-Suspended Robot(ICaSbot).This robot is an under constrained spatial cable robot with six degrees of freedom and six active cables.The good compatibil-ity of the simulation and experimental results validates the correctness of the proposed algorithm and the opti-mality of the proposed moving boundary strategy is practically investigated.2Optimal feedback linearization formulation Formulation of optimal path planning between two points in a closed loop way and defining its DLCC is presented in this section.The moving boundary con-dition is solved as a new constraint of optimization.2.1Dynamic and optimal control schemeThe“n”notation presents the global coordinate of the robot while the“b”presents its local one.Dynamic equation of an under constrained cable-suspended robot with six DOFs and six supporting cables of Fig.1 can be described as Eq.(1)[1]:D(X)¨X+C(X,˙X)˙X+g(X)=S T J(q(X))T;X={x m,y m,z m,ϕ, ,ψ}(1)where X is the vector of translational and rotational DOFs of the end-effector,T is the tension vector oftheFig.1Spatial sample of cable robot[1]cables,q is the length of the cables m is the load of the end-effector and I is the moment of inertia of the end-effector.Also we have:D=m I300P T I P;C=03P T{I˙P˙o+(P˙o)×I(P o)};g=⎡⎢⎢⎣−mg03⎤⎥⎥⎦;S J=∂q i∂x ji×j;P=⎡⎣10−sinθ0cosψsinψcos0−sinψcosψcos⎤⎦;˙o=⎡⎣˙˙˙ϕ⎤⎦D is the inertia matrix of the robot;C and g are its Cori-olis matrix and gravity vector respectively and S J is the Jacobian matrix.According to[22]using feedback linearization method results in the following required cables’tension and motors’torque:T i=S−T J(D(X)ν+C(X,˙X)+g(X))i⇒τi=r S−T J(Dν+C+g)+J¨β+c˙β;i=1,...6(2) where J is the rotary inertia of the motor,r is the radius of the drum,˙βis its angular velocity,c is the viscose damping of the motor andνis the control input of the outer closed loop of feedback linearization and can be evaluated as below to create a controllable error equa-tion which can be stabilized using LQR:νi=¨X d+K i D(˙X d−˙X a)+K i P(X d−X a);i=1,...,6(3) where K i D and K i P are controlling gains of deriva-tive and proportional errors respectively and should be optimized in order to have an optimal regulation[22]. Indeed,outer closed loop controlling signal deals with converting the non-linear state space of the robot into linearized closed loop state space which can be opti-mized using LQR,while the inner closed loop con-trolling signal is related to calculating the optimized required motors’torque.Based on Eqs.(2,3),the non-linear state space of the cable robot,is converted into linear one which can be stabilized and optimized using proper controlling gains by the aid of LQR.The control procedure can be shown in Fig.2.Since the related state space is linearized,it is pos-sible to use LQR to optimize the controlling gains.ToOptimal path planning of a cable-suspended robot with moving boundary1519Fig.2Control procedure of the robot based on optimal feedback linearizationachieve this goal,the constraints which are involved in optimizing the objective function should be deter-mined.Two main constraints of calculating the DLCC of a robot including required motors’torque and track-ing error are supposed to be minimized by the aid of LQR method in order to obtain the maximum load capacity.So,the cost function can be defined as below:J L Q R (X ,t )=∞t(e r (X ,t )T Qe r (X ,t )+u (X ,t )T Ru (X ,t ))d τ(4)where e r is the error of the state space of the system,Q is the penalty matrix of accuracy,R is the penalty matrix of control input and u is controlling input.Solv-ing this cost function results in the Ricatti equation in which its stability is also assured by the aid of Lyaponov Equation [23].Finally solving the mentioned Riccati equation results in the following optimal control input:u ∗=−R −1B T Se r (X ,t )=−R −1B T S [e ;˙e](5)where e and ˙e are the error of position and velocityrespectively,S is the resultant of Riccati equation and B is the controlling matrix of the linearized state space.This obtained optimal value u ∗should be substituted in the last two terms of Eq.(3)to calculate the linearized optimal controlling input.νio=¨X id −R −1B T Se r (X ,t )⇒τio =r S −T JD ¨X id −R −1B T Se r (X ,t ) +C +g +J ¨β+C ˙β;i =1,...6(6)In a regulation process the setpoint is constant so,wehave ¨Xd =0.Also other matrices of the controlling vector in a regulation process should be substituted using feed forward terms of inverse dynamics.Thus,by optimizing the controlling gains,whole the controlling input will be optimized and the resultant path which is tracked by this control signal will be the optimum one:X o =1D (X ) −C (X ,˙X )˙X −g (X )+S J J −T (D (X )(v o )+C (X ,˙X )+g (X ))d t (7)where X o is the optimal path and v o is the optimalcontrolling signal.As a result,a closed loop optimal path planning is provided using feedback linearization together with LQR,by which both the optimality and stability are provided.The moving boundary constraint is also satisfied in the next section.2.2DLCC formulationIn this section,the goal is to define the optimal DLCC of the robot based on the motors’torque constraint,accuracy constraint and moving boundaries constraint.Considering the fact that for an under constrained cable robot there is no remedy to guide the end-effector along a path with negative cable’s tension,the positive ten-sion of the cables are supposed to be satisfied since the robot is considered to be operating within its allowable dynamic workspace.In [14]the required iterative method was presented to find the DLCC of a robot based on torque and error1520M.H.Korayem et al. Fig.3Flowchart ofdetermining the DLCC of aclose loop cable robotconsidering movingboundaryconstraints.This method is modified for the closed loop form and considering boundary constraint.Thefinal algorithm can be explained like theflowchart of Fig.3. The maximum DLCC is estimated by the aid of an itera-tive procedure.In this closed loop approach,the torque and accuracy constraints should be satisfied simulta-neously afterfinding the optimal point of the moving boundary in order to extract the maximum DLCC.The payload of the system after defining the optimal point of the moving boundary is increased gradually in an iteration loop as long as thefirst constraint is violated. The penalty gains of error and controlling input in the objective function of Eq.(4)should be set by the aid of LQR in a way that result in simultaneous satura-tion of torque and accuracy to have the highest DLCC. So,the procedure in each iteration,is a pure analytic approach which is applicable in online regulation and is secure from numerical problems or local optimiza-tion syndromes.It is good to note that,only the process offinding the maximum DLCC is iterative,and this process do not affect the proposed optimal closed loop control method described in Sect.2.2and Fig.3.Thus, the proposed optimal path planning process with mov-ing boundary and regulation method of control proce-dure is completely on-line which is safe from iterative destructive effects.2.2.1Allowable motors’torque and accuracyconstraintsThe constraints for estimating the DLCC includes two dynamic constraints i.e.,allowable motors’torque and allowable accuracy of the end-effectors and a geomet-rical constraint which is moving boundary condition of the end-effector.The motor torque constraint restricts the applied torque within the allowable bounds of the motor and accuracy constraint restricts the end-effector within its predefined allowable error bounds which should be defined by the operator.The coefficient of these two constraints and their effects on the DLCC of the robots are studied in[14].Optimal path planning of a cable-suspended robot with moving boundary 15212.2.2Straight line moving boundaryIn this section it is supposed that the second bound-ary of the optimal path is not a determined point and is substituted by a line.It is expected that the designed optimal controller finds its optimal way toward the spe-cial point of this moving boundary for which the max-imum load capacity is achieved.A typical sample of this situation is the automation lines in which the grip-per is supposed to transfer the product from a specific initial point toward a point on the conveyor line which can be treated as the moving boundary.Considering the fact that the destination of all of the points of the moving line in a conveyor is a unique target,the loca-tion of the second point is not important for the object.However,choosing the point of the automation line for which the heaviest load can be carried is a privilege.It is shown in [21]that whenever an objective func-tion (F (t ,y 1,y 2...,y n ,y 1,y 2,...,y n ))like Eq.(8a )is supposed to be optimized and its final point is a mov-ing boundary (g (t ,y 1f ,y 2f ,...y n f )),the condition of Eq.(8b )should be satisfied based on variational prin-ciple:I =BAF (t ,y 1,y 2...,y n ,y 1,y 2,...,yn )d t ;yi =d y i /d t (8a)F −n i =1y i ∂F ∂y i t =t f δt f + n i =1∂F ∂yi t =t fδy i f =0(8b)where y 1,...,y n are the DOFs of the end-effector.Forthe closed loop condition in which the objective func-tion of LQR is supposed to be satisfied,and considering the paths with the same time duration (δt f =0)substi-tuting the LQR objective function in the above equation results in:6 i =1∂F∂X i δX f =0;X =[x ;y ;z ;ψ;θ;φ](9)where X f is the coordinate of the final point of the end-effector on the moving boundary.The mentioned equa-tion should be solved to find out the optimal point of the moving boundary which needs slightly a heavy cal-culation.However,based on the mentioned proposed controlling strategy in which the motors’torque are defined based on feedback linearization and are opti-mized based on LQR,the functional of LQR (Eq.(4))can be rewritten as Eq.(12)in which the controlling input is substituted by Eq.(6).According to Eqs.(2,3)we have the following error equation for the closed loop system:¨e i +K D ˙ei +K P e i =0;i =1,...6⇒e i =e λj t;λj = −K i P +− K 2i P −4K i D /2(10)Thus:˙e i = λj e λj t ⇒˙e=f (e )(11)And so,the objective function is purely a polynomial function of single term of DOFs error:F (X ,t )=e r (f (e ))T Qe r (f (e ))+(K i D (f (e ))+K i P (e ))TR (K i D (f (e ))+K i P (e ))F =f (e )⇒e →min(12)where e r is the vector including e and ˙e .Base on Eq.(8),time (t)is the independent variable,error and itsderivatives are the dependent variables (y =e ;y =˙e).Substituting the above functional in Eq.(8)and con-sidering the fact that the error’s derivative are also a function of error vector,illustrate that the error vector needs to be minimized in order to satisfy the moving boundary condition and obtaining the minimal motors’torque.It means that based on the proposed regulation process the strategy of finding the optimal path is eas-ier with less calculation process which is applicable for on-line robotic applications.In other words,it is math-ematically proved that in order to find the optimal point of a moving boundary it is sufficient to find the nearest point of the moving boundary to the instantaneous posi-tion of the end effector for each time interval instead of solving the heavy calculation of the Eq.(9).The nearest point of the moving boundary doesn’t remain a unique point during the whole period of end-effectors’movement since the optimal path of a non-linear system for a determined target point is not a straight line and changes during the time based on the objective func-tion which itself is a function of non-linear dynamics of the robot.Therefore,the instantaneous setpoint of the end-effector is not constant and should be calculated versus the time.Also,as it was proved,the nearest point should be found respect to this actual position of the end-effector since whole the objective function for the proposed regulation strategy is a function of error.As a result of minimizing the mentioned objective function,controlling input which is the motors’torque is mini-mized together with the state error which is related to the end-effector displacement or motor speed.Thus,the power which is the multiplication of these two parame-ters is minimized by the proposed optimal regulation1522M.H.Korayem et al. with moving boundaries which consequently results inoptimizing the consumed energy.Three different cases are considered for the movingboundary problem.First a straight line is assumed as anopen loop curve of moving boundary.In order to definethe nearest point of this line with respect to instanta-neous position of the end-effector,it is sufficient tofindthe orthogonal projection of the instantaneous positionof the end-effector on the moving boundary line(X f):X f=X1l+ 3j=1((−(X2l(j)−X1l(j))∗(X1l(j)−X e(j)) 3j=1(X2l(j)−X1l(j))2∗(X2l(j)−X1l(j));X={x;y;z;ϕ;θ;φ}(13) where X1l and X2l are the initial andfinal points of the straight moving boundary line,X e is the instantaneous position of the end-effector and X f is the instanta-neous optimal setpoint of the end-effector on the mov-ing boundary.Since the moving boundary is not a line but a segment of a line as an open loop curve,the cal-culated setpoint can be located within the segment or along it.This fact affects on the choosing the instanta-neous setpoint of the end-effector.To verify this prob-lem it should be considered that if the following condi-tion is satisfied,the setpoint is within the segment of the line and therefore,can be treated as the instantaneous setpoint of the end-effector:(X f−x1l)(X f−x2l)<0(14) Otherwise,if the above condition is not satisfied,the setpoint is out of the line segment or along the line. In this situation the nearest point to the instantaneous position of the end-effector which should be considered as the new setpoint is one of the two end-sides of the line segment.Therefore,it is sufficient to calculate these two distances and choose the shorter one as the new setpoint of the end-effector.Given the mentioned explanations,it is required to update the setpoint of the regulation process based on the moving boundary strategy to feedback the nearest optimal point of the moving boundary to instantaneous position of the end-effector as the desired setpoint.This new setpoint should be calculated using the instan-taneous position of the end-effector thus,this updat-ing needs the actual position of the end-effector which should be provided by the aid of sensors.Thus,this upgrading should be performed in each time interval of controlling procedure which depends on the sam-pling rate of the installed sensors.In the experimental study(and also simulation),the sample rate is consid-ered0.01second so,the upgrading rate of the setpoint is done each0.01second.(Encoders are2400pulse per revolution and camera is24frames per second). The strategy of defining the optimal setpoint is shown in Fig.4for eachcase.Fig.4Methodology offinding the instantaneous optimal setpoint for straight line(left)and curve(right)。