Patent Publication Number: US-8977394-B2

Title: Control method for mobile parallel manipulators

Description:
BACKGROUND OF THE INVENTION 
     1. Field of the Invention 
     The present invention relates to control mechanisms for robotic devices, and more particularly to a time- and energy-optimized control method for mobile parallel manipulators. 
     2. Description of the Related Art 
     U.S. Patent Application Publication Number 2012/0290131, titled “PARALLEL KINEMATIC MACHINE TRAJECTORY PLANNING METHOD” and published Nov. 15, 2012, which is hereby incorporated by reference in its entirety, discloses a parallel kinematic machine (PKM) trajectory planning method operable via a data-driven neuro-fuzzy multistage-based system. Offline planning based on robot kinematic and dynamic models, including actuators, was performed to generate a large dataset of trajectories covering most of the robot workspace and minimizing time and energy, while avoiding singularities and limits on joint angles, rates, accelerations and torques. The method implements an augmented Lagrangian solver on a decoupled form of the PKM dynamics in order to solve the resulting non-linear constrained optimal control problem. Using outcomes of the offline-planning, the data-driven neuro-fuzzy inference system was built to learn, to capture, and to optimize the desired dynamic behavior of the PKM. While the above method works for stationary machines, there remains the problem of planning a trajectory for a mobile parallel kinematic machine (MPM). 
     Thus, a control method for mobile parallel manipulators solving the aforementioned problems is desired. 
     SUMMARY OF THE INVENTION 
     In the present control method for mobile parallel manipulators, kinematic singularity and redundancy are solved through joint limits avoidance and manipulability criteria. By taking the MPM self-motion into consideration due to its redundancy, the inverse kinematic is derived using a hybrid neuro-fuzzy system, such as NeFIK. The discrete augmented Lagrangian (AL) technique is used to solve the highly nonlinear constrained multi-objective optimal control problem. An adaptive-neural fuzzy inference system (ANFIS)-based structure (based on the result of the AL solution) is used to solve the online trajectory planning of the MPM. 
     These and other features of the present invention will become readily apparent upon further review of the following specification and drawings. 
    
    
     
       BRIEF DESCRIPTION OF THE DRAWINGS 
         FIG. 1  is a schematic diagram illustrating a mobile parallel manipulator. 
         FIG. 2  is a schematic representation of locomotion capability of the MPM within designated frames of reference. 
         FIG. 3  is a block diagram of a controller that may be used in a control method for mobile parallel manipulators according to the present invention. 
         FIGS. 4A ,  4 B is a flowchart showing the AL algorithm function and operation used in a control method for mobile parallel manipulators according to the present invention. 
         FIG. 5  is a block diagram showing the AL solution for learning of an ANFIS module in a control method for mobile parallel manipulators according to the present invention. 
         FIG. 6  is a block diagram showing the FK solution for learning of a NeFIK module in a control method for mobile parallel manipulators according to the present invention. 
     
    
    
     Similar reference characters denote corresponding features consistently throughout the attached drawings. 
     DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS 
     At the outset, it will be understood that the diagrams in the Figures depicting the present control method for mobile parallel manipulators are exemplary only, and may be embodied in a dedicated electronic device having a microprocessor, microcontroller, digital signal processor, application specific integrated circuit, field programmable gate array, any combination of the aforementioned devices, or other device that combines the functionality of the control method for mobile parallel manipulators onto a single chip or multiple chips programmed to carry out the method steps described herein, or may be embodied in a general purpose computer having the appropriate peripherals attached thereto and software stored on non-transitory computer readable media, such as hard drives, programmable memory chips, floppy disks, USB drives, and the like, that can be loaded into main memory and executed by a processing unit to carry out the functionality and steps of the method described herein. 
     In the control method for mobile parallel manipulators, kinematic singularity and redundancy are solved through joint limits avoidance and manipulability criteria. As shown in  FIG. 1 , the mobile parallel manipulator (MPM)  100  is composed of a multiple degree of freedom parallel platform carried by an autonomous wheeled mobile robot. In the example shown, the manipulator is a 3-RRPaR parallel robot. A 3-RRPaR parallel manipulator has a moving platform disposed over a base, the moving platform being supported by three limbs of identical RRPaR kinematic structure in which the first R joint is actuated by a rotary actuator, where the notation of R and Pa stands for the revolute joint and parallelogram, respectively. By taking the MPM self-motion into consideration due to its redundancy, the inverse kinematic is derived using a hybrid neuro-fuzzy system, such as NeFIK. The discrete augmented Lagrangian (AL) technique is used to solve the highly nonlinear constrained multi-objective optimal control problem. An adaptive neuro-fuzzy inference system (ANFIS)-based structure (based on the result of the AL solution) is used to solve the online trajectory planning of the MPM. 
     The present method generates full set of torques that derive motion of the MPM  100  from start point to end point while achieving optimal time, energy, or both as required. The present control method for mobile parallel manipulators uses neuro-fuzzy structure to solve the inverse kinematic problem. The MPM  100  is composed of a multiple degree of freedom parallel platform carried by an autonomous wheeled mobile robot. The multiple degree of freedom parallel platform is a modified version of a DELTA parallel robot. The autonomous wheeled platform is a three-wheeled nonholonomic species. As shown in  FIG. 2 , the MPM  100  is assumed to just move on a plane. A fixed Cartesian frame (global frame) O{X O , Y O , Z O } is assigned on the plane of motion, a moving frame M{X M , Y M , Z M } on the mobile platform, a moving frame B{X B , Y B , Z B } at the centered point B of the base platform ΔA 1 A 2 A 3 , and another moving Cartesian frame P{X P , Y P , Z P } on the triangular moving platform ΔC 1 C 2 C 3  at the centered point P. 
     For frame M, the Y M  axis is along the coaxial line of the two fixed wheels, X M  is perpendicular to Y M  and passes through the midpoint of the line segment connecting the two fixed wheel centers, and the Z M  axis is vertical to the mobile platform. In addition, the X B  and X P  axes are parallel to the X M  axis, and the Y B  and Y P  axes are parallel to the Y M  axis, respectively. 
     In order to get a compact structure, such as in a parallel manipulator, both the base and moving platforms are designed to be isosceles right triangles described by parameters of e and u, respectively, i.e., BA i =e and PC i =u, for i=1, 2, and 3. Also, the actuated variable of the i th  limb is denoted by angle θ i . The connecting joints between the upper and lower links are denoted as B i , and the lengths of upper and lower links for each limb are a and b respectively. The plane of motion can be described as follows. The kinematics of the mobile platform consists of three parameters of coordinates of point M (x m , y m ) and the heading angle (φ m ). Referring to  FIG. 1 , let d be the distance between the two fixed wheels, let l b  be the offset of the base platform of the parallel robot with respect to the origin of frame M, and let l a  denote the offset of the mass center A of the mobile platform with respect to frame M. Additionally, P i (x m   i , y m   i , φ m   i ) and P i+1 (x m   i+1 , y m   i+1 , φ m   i+1 ) represent coordinates of the mobile platform at time t and t i+1  respectively. β i  and r i  are the corresponding yaw angle and steering radius at time t i . ΔS l , ΔS r , and ΔS m  denote the advance of the left wheel, the right wheel, and the origin of the frame M in the time interval (Δt=t i+1 −t i ), respectively. 
     Utilizing the plane of motion description illustrated in  FIG. 2 , it can be shown that 
                   {               x   .     m     =         r   ⁢           ⁢   c   ⁢           ⁢     ϕ   m       2     ⁢     (         θ   .     L     +       θ   .     r       )                       y   .     m     =         r   ⁢           ⁢   s   ⁢           ⁢     ϕ   m       2     ⁢     (         θ   .     L     +       θ   .     r       )                       ϕ   .     m     =       r   d     ⁢     (         θ   .     r     -       θ   .     L       )                       (   1   )               
Using abbreviated notation, c stands for cosine, s stands for sine, and r is the radius of each driving wheel. Also, θ L , and θ r  denote the rotating angles of the left and right driving wheels, respectively.
 
     Let the general coordinates of the mobile platform be designated by ξ=[x m  y m  φ m θ L  θ r ] T . Solve equation (1) for the nonholonomic constraints of the MPR, which can be written as: 
     
       
         
           
             
               
                 
                   
                     
                       
                         D 
                         ⁡ 
                         
                           ( 
                           ξ 
                           ) 
                         
                       
                       · 
                       
                         ξ 
                         . 
                       
                     
                     = 
                     0 
                   
                   , 
                   where 
                 
               
               
                 
                   ( 
                   2 
                   ) 
                 
               
             
             
               
                 
                   
                     D 
                     ⁡ 
                     
                       ( 
                       ξ 
                       ) 
                     
                   
                   = 
                   
                     [ 
                     
                       
                         
                           
                             c 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               ϕ 
                               m 
                             
                           
                         
                         
                           
                             s 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               ϕ 
                               m 
                             
                           
                         
                         
                           
                             
                               - 
                               d 
                             
                             / 
                             2 
                           
                         
                         
                           
                             - 
                             r 
                           
                         
                         
                           0 
                         
                       
                       
                         
                           
                             c 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               ϕ 
                               m 
                             
                           
                         
                         
                           
                             s 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               ϕ 
                               m 
                             
                           
                         
                         
                           
                             d 
                             / 
                             2 
                           
                         
                         
                           0 
                         
                         
                           
                             - 
                             r 
                           
                         
                       
                       
                         
                           
                             s 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               ϕ 
                               m 
                             
                           
                         
                         
                           
                             
                               - 
                               c 
                             
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               ϕ 
                               m 
                             
                           
                         
                         
                           0 
                         
                         
                           0 
                         
                         
                           0 
                         
                       
                     
                     ] 
                   
                 
               
               
                 
                   ( 
                   3 
                   ) 
                 
               
             
           
         
       
     
     The forward kinematics problem is very complex for a parallel robot, while the inverse kinematics problem is extremely straightforward, in general. The forward kinematics solution is generated for the designed MPM. 
     Assuming that linear (ν m ) and angular (ω m ) velocities of the mobile platform and assuming the actuated inputs of the actuators (θ i , i=1, 2, 3), the position (x, y, z) and orientation (φ) of the mobile platform are solved using the forward kinematics. 
     In this research we assume that there is no slip in the wheels of mobile platform on all directions. As Δt→0, the velocities during this time interval can be considered as a constant:
 
| P   i   P   i+1   |≈ΔS   m =ν m   ·Δt,  
 
Δ x   m   i   =x   m   i+1   −x   m   i   ≈|P   i   P   i+1   |·cφ   m   i =ν m   ·Δt·cφ   m   i ,
 
Δ y   m   i   =y   m   i+1   −y   m   i   ≈|P   i   P   i+1   |·sφ   m   i =ν m   ·Δt·sφ   m   i ,
 
and
 
β≈Δφ m   i =φ i+1 −φ i =ω m   ·Δt.   (4)
 
     Since eq. (4) is applied in all the motions of the mobile platform, we can delete the superscript i. Thus: 
     
       
         
           
             
               
                 
                   
                     
                       
                         x 
                         . 
                       
                       m 
                     
                     = 
                     
                       
                         
                           lim 
                           
                             
                               Δ 
                               ⁢ 
                               
                                   
                               
                               ⁢ 
                               t 
                             
                             -&gt; 
                             0 
                           
                         
                         ⁢ 
                         
                           ( 
                           
                             
                               Δ 
                               ⁢ 
                               
                                   
                               
                               ⁢ 
                               
                                 x 
                                 m 
                               
                             
                             
                               Δ 
                               ⁢ 
                               
                                   
                               
                               ⁢ 
                               t 
                             
                           
                           ) 
                         
                       
                       = 
                       
                         
                           
                             v 
                             m 
                           
                           · 
                           c 
                         
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           ϕ 
                           m 
                         
                       
                     
                   
                   , 
                   
                     
 
                   
                   ⁢ 
                   
                     
                       
                         y 
                         . 
                       
                       m 
                     
                     = 
                     
                       
                         
                           lim 
                           
                             
                               Δ 
                               ⁢ 
                               
                                   
                               
                               ⁢ 
                               t 
                             
                             -&gt; 
                             0 
                           
                         
                         ⁢ 
                         
                           ( 
                           
                             
                               Δ 
                               ⁢ 
                               
                                   
                               
                               ⁢ 
                               
                                 y 
                                 m 
                               
                             
                             
                               Δ 
                               ⁢ 
                               
                                   
                               
                               ⁢ 
                               t 
                             
                           
                           ) 
                         
                       
                       = 
                       
                         
                           
                             v 
                             m 
                           
                           · 
                           s 
                         
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           ϕ 
                           m 
                         
                       
                     
                   
                   , 
                   
                     
 
                   
                   ⁢ 
                   
                     
                       
                         ϕ 
                         . 
                       
                       m 
                     
                     = 
                     
                       
                         
                           lim 
                           
                             
                               Δ 
                               ⁢ 
                               
                                   
                               
                               ⁢ 
                               t 
                             
                             -&gt; 
                             0 
                           
                         
                         ⁢ 
                         
                           ( 
                           
                             
                               Δ 
                               ⁢ 
                               
                                   
                               
                               ⁢ 
                               
                                 ϕ 
                                 m 
                               
                             
                             
                               Δ 
                               ⁢ 
                               
                                   
                               
                               ⁢ 
                               t 
                             
                           
                           ) 
                         
                       
                       = 
                       
                         
                           ω 
                           m 
                         
                         . 
                       
                     
                   
                 
               
               
                 
                   ( 
                   5 
                   ) 
                 
               
             
           
         
       
     
     Integration of eq. (5), gives the posture of the mobile platform: 
                         x   m     ⁡     (   t   )       =         ∫   0   t     ⁢         x   .     m     ·     ⅆ   t         =       ∫   0   t     ⁢         v   m     ⁡     (   t   )       ·     c   ⁡     (       ϕ   m     ⁡     (   t   )       )       ·     ⅆ   t             ,     
     ⁢         y   m     ⁡     (   t   )       =         ∫   0   t     ⁢         y   .     m     ·     ⅆ   t         =       ∫   0   t     ⁢         v   m     ⁡     (   t   )       ·     s   ⁡     (       ϕ   m     ⁡     (   t   )       )       ·     ⅆ   t             ,     
     ⁢         ϕ   m     ⁡     (   t   )       =         ∫   0   t     ⁢         ϕ   .     m     ·     ⅆ   t         =       ∫   0   t     ⁢         ω   m     ⁡     (   t   )       ·       ⅆ   t     .                     (   6   )               
Let us assume that the wheels of mobile platform have no slip in any direction. Let P=[x y z] T  and  B P=[x p  y p  z p ] T  be the vectors of point P in the fixed frame O and the moving frame B, respectively. Also, in frame B, let  B e i ={right arrow over (BA l )},  B b i ={right arrow over (A l B l  )} and  B c i ={right arrow over (PC l )}. Referring to  FIG. 2 , we obtain:
 
 B   e   i   =[ecφ   i   esφ   i 0] T , B   c   i   =[ucφ   i   usφ   i 0] T ,
 
 B   b   i   =[bcθ   i   cφ   i   bcθ   i   sφ   i   bsθ   i ] T   (7)
 
where φ i =(iπ)/2, for i=1, 2, and 3. Since the distance between B i  and C i  is a constant a, we have:
 
∥ B   P+   B   c   i − B   e   i − B   b   i   ∥=a   (8)
 
By substituting eq. (7) in eq. (8), we get,
 
 x   p   2 +( y   p   +u−e−bcθ   1 ) 2 +( z   p   −bsθ   1 ) 2   =a   2 ,  (9)
 
( x   p   −u+e+bcθ   2 ) 2   +y   p   2 +( z   p   −bsθ   2 ) 2   =a   2 ,  (10)
 
 x   p   2 +( y   p   +u−e−bcθ   3 ) 2 +( z   p   −bsθ   3 ) 2   =a   2 ,  (11)
 
and solving equations (9) and (10) to get x p  and y p  as a function of z p , we get the following result:
 
     
       
         
           
             
               x 
               p 
             
             = 
             
               
                 - 
                 
                   ( 
                   
                     b 
                     ⁡ 
                     
                       ( 
                       
                         
                           2 
                           ⁢ 
                           bc 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             θ 
                             1 
                           
                           ⁢ 
                           c 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             θ 
                             3 
                           
                           ⁢ 
                           w 
                         
                         - 
                         
                           bc 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             θ 
                             2 
                           
                           ⁢ 
                           wc 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             θ 
                             1 
                           
                         
                         - 
                         
                           bc 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             θ 
                             2 
                           
                           ⁢ 
                           wc 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             θ 
                             3 
                           
                         
                         - 
                         
                           
                             w 
                             2 
                           
                           ⁢ 
                           c 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             θ 
                             1 
                           
                         
                         - 
                         
                           
                             w 
                             2 
                           
                           ⁢ 
                           c 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             θ 
                             3 
                           
                         
                         + 
                         
                           2 
                           ⁢ 
                           
                             w 
                             2 
                           
                           ⁢ 
                           c 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             θ 
                             2 
                           
                         
                         + 
                         
                           2 
                           ⁢ 
                           
                             z 
                             p 
                           
                           ⁢ 
                           s 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             θ 
                             2 
                           
                           ⁢ 
                           w 
                         
                         + 
                         
                           
                             z 
                             p 
                           
                           ⁢ 
                           bs 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             θ 
                             1 
                           
                           ⁢ 
                           c 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             θ 
                             3 
                           
                         
                         - 
                         
                           w 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             z 
                             p 
                           
                           ⁢ 
                           s 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             θ 
                             1 
                           
                         
                         - 
                         
                           
                             wz 
                             p 
                           
                           ⁢ 
                           s 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             θ 
                             3 
                           
                         
                         + 
                         
                           
                             bz 
                             p 
                           
                           ⁢ 
                           c 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             θ 
                             1 
                           
                           ⁢ 
                           s 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             θ 
                             3 
                           
                         
                         - 
                         
                           
                             bz 
                             p 
                           
                           ⁢ 
                           c 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             θ 
                             1 
                           
                           ⁢ 
                           s 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             θ 
                             2 
                           
                         
                         - 
                         
                           
                             bz 
                             p 
                           
                           ⁢ 
                           c 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             θ 
                             3 
                           
                           ⁢ 
                           s 
                           ⁢ 
                           
                               
                           
                           ⁢ 
                           
                             θ 
                             2 
                           
                         
                       
                       ) 
                     
                   
                   ) 
                 
               
               / 
               
                 ( 
                 
                   
                     
                       - 
                       bwc 
                     
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       θ 
                       1 
                     
                   
                   - 
                   
                     bwc 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       θ 
                       3 
                     
                   
                   - 
                   
                     2 
                     ⁢ 
                     bwc 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       θ 
                       2 
                     
                   
                   + 
                   
                     
                       b 
                       2 
                     
                     ⁢ 
                     c 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       θ 
                       1 
                     
                     ⁢ 
                     c 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       θ 
                       2 
                     
                   
                   + 
                   
                     
                       b 
                       2 
                     
                     ⁢ 
                     c 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       θ 
                       3 
                     
                     ⁢ 
                     c 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       θ 
                       2 
                     
                   
                   + 
                   
                     2 
                     ⁢ 
                     
                       w 
                       2 
                     
                   
                 
                 ) 
               
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 
                   y 
                   p 
                 
                 = 
                 
                   - 
                   
                     
                       b 
                       ⁡ 
                       
                         ( 
                         
                           
                             wc 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               θ 
                               1 
                             
                           
                           - 
                           
                             wc 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               θ 
                               3 
                             
                           
                           + 
                           
                             
                               z 
                               p 
                             
                             ⁢ 
                             s 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               θ 
                               1 
                             
                           
                           - 
                           
                             
                               z 
                               p 
                             
                             ⁢ 
                             s 
                             ⁢ 
                             
                                 
                             
                             ⁢ 
                             
                               θ 
                               3 
                             
                           
                         
                         ) 
                       
                     
                     
                       
                         
                           - 
                           2 
                         
                         ⁢ 
                         w 
                       
                       + 
                       
                         bc 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           θ 
                           1 
                         
                       
                       + 
                       
                         bc 
                         ⁢ 
                         
                             
                         
                         ⁢ 
                         
                           θ 
                           3 
                         
                       
                     
                   
                 
               
               , 
               
                 
 
               
               ⁢ 
               
                   
               
               ⁢ 
               
                 
                   where 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   w 
                 
                 = 
                 
                   u 
                   - 
                   
                     e 
                     . 
                   
                 
               
             
           
         
       
     
     Substituting x p  and y p  in (11) and solving for z p , and then solving eq. (10) and eq. (11), leads to solutions for the forward kinematics of the parallel robot, i.e., 
                         x   p     =         f   1     ⁢     z   p       +     f   0         ,       y   p     =         e   1     ⁢     z   p       +     e   0         ,       z   p     =         -     h   1       +         h   1   2     -     4   ⁢     h   2     ⁢     h   0               2   ⁢           ⁢     h   2             ⁢     
     ⁢   where   ⁢     
     ⁢         e   1     =     -       n   1       n   2           ,       e   0     =     -       n   0       n   2           ,     
     ⁢       f   1     =           m   2     ⁢     n   1           m   3     ⁢     n   2         -       m   1       m   3           ,       f   0     =           m   2     ⁢     n   0           m   3     ⁢     n   2         -       m   0       m   3           ,     
     ⁢       h   2     =     1   +     e   1   2     +     f   1   2         ,     
     ⁢       h   1     =     2   ⁡     [         e   1     ⁡     (       e   0     +   u   -   e   -     bc   ⁢           ⁢     θ   1         )       +       f   1     ⁢     f   0       -     bs   ⁢           ⁢     θ   1         ]         ,     
     ⁢       h   0     =         (       e   0     +   u   -   e   -     bc   ⁢           ⁢     θ   1         )     2     +     f   0   2     -     a   2     +       b   2     ⁢     s   2     ⁢     θ   1           ,     
     ⁢       m   3     =     u   -   e   -     bc   ⁢           ⁢     θ   2           ,       m   2     =     u   -   e   -     bc   ⁢           ⁢     θ   1           ,     
     ⁢       m   1     =     b   ⁡     (       s   ⁢           ⁢     θ   1       -     s   ⁢           ⁢     θ   1         )         ,     
     ⁢       m   0     =       b   ⁡     (     u   -   e     )       ⁢     (       c   ⁢           ⁢     θ   2       -     c   ⁢           ⁢     θ   1         )         ,     
     ⁢       n   2     =       2   ⁢     (     u   -   e     )       -     b   ⁡     (       c   ⁢           ⁢     θ   1       +     c   ⁢           ⁢     θ   3         )           ,     
     ⁢       n   1     =     b   ⁡     (       s   ⁢           ⁢     θ   1       -     s   ⁢           ⁢     θ   3         )         ,   and     ⁢     
     ⁢       n   0     =       b   ⁡     (     u   -   e     )       ⁢       (       c   ⁢           ⁢     θ   3       -     c   ⁢           ⁢     θ   1         )     .                 (   12   )               
The parallel robot has only a translation motion, so the rotation around the Z O  axis is the only factor to determine the orientation of the MPM, i.e., φ=φ m . Referring to  FIGS. 1 and 2 , one can derive the position of the mobile platform to be:
 
                     p   =     b   +       R   B     ⁢   p         ⁢     
     ⁢   with           (   13   )                 b   =       OB   ⟶     =       [       x   m     -       l   b     ⁢   c   ⁢           ⁢     ϕ   m     ⁢           ⁢     y   m       -       l   b     ⁢   s   ⁢           ⁢     ϕ   m     ⁢           ⁢   h       ]     T         ,     
     ⁢   and           (   14   )                 R   =     [           c   ⁢           ⁢     ϕ   m               -   s     ⁢           ⁢     ϕ   m           0             s   ⁢           ⁢     ϕ   m             c   ⁢           ⁢     ϕ   m           0           0       0       1         ]       ,           (   15   )               
is the rotation matrix of the moving frame B regarding to the fixed frame O.
 
     Regarding the differential kinematics analysis, let the vector for the output velocities of the moving platform be {dot over (x)}=[{dot over (x)} {dot over (y)}ż {dot over (φ)}] T , and the vector of input joint rates be represented by {dot over (q)}=[{dot over (θ)} 1  {dot over (θ)} r {dot over (θ)} 1  {dot over (θ)} 2  {dot over (θ)} 3 ] T . Differentiating eq. (13) with respect to time, leads to:
 
 {dot over (p)}={dot over (b)}+{dot over (R)}   B   p+R   B   {dot over (p)}   (16)
 
     Also, let {dot over (q)} p =[{dot over (θ)} 1  {dot over (θ)} 2  {dot over (θ)} 3 ] T  to be the vector of actuated joint rates for the parallel robot. Taking the derivative of both sides of equations (9) through (11) with respect to time and rewriting them into a matrix form yields:
 
 A   B   {dot over (p)}=B{dot over (q)}   p   (17)
 
The 3×3 forward and inverse Jacobian matrices A and B of the parallel robot can be written as:
 
                       A   =     [           a   11           a   12           a   13               a   21           a   22           a   23               a   31           a   32           a   33           ]       ,     B   =     [           b   11         0       0           0         b   22         0           0       0         b   33           ]       ,     
     ⁢   with     ⁢     
     ⁢         a   11     =     x   p       ,       a   12     =       y   p     +   u   -   e   -     bc   ⁢           ⁢     θ   1           ,       a   13     =       z   p     -     bs   ⁢           ⁢     θ   1           ,     
     ⁢       a   21     =       x   p     -   u   +   e   +     bc   ⁢           ⁢     θ   2           ,       a   22     =     y   p       ,       a   23     =       z   p     -     bs   ⁢           ⁢     θ   2           ,     
     ⁢       a   31     =     x   p       ,       a   32     =       y   p     -   u   +   e   +     bc   ⁢           ⁢     θ   3           ,       a   33     =       z   p     -     bs   ⁢           ⁢     θ   3           ,     
     ⁢       b   11     =     -     b   ⁡     [         (       y   p     +   u   -   e     )     ⁢   s   ⁢           ⁢     θ   1       -       z   p     ⁢   c   ⁢           ⁢     θ   1         ]           ,     
     ⁢       b   22     =     b   ⁡     [         (       x   p     -   u   +   e     )     ⁢   s   ⁢           ⁢     θ   2       -       z   p     ⁢   c   ⁢           ⁢     θ   2         ]         ,     
     ⁢       b   33     =     b   ⁡     [         (       y   p     -   u   +   e     )     ⁢   s   ⁢           ⁢     θ   3       -       z   p     ⁢   c   ⁢           ⁢     θ   3         ]                   (   18   )               
It can be derived from eq. (17) that when the parallel robot is away from the singularity:
 
 B   {dot over (p)}=J   p   {dot over (q)}   p ,  (19)
 
where J p =A −1 B is the Jacobian matrix of a 3-RRPaR parallel robot. By substituting equations (14), (15) and (19) into eq. (16), and considering eq. (1), this will give:
 
 {dot over (x)}=J{dot over (q)}   (20)
 
Defining J 4×5 to be the Jacobian matrix of the MPM, relating the output velocities to the actuated joint rates allows rewriting as shown in the following matrix:
 
                       J   =     [           J   11           J   12                                                   J   21           J   22                       R   ⁢           ⁢     A     -   1       ⁢   B                         0       0                                                   -   r     /   d           r   /   d         0       0       0         ]       ,     
     ⁢   with     ⁢     
     ⁢         J   11     =         (       1   2     +       y   p     d       )     ⁢   r   ⁢           ⁢   c   ⁢           ⁢     ϕ   m       -       r   d     ⁢   s   ⁢           ⁢       ϕ   m     ⁡     (       l   b     -     x   b       )             ,     
     ⁢       J   12     =         (       1   2     -       y   p     d       )     ⁢   r   ⁢           ⁢   c   ⁢           ⁢     ϕ   m       +       r   d     ⁢   s   ⁢           ⁢       ϕ   m     ⁡     (       l   b     -     x   b       )             ,     
     ⁢       J   21     =         (       1   2     +       y   p     d       )     ⁢   r   ⁢           ⁢   s   ⁢           ⁢     ϕ   m       +       r   d     ⁢   c   ⁢           ⁢       ϕ   m     ⁡     (       l   b     -     x   b       )             ,     
     ⁢       J   22     =         (       1   2     -       y   p     d       )     ⁢   r   ⁢           ⁢   s   ⁢           ⁢     ϕ   m       +       r   d     ⁢   c   ⁢           ⁢         ϕ   m     ⁡     (       l   b     -     x   b       )       .                     (   21   )               
Taking differentiation equation (19) with respect to time gives:
 
 {umlaut over (x)}={dot over (J)}{dot over (q)}+J{umlaut over (q)}   (22)
 
Solving equation (20) leads to:
 
 {dot over (q)}=J†{dot over (x)}+ ( I   5×5   −J†J ) {dot over (q)}   s ,  (23)
 
where J†=J T (JJ T ) −1  the generalized pseudo inverse of J, and {dot over (q)} s ε   5×1  is an arbitrary vector, which can be chosen to achieve a secondary task.
 
     With respect to kinematic singularity characterization, the robot Jacobian allows motion and force transformation from the actuators to the End Effector, so that the forces demand at a given point on the trajectory need to be continuously checked for possible violation of the preset limits as the robot moves close to singularity. The condition number of the Jacobian is used as a local performance index for evaluating the velocity, accuracy, and rigidity mapping characteristics between the joint variables and the moving platform. With respect to a detailed characterization of robot singularities, from equation (16) it is clear that singularity in the MPM structure occurs in the following cases:
         1 st  case: |A|=0, and |B|≠0. This corresponds to a type-1 singularity.   2 nd  case: |A|≠0, and |B|=0. This is a type-2 singularity.   3 rd  case: |A|=0, and |B|=0. This is a type-3 singularity for which both the determinant of A and B will equal to zero.
 
These cases are programmed to be calculated during running the simulation.
       

     Regarding redundancy resolution through joint limits and singularity avoidance, to include a secondary task criterion by a performance index g(q), {dot over (q)} 2  in equation (22) is chosen to be {dot over (q)} s =±k∇g(q), where k is a positive real number and ∇g(q) the gradient of g(q), where a positive sign indicates that the criterion is to be maximized and a negative sign indicates minimization. To avoid joint limits we chose {dot over (q)} s  as follows:
 
 {dot over (q)}   s1 =( q   max   −q ) W ( q   min   −q )  (24)
 
where
 
                   W   =       1   2     ⁢     (       q   max     -     q   min       )               (   25   )               
The related criterion to avoid the singularity is to maximize the manipulability, i.e., we choose {dot over (q)} s , as follows:
 
 {dot over (q)}   s2 =√{square root over (det( JJ ′))} W   s   (26)
 
where W s  is weight vector with appropriate dimension. The formula of the augmented function to avoid singularity and joint limits is as follows:
 
 {dot over (q)}   s   ={dot over (q)}   s1   +{dot over (q)}   s2 .  (27)
 
     Kinematic initialization first involves optimal time trajectory parameterization. Considering the MPM, the task of the robot is to move its end-effector within a limited workspace and time interval. Also, each robot joint has to produce zero rates and accelerations at the ends of the interval of motion. A cycloidal function is chosen to achieve this purpose for modeling the trajectory time (t) from 0 to T, with the normalized time s as 
               s   =     t   T       ,         
0≦t≦T, 0≦s≦1.
 
     The cycloidal function is characterized by the relation: 
                       q   ⁡     (   s   )       =     s   -       1     2   ⁢   π       ⁢     sin   ⁡     (     2   ⁢   π   ⁢           ⁢   s     )             ,           (   28   )               
where the first and second derivatives obtained are:
 
 {dot over (q)} ( s )=1−cos(2 πs ),  (29)
 
 {umlaut over (q)} ( s )=2π sin( s )  (30)
 
The cycloidal motion and its derivatives are defined within the range (−1, 1). With zero velocity and acceleration at the ends of the interval, i.e., s=0 and s=1, the initial and final joint values be detailed as q I  and q F . The maximum velocity for the motion of joint j is attained at the center of the interval, i.e. s=0.5, the maximum being {dot over (q)} max ={dot over (q)}(0.5)=2, so that:
 
                         (       q   .     j     )     max     ⁢     (   s   )       =       2   T     ⁢              q   j   F     -     q   j   I            .               (   31   )               
In the same way, it can be shown that the acceleration of joint j allows its maximum and minimum values at s=0.25 and s=0.75, the maximum being {umlaut over (q)} max ={umlaut over (q)}(0.25)={umlaut over (q)}(0.75)=2π, and hence:
 
                         (       q   ¨     j     )     max     ⁢     (   s   )       =         2   ⁢   π       T   2       ⁢              q   j   F     -     q   j   I            .               (   32   )               
And finally, the maximum jerk of joint j is achieved at s=0.0, and s=1.0, the maximum being
 
                       q   …     max     =       ⁢       q   …     ⁡     (   0   )                   =       ⁢       q   …     ⁡     (   1   )                   =       ⁢     4   ⁢       π   2     .                   
Thus:
 
                         (       q   ⃛     j     )     max     ⁢     (   s   )       =         4   ⁢     π   2         T   3       ⁢            q   j   F     -     q   j   I                      (   33   )               
The motion of the robot is constrained by the maximum joint velocity, accelerations and jerk that the motors produce. This can be interpreted as:
 
                           q   .     j     ≤       (       q   .     j     )     motor       ,     
     ⁢         q   ¨     j     ≤       (       q   ¨     j     )     motor       ,     
     ⁢   and     ⁢     
     ⁢           q   ...     j     ≤       (       q   …     j     )     motor       ,   .             (   34   )               
This means that the strongest constraint among ({dot over (q)} j ) motor , ({umlaut over (q)} j ) motor , and ( ) motor  limits the minimum-time trajectory of joint j, which means that:
 
                     T   j     =     Max   ⁢     {         2   ⁢            q   j   F     -     q   j   I                  (       q   .     j     )     motor       ,           2   ⁢   π         (       q   ¨     j     )     motor       ⁢            q   j   F     -     q   j   I                ,       (         4   ⁢     π   2           (       q   ⃛     j     )     motor       ⁢            q   j   F     -     q   j   I              )       1   /   3         }               (   35   )               
The overall minimum-time trajectory (for all the five joints together of the MPM) is written as:
 
 T   Min =Max{ T   1   ,T   2   ,T   3   ,T   4   ,T   5 }  (36)
 
Thus, the resulting minimum-time trajectory, which is characterized by joints position, velocity, and acceleration, is obtained as:
 
     
       
         
           
             
               
                 
                   
                     
                       
                         q 
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       = 
                       
                         
                           q 
                           I 
                         
                         + 
                         
                           
                             ( 
                             
                               
                                 q 
                                 F 
                               
                               - 
                               
                                 q 
                                 I 
                               
                             
                             ) 
                           
                           ⁢ 
                           
                             ( 
                             
                               
                                 t 
                                 
                                   T 
                                   Min 
                                 
                               
                               - 
                               
                                 
                                   1 
                                   
                                     2 
                                     ⁢ 
                                     π 
                                   
                                 
                                 ⁢ 
                                 
                                   sin 
                                   ⁡ 
                                   
                                     ( 
                                     
                                       2 
                                       ⁢ 
                                       π 
                                       ⁢ 
                                       
                                         t 
                                         
                                           T 
                                           Min 
                                         
                                       
                                     
                                     ) 
                                   
                                 
                               
                             
                             ) 
                           
                         
                       
                     
                     , 
                     
                       
 
                     
                     ⁢ 
                     
                       
                         
                           q 
                           . 
                         
                         ⁡ 
                         
                           ( 
                           t 
                           ) 
                         
                       
                       = 
                       
                         
                           
                             
                               q 
                               F 
                             
                             - 
                             
                               q 
                               I 
                             
                           
                           
                             T 
                             Min 
                           
                         
                         ⁢ 
                         
                           ( 
                           
                             1 
                             - 
                             
                               cos 
                               ⁡ 
                               
                                 ( 
                                 
                                   2 
                                   ⁢ 
                                   π 
                                   ⁢ 
                                   
                                       
                                   
                                   ⁢ 
                                   
                                     t 
                                     
                                       T 
                                       Min 
                                     
                                   
                                 
                                 ) 
                               
                             
                           
                           ) 
                         
                       
                     
                   
                   ⁢ 
                   
                     
 
                   
                   ⁢ 
                   
                     
                       
                         q 
                         ¨ 
                       
                       ⁡ 
                       
                         ( 
                         t 
                         ) 
                       
                     
                     = 
                     
                       
                         
                           
                             q 
                             F 
                           
                           - 
                           
                             q 
                             I 
                           
                         
                         
                           T 
                           Min 
                           2 
                         
                       
                       ⁢ 
                       2 
                       ⁢ 
                       π 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         
                           sin 
                           ⁡ 
                           
                             ( 
                             
                               2 
                               ⁢ 
                               π 
                               ⁢ 
                               
                                   
                               
                               ⁢ 
                               
                                 t 
                                 
                                   T 
                                   Min 
                                 
                               
                             
                             ) 
                           
                         
                         . 
                       
                     
                   
                 
               
               
                 
                   ( 
                   37 
                   ) 
                 
               
             
           
         
       
     
     Neuro-Fuzzy Inverse Kinematics (NeFIK) can assist in solving the inverse kinematics problem, which is the problem of finding the joint coordinates (q 1 , q 2 , q 3 , q 4 , q 5 ), from Cartesian coordinates (x, y, z, φ) where the starting and ending Cartesian positions of the manipulator are specified in the workspace of the robot. NeFIK is proposed to be used in the present method for resolving the redundancy of the inverse kinematic problem. The NeFIK is trained to produce joint position in a preferred configuration. The training dataset is generated with the aforementioned forward kinematic (FK) equations of the manipulator. The use of the FK to the learning of the NeFIK module is shown as system circuit  600  in  FIG. 6 . A set of derivatives of (q 1 , q 2 , q 3 , q 4 , q 5 ) is used to construct the true derivatives (x, y, z, φ, x n ), and thus to get an error on which to apply the back-propagation algorithm. {dot over (x)} n  related to {dot over (q)} s  is added to remove the redundancy of the system. 
     NeFIK is a multi-layer feed-forward adaptive network. The first layer is a two input layer, characterizing the Cartesian position crisp values. The last layer is a three output-layer characterizing the corresponding crisp joint values. NeFIK involves three hidden layers. The first one is the fuzzification layer, which transfers the crisp inputs to linguistic variables through sigmoidal transfer functions. The second is the rule layer, which applies the Product t-norm to produce the firing strengths of each rule. This is followed by a normalization layer. The training rule option is the Levenberg-Marquard version of the gradient back propagation algorithm. This choice allows speeding up the learning process substantially, with less iteration as compared to standard back-propagation (e.g., gradient descent). To construct NeFIK, the forward kinematic equations are applied. Its learning is obtained through 400 samples, among which 320 are considered for training, whereas the testing and validation datasets are each obtained using 10 entry samples. Training performance for NeFIK has been shown to achieve a very small root mean square error (RMSE), less than 10 −3  in less than 10 epochs. It should be understood that the configuration used for the learning is determined among infinitely many solutions that exist for each input. The model derived by the NeFIK structure is used to process trajectories motion of the MPR where the motion is subjected to the parallel mechanism only, and where the motion is a combined motion of both the mobile and parallel structures. 
     The robot dynamic model is developed using a Lagrangian formalism, which includes actuators and friction models. This model allows closed-form expression of joint rates and accelerations characterizing the motion resulting from joint torques. Using the minimum-time trajectory of equation (36) and an inverse dynamic solution, one can write: 
     
       
         
           
             
               
                 
                   τ 
                   = 
                   
                     
                       [ 
                       
                         
                           
                             1 
                             
                               T 
                               Min 
                               2 
                             
                           
                           ⁢ 
                           
                             ( 
                             
                               2 
                               ⁢ 
                               
                                 πsin 
                                 ⁡ 
                                 
                                   ( 
                                   
                                     2 
                                     ⁢ 
                                     π 
                                     ⁢ 
                                     
                                       t 
                                       
                                         T 
                                         Min 
                                       
                                     
                                   
                                   ) 
                                 
                               
                             
                             ) 
                           
                           ⁢ 
                           
                             D 
                             ⁡ 
                             
                               ( 
                               q 
                               ) 
                             
                           
                         
                         + 
                         
                           
                             1 
                             
                               T 
                               Min 
                             
                           
                           ⁡ 
                           
                             [ 
                             
                               
                                 
                                   ( 
                                   
                                     1 
                                     - 
                                     
                                       cos 
                                       ⁡ 
                                       
                                         ( 
                                         
                                           2 
                                           ⁢ 
                                           π 
                                           ⁢ 
                                           
                                             t 
                                             
                                               T 
                                               Min 
                                             
                                           
                                         
                                         ) 
                                       
                                     
                                   
                                   ) 
                                 
                                 ⁢ 
                                 
                                   ( 
                                   
                                     
                                       C 
                                       ⁡ 
                                       
                                         ( 
                                         
                                           q 
                                           , 
                                           
                                             q 
                                             . 
                                           
                                         
                                         ) 
                                       
                                     
                                     + 
                                     
                                       F 
                                       V 
                                     
                                   
                                   ) 
                                 
                               
                               + 
                               
                                 
                                   F 
                                   c 
                                 
                                 ⁢ 
                                 
                                   sgn 
                                   ⁡ 
                                   
                                     ( 
                                     
                                       1 
                                       - 
                                       
                                         cos 
                                         ⁡ 
                                         
                                           ( 
                                           
                                             2 
                                             ⁢ 
                                             π 
                                             ⁢ 
                                             
                                               t 
                                               
                                                 T 
                                                 Min 
                                               
                                             
                                           
                                           ) 
                                         
                                       
                                     
                                     ) 
                                   
                                 
                               
                             
                             ] 
                           
                         
                       
                       ] 
                     
                     ⁢ 
                     
                       
                         ( 
                         
                           
                             q 
                             F 
                           
                           - 
                           
                             q 
                             I 
                           
                         
                         ) 
                       
                       T 
                     
                   
                 
               
               
                 
                   ( 
                   38 
                   ) 
                 
               
             
           
         
       
     
     Equation (37) allows for computation of the torques τ corresponding to the joint motion (q, {dot over (q)}, {umlaut over (q)}) and then projection of T onto the admissible domain of torque limits (as provided by the manufacturer), i.e.:
 
τ i =Max(Min(τ i ,τ i Max ),τ i Min )  (39)
 
     Regarding dynamic modeling, in order to get the dynamic modeling of the hybrid MPM system the Lagrange method is used. This can be done by applying Lagrange equation to the mechanical systems with either holonomic or nonholonomic constraints, along with the equations of constraint and their first and second derivatives involved into the equations of motion to produce the number of equations that is equal to the number of unknowns. 
     Considering ξ, which contains the variables of the mobile platform, let the generalized coordinates be ζ=[ξ T  θ 1  θ 2  θ 3  x y z] T . Notice that ζ contains all the variables of both the mobile platform and the parallel manipulator. In order to use the approach of Lagrangian equations for the derivation of the dynamic equations of the MPM, the kinetic and potential energies for all components of the manipulator must be expressed in terms of the chosen generalized coordinates and their derivatives. In this way we will get a number of equations equal to the number of the generalized coordinates (11 equations), which aids in obtaining the dynamic model. 
     Dynamic model analysis applies conventional simplifications, while also addressing the mechanical structure of the MPM  10 . With respect to a 3-RRPaR parallel manipulator, the upper parallelogram links cause the complexity of the dynamic model. These connecting links can be made of lightweight materials, such as aluminum alloy. Because of the lightweight assumption, the dynamic modeling can be simplified by the following hypothesis. The mass of each upper link is equally divided into two portions and placed at its two extremities, i.e., one half at its lower extremity (the end of lower link) and the other half at its upper extremity (moving platform). Thus, the rotational inertias of the upper links can be neglected. Also, the castor of the mobile platform can be made to be very light, so its dynamics are neglected. 
     Assume that the mobile robot cannot move in the lateral direction, i.e., it satisfies the conditions of pure rolling and non-slipping. Then, regarding constraint equations, the three constraints for the mobile platform can be represented by equation (2). 
     Another three constraint equations for the MPM can be derive from eq. (8), i.e.:
 
Γ 4   =x   p   2 +( y   p   +u−e−bcθ   1 ) 2 +( z   p   bsθ   1 ) 2   −a   2 ,
 
Γ 5 =( x   p   −u+e+bcθ   2 ) 2   +y   p   2 +( z   p   −bsθ   2 ) 2   −a   2 ,
 
Γ 6   =x   p   2 +( y   p   −u+e+bcθ   3 ) 2 +( z   p   −bsθ   3 ) 2   −a   2 ,  (40)
 
     With respect to the dynamic equations, because the mobile platform moves on a horizontal plane, there is no change in the potential energy U m  of the mobile platform. Thus, the kinetic energy can be calculated by: 
                     T   m     =         1   2     ⁢       m   c     ⁡     (         x   .     m   2     +       y   .     m   2     +       l   α   2     ⁢       ϕ   .     m   2         )         +       1   2     ⁢     I   c     ⁢       ϕ   .     m   2       +       1   2     ⁢     I   f     ⁢       θ   .     l   2       +       1   2     ⁢     I   f     ⁢       θ   .     r   2                 (   41   )               
where m c  is the mass of cart including the mobile platform, the base platform and three actuators for the parallel robot, while without the two driving wheels and rotors of the two motors; I c  is the moment of inertia of the mobile cart about a vertical axis through the mass center A, and I f  denotes the moment of inertia of each driving wheel and the motor rotor about the wheel axis. The potential energy of the parallel manipulator is:
 
                       U   p     =         (       m   p     +     3   ⁢     m   a         )     ⁢     z   p     ⁢   g     +       ∑     i   =   1     3     ⁢       (         1   2     ⁢     m   b       +     m   a       )     ⁢   bgs   ⁢           ⁢     θ   i             ,           (   42   )               
where, m b , m a , and m p , represent the mass of lower link, each connecting rod of upper link, and the moving platform, respectively.
 
     The kinetic energy for the parallel manipulator consists of kinetic energy of the upper moving platform, the upper links, and the connecting rods, and is derived to be: 
                       T   p     =         1   2     ⁢     (       m   p     +     3   ⁢     m   a         )     ⁢     (         x   .     2     +       y   .     2     +       z   .     2       )       +       1   2     ⁢     I   p     ⁢       ϕ   .     m   2       +       3   2     ⁢     (       m   b     +     m   a       )     ⁢     (         x   .     m   2     +       y   .     m   2     +         (       l   b     +   e     )     2     ⁢       ϕ   .     m   2         )       +       ∑     i   =   1     3     ⁢       1   2     ⁢     (         1   3     ⁢     m   b       +     m   a       )     ⁢     b   2     ⁢       θ   .     i             ,           (   43   )               
where, I p , denotes the moment of inertia of the moving platform about a vertical axis through its mass center. Thus, the Lagrange function for the MPM becomes:
 
 L=T+T   p   −U   m   −U   p   (44)
 
The constrained dynamics for the entire system of the MPM can be determined by:
 
                               ⅆ               ⅆ   t       ⁢     (       ∂   L       ∂       ζ   .     j         )       -       ∂   L       ∂     ζ   j           =       Q   j     +       ∑     i   =   1     6     ⁢       λ   i     ⁢       ∂     Γ   i         ∂     ζ   j                 ,     
     ⁢     (       j   =   1     ,   2   ,   …   ⁢           ,   11     )       ⁢     
     ⁢     i   .   e   .     :       ⁢     
     ⁢         Q   j     =           ⅆ               ⅆ   t       ⁢     (       ∂   L       ∂       ζ   .     j         )       -       ∂   L       ∂     ζ   j         -       ∑     i   =   1     6     ⁢       λ   i     ⁢       ∂     Γ   i         ∂     ζ   j                 ,     
     ⁢     (       j   =   1     ,   2   ,   …   ⁢           ,   11     )               (   45   )               
where Q=[0 0 0 τ l  τ r  τ 1  τ 2  τ 3  0 0 0] T  are the generalized forces under the assumption that no external forces/torques are exerted. Note that λ i  (i=1, 2, . . . , 6) are Lagrange multipliers associated with the constraints equations (2) and (22). The Lagrange multipliers can be calculated from the first set of linear equations of equation (33) for j=1, 2, 3, 9, 10, and 11. Once the Lagrange multipliers are found, the actuated torques τ=[τ l  τ r  τ 1  τ 2  τ 3 ] T  can be solved from the second set of equations of equation (27) for j=4, 5, 6, 7, and 8, which can be written into a matrix form:
 
 H (ζ) {umlaut over (q)}+V (ζ,{dot over (ζ)}) {dot over (q)}+G (ζ)=τ+ C (ζ)λ.  (46)
 
     Matlab software is used to get complete expressions of eq. (46) as follows. The dynamic parameters are: m a =0.2 kg, m b =0.5 kg, m p =0.8 kg, m c =7.5 kg, I p =0.00034 kg·m 2 ,I c =0.13982 kg·m 2 , and I f =0.00045 kg·m 2 . Solving the augmented Lagrange equation gives Q i =λ 4 g 1j (ζ)+λ 5 g 2j (ζ)+λ 6 g 4j ({umlaut over (ζ)}), where i=1, 2, 3, . . . , 11. H(ζ)=diag([A 1 , A 2 , A 4 , A 7 , A 10 ])ε   5×5  is the symmetric and positive definite inertial matrix. A l =A 2 =9/20000, A 4 =A 7 =A 10 =11/750. V(ζ,{dot over (ζ)})ε   5×5  is the centripetal and Coriolis forces matrix, here equal to zero. G(ζ)=[0; 0; A 5 ; A 8 ; A 11 ]ε   5×1  represents the vector of gravity forces, A 5 =0.8829 cθ 1 , A 8 =0.8829 cθ 2 , A 11 =0.8829 cθ 3 , while λ=[λ 1  λ 2  . . . λ 6 ]ε   6×1  denotes the vector for Lagrange multipliers. Thus: 
     
       
         
           
             
                 
             
             ⁢ 
             
               
                 λ 
                 = 
                 
                   
                     
                       [ 
                       
                         
                           
                             
                               f 
                               1 
                             
                           
                           
                             
                               f 
                               2 
                             
                           
                           
                             
                               f 
                               3 
                             
                           
                         
                         
                           
                             
                               f 
                               5 
                             
                           
                           
                             
                               f 
                               6 
                             
                           
                           
                             
                               f 
                               7 
                             
                           
                         
                         
                           
                             
                               f 
                               9 
                             
                           
                           
                             
                               f 
                               10 
                             
                           
                           
                             
                               f 
                               11 
                             
                           
                         
                       
                       ] 
                     
                     
                       - 
                       1 
                     
                   
                   ⁡ 
                   
                     [ 
                     
                       
                         
                           
                             f 
                             4 
                           
                         
                       
                       
                         
                           
                             f 
                             8 
                           
                         
                       
                       
                         
                           
                             f 
                             12 
                           
                         
                       
                     
                     ] 
                   
                 
               
               , 
               
                 
 
               
               ⁢ 
               
                   
               
               ⁢ 
               and 
               , 
               
                  
               
               ⁢ 
               
                   
               
               ⁢ 
               
                 
                   f 
                   1 
                 
                 = 
                 
                   
                     
                       - 
                       0.4 
                     
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     s 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       ϕ 
                       m 
                     
                     ⁢ 
                     c 
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       θ 
                       1 
                     
                   
                   = 
                   
                     
                       0.08 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       s 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         ϕ 
                         m 
                       
                     
                     - 
                     
                       0.2 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       c 
                       ⁢ 
                       
                           
                       
                       ⁢ 
                       
                         ϕ 
                         m 
                       
                     
                     + 
                     
                       2 
                       ⁢ 
                       
                         ( 
                         
                           
                             x 
                             m 
                           
                           - 
                           x 
                         
                         ) 
                       
                     
                   
                 
               
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 f 
                 2 
               
               = 
               
                 
                   
                     - 
                     0.28 
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   c 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     ϕ 
                     m 
                   
                 
                 - 
                 
                   0.4 
                   ⁢ 
                   c 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     ϕ 
                     m 
                   
                   ⁢ 
                   c 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     θ 
                     2 
                   
                 
                 + 
                 
                   2 
                   ⁢ 
                   
                     ( 
                     
                       
                         x 
                         m 
                       
                       - 
                       x 
                     
                     ) 
                   
                 
               
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 f 
                 3 
               
               = 
               
                 
                   0.08 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   s 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     ϕ 
                     m 
                   
                 
                 + 
                 
                   0.4 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   s 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     ϕ 
                     m 
                   
                   ⁢ 
                   c 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     θ 
                     3 
                   
                 
                 - 
                 
                   0.2 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   c 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     ϕ 
                     m 
                   
                 
                 + 
                 
                   2 
                   ⁢ 
                   
                     ( 
                     
                       
                         x 
                         m 
                       
                       - 
                       x 
                     
                     ) 
                   
                 
               
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 f 
                 4 
               
               = 
               
                 1.4 
                 ⁢ 
                 
                   x 
                   ¨ 
                 
               
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 f 
                 5 
               
               = 
               
                 
                   2 
                   ⁢ 
                   
                     ( 
                     
                       
                         y 
                         m 
                       
                       - 
                       y 
                     
                     ) 
                   
                 
                 - 
                 
                   0.2 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   s 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     ϕ 
                     m 
                   
                 
                 + 
                 
                   0.08 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   c 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     ϕ 
                     m 
                   
                 
                 + 
                 
                   0.4 
                   ⁢ 
                   c 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     ϕ 
                     m 
                   
                   ⁢ 
                   c 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     θ 
                     1 
                   
                 
               
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 f 
                 6 
               
               = 
               
                 
                   2 
                   ⁢ 
                   
                     ( 
                     
                       
                         y 
                         m 
                       
                       - 
                       y 
                     
                     ) 
                   
                 
                 - 
                 
                   0.28 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   s 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     ϕ 
                     m 
                   
                 
                 - 
                 
                   0.4 
                   ⁢ 
                   s 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     ϕ 
                     m 
                   
                   ⁢ 
                   c 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     θ 
                     2 
                   
                 
               
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 f 
                 7 
               
               = 
               
                 
                   2 
                   ⁢ 
                   
                     ( 
                     
                       
                         y 
                         m 
                       
                       - 
                       y 
                     
                     ) 
                   
                 
                 - 
                 
                   0.2 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   s 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     ϕ 
                     m 
                   
                 
                 - 
                 
                   0.08 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   c 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     ϕ 
                     m 
                   
                 
                 + 
                 
                   
                     - 
                     0.4 
                   
                   ⁢ 
                   c 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     ϕ 
                     m 
                   
                   ⁢ 
                   c 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     θ 
                     3 
                   
                 
               
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 f 
                 8 
               
               = 
               
                 1.4 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   y 
                   ¨ 
                 
               
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 f 
                 9 
               
               = 
               
                 
                   
                     - 
                     2 
                   
                   ⁢ 
                   z 
                 
                 + 
                 0.4 
                 + 
                 
                   0.4 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   s 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     θ 
                     1 
                   
                 
               
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 f 
                 10 
               
               = 
               
                 
                   
                     - 
                     2 
                   
                   ⁢ 
                   z 
                 
                 + 
                 0.4 
                 + 
                 
                   0.4 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   s 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     θ 
                     2 
                   
                 
               
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 f 
                 11 
               
               = 
               
                 
                   
                     - 
                     2 
                   
                   ⁢ 
                   z 
                 
                 + 
                 0.4 
                 + 
                 
                   0.4 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   s 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     θ 
                     3 
                   
                 
               
             
           
         
       
       
         
           
             
                 
             
             ⁢ 
             
               
                 f 
                 12 
               
               = 
               
                 
                   1.4 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     z 
                     ¨ 
                   
                 
                 + 
                 13.734 
               
             
           
         
       
       
         
           
             
               C 
               ⁡ 
               
                 ( 
                 ζ 
                 ) 
               
             
             = 
             
               
                 
                   [ 
                   
                     
                       
                         000 
                       
                       
                         0 
                       
                       
                         0 
                       
                       
                         0 
                       
                     
                     
                       
                         000 
                       
                       
                         0 
                       
                       
                         0 
                       
                       
                         0 
                       
                     
                     
                       
                         000 
                       
                       
                         
                           - 
                           
                             A 
                             3 
                           
                         
                       
                       
                         0 
                       
                       
                         0 
                       
                     
                     
                       
                         000 
                       
                       
                         0 
                       
                       
                         0 
                       
                       
                         
                           - 
                           
                             A 
                             9 
                           
                         
                       
                     
                   
                   ] 
                 
                 ∈ 
                 
                   
                     
                       5 
                       × 
                       6 
                     
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   is 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   the 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   parameter 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   matrix 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   for 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     λ 
                     . 
                     
                       
 
                     
                     ⁢ 
                     
                       A 
                       3 
                     
                   
                 
               
               = 
               
                 
                   0.4 
                   ⁢ 
                   s 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     θ 
                     1 
                   
                   ⁢ 
                   s 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       ϕ 
                       m 
                     
                     ⁡ 
                     
                       ( 
                       
                         x 
                         - 
                         
                           x 
                           m 
                         
                       
                       ) 
                     
                   
                 
                 - 
                 
                   0.4 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   s 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     θ 
                     1 
                   
                   ⁢ 
                   c 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       ϕ 
                       m 
                     
                     ⁡ 
                     
                       ( 
                       
                         y 
                         + 
                         
                           y 
                           m 
                         
                       
                       ) 
                     
                   
                 
                 + 
                 
                   0.016 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   s 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     θ 
                     1 
                   
                 
                 + 
                 
                   0.4 
                   ⁢ 
                   c 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     θ 
                     1 
                   
                   ⁢ 
                   z 
                 
                 - 
                 
                   0.08 
                   ⁢ 
                   c 
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     θ 
                     1 
                   
                 
               
             
           
         
       
       
         
           
             
               A 
               6 
             
             = 
             
               
                 0.4 
                 ⁢ 
                 s 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   θ 
                   2 
                 
                 ⁢ 
                 c 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   
                     ϕ 
                     m 
                   
                   ⁡ 
                   
                     ( 
                     
                       x 
                       - 
                       
                         x 
                         m 
                       
                     
                     ) 
                   
                 
               
               + 
               
                 0.4 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 s 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   θ 
                   2 
                 
                 ⁢ 
                 s 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   
                     ϕ 
                     m 
                   
                   ⁡ 
                   
                     ( 
                     
                       y 
                       - 
                       
                         y 
                         m 
                       
                     
                     ) 
                   
                 
               
               + 
               
                 0.056 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 s 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   θ 
                   2 
                 
               
               + 
               
                 0.4 
                 ⁢ 
                 c 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   θ 
                   2 
                 
                 ⁢ 
                 z 
               
               - 
               
                 0.08 
                 ⁢ 
                 c 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   θ 
                   2 
                 
               
             
           
         
       
       
         
           
             
               A 
               9 
             
             = 
             
               
                 
                   - 
                   0.4 
                 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 s 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   θ 
                   3 
                 
                 ⁢ 
                 s 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   
                     ϕ 
                     m 
                   
                   ⁡ 
                   
                     ( 
                     
                       x 
                       - 
                       
                         x 
                         m 
                       
                     
                     ) 
                   
                 
               
               + 
               
                 0.4 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 s 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   θ 
                   3 
                 
                 ⁢ 
                 c 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   
                     ϕ 
                     m 
                   
                   ⁡ 
                   
                     ( 
                     
                       y 
                       - 
                       
                         y 
                         m 
                       
                     
                     ) 
                   
                 
               
               + 
               
                 0.016 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 s 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   θ 
                   3 
                 
               
               + 
               
                 0.4 
                 ⁢ 
                 c 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   θ 
                   3 
                 
                 ⁢ 
                 z 
               
               - 
               
                 0.08 
                 ⁢ 
                 c 
                 ⁢ 
                 
                     
                 
                 ⁢ 
                 
                   
                     θ 
                     3 
                   
                   . 
                 
               
             
           
         
       
     
     Recalling equation (20), {dot over (q)}=J†{dot over (x)}+(I 5×5 −J†J){dot over (q)} s . Let J n ε   5×1  be the normalized base of n(J) which is the null space of J. Then we have:
 
 JJ   n =0 4×1   ,J   n   T   J   n =1,
 
 J   n   T   J†= 0 1×4   ,J   n   J   n   T   =I   5×5   −J†J   (47)
 
By definition of x n =J n   T {dot over (q)} s , so that it can be shown that:
 
 {dot over (q)}=J†{dot over (x)}+J   n   {dot over (x)}   n  
 
 {umlaut over (q)}=J†{umlaut over (x)}−J†{dot over (J)}J†{dot over (x)}+J   n   {umlaut over (x)}   n   −J†{dot over (J)}J   n   {dot over (x)}   n   (48)
 
With the definition of x E =[x T x n   T ] T , and J E   † =[J † J n ], then substituting eq. 5.2 into equation (28), we can get the derivation of the dynamic equations described in Cartesian space, which is described by the following equation:
 
   H   (ζ) {umlaut over (x)}   E   +  V   (ζ,{dot over (ζ)}) {dot over (x)}   E   +  G   (ζ)=  τ ,  (49)
 
where
 
   H   (ζ)= J   E   †     T     H (ζ) J   E   † ,
 
   V   (ζ,{dot over (ζ)})= J   E   †     T     [V (ζ,{dot over (ζ)})− H (ζ) J   †     T     {dot over (J)}]J   E   † ,
 
   G   (ζ)= J   E   †     T     [G (ζ)− c (ζ)λ], and
 
  τ = J   E   †     T   τ.  (49)
 
     Regarding discrete time dynamic model, from a state-space form of the continuous-time dynamic model of the MPM we obtain the approximate state space discrete-time model. By deleting the time index and the contact forces, from equation (42) we obtain:
 
 {umlaut over (x)}   E   =  H     −1 (ζ)  τ −  H   −1 (ζ)(   V   (ζ,{dot over (ζ)}) {dot over (x)}   E   +  G   (ζ))  (50)
 
Using the state x 1 , and its time derivative x 2 , such that x 1 =x E  and x 2 ={dot over (x)} E  i.e., x=[x 1   T  x 2   T ] T , equation (47) is rewritten as:
 
   H   (ζ) {dot over (x)}   2   +  V   (ζ,{dot over (ζ)}) x   2   +  G   (ζ)=  τ   (51)
 
Also, equation (48) can be transformed to the following form:
 
                     x   .     =         [           0     5   ×   5             I     5   ×   5                 0     5   ×   5             0     5   ×   5             ]     ⁢   x     -     [           0     5   ×   1                       H   _       -   1       ⁡     (   ζ   )       ⁢     (           V   _     ⁡     (     ζ   ,     ζ   .       )       ⁢     x   2       +       G   _     ⁡     (   ζ   )         )             ]     +       [           0     5   ×   5                     H   _       -   1       ⁡     (   ζ   )             ]     ⁢     τ   _                 (   52   )               
To obtain the discrete time dynamic model of the MPM, equation (49) is expressed as follows:
 
 {dot over (x)}=Fx−D ( x )+ B ( x )  τ   (53)
 
where
 
                       F   =     [           0     5   ×   5             I     5   ×   5                 0     5   ×   5             0     5   ×   5             ]       ,     
     ⁢       D   ⁡     (   x   )       =     [           0     5   ×   1                       H   _       -   1       ⁡     (   ζ   )       ⁢     (           V   _     ⁡     (     ζ   ,     ζ   .       )       ⁢     x   2       +       G   _     ⁡     (   ζ   )         )             ]       ,     
     ⁢   and     ⁢     
     ⁢       B   ⁡     (   x   )       =       [           0     5   ×   5                     H   _       -   1       ⁡     (   ζ   )             ]     .               (   54   )               
By defining the sampling period as h k , such that h k &lt;t&lt;h k+1  and τ k=1   N h k =T, with being the total traveling time and the robot state is defined between two sampling points k and k+1 as:
 
 x ( t )= x ( h   k ), for  k= 1,2 , . . . , N.   (55)
 
The discrete time model to model equation (50) is written as:
 
 x   k+1   =F   d ( h   k ) x   k   −D   d ( x   k   ,h   k )+ B   d ( x   k   ,h   k )  τ   k ,  (56)
 
where F d , D d  and B d  are the discrete equivalents to F, D, and B matrices, and described below.
 
                               F   d     ⁡     (     h   k     )       =       ⁢       F   d     ⁡     (       k   +   1     ,   k     )                   =       ⁢     ⅇ     Fh   k                     =   ~     ⁢       ⁢     [           I     5   ×   5               h   k     ⁢     I     5   ×   5                   0     5   ×   5             I     5   ×   5             ]             ⁢     
     ⁢               D   d     ⁡     (       x   k     ,     h   k       )       =       ⁢       ∫   0     h   k       ⁢         F   d     ⁡     (       h   k     -   t     )       ⁢     G   ⁡     (       h   k     -   t     )       ⁢     (     D   ⁡     (     x   k     )       )     ⁢           ⁢     ⅆ   t                       =   ~     ⁢       ⁢             H   _       -   1       ⁡     (   ζ   )       ⁡     [               h   k   2     2     ⁢     I     5   ×   5                     h   k     ⁢     I     5   ×   5               ]       ⁢     (           V   _     ⁡     (     ζ   ,   ζ     )       ⁢     x   2       +       G   _     ⁡     (   ζ   )         )               ⁢     
     ⁢               B   d     ⁡     (       x   k     ,     h   k       )       =       ⁢       ∫   0     h   k       ⁢         F   d     ⁡     (       h   k     -   t     )       ⁢     B   ⁡     (     x   k     )       ⁢     ⅆ   t                     =       ⁢       [               h   k   2     2     ⁢     I     5   ×   5                     h   k     ⁢     I     5   ×   5               ]     ⁢         H   _       -   1       ⁡     (   ζ   )                         (   57   )               
So, the discrete time state-space dynamic model of the MPM is rewritten in the final form:
 
     
       
         
           
             
               
                 
                   
                     x 
                     
                       k 
                       + 
                       1 
                     
                   
                   = 
                   
                     
                       
                         [ 
                         
                           
                             
                               
                                 I 
                                 
                                   5 
                                   × 
                                   5 
                                 
                               
                             
                             
                               
                                 
                                   h 
                                   k 
                                 
                                 ⁢ 
                                 
                                   I 
                                   
                                     5 
                                     × 
                                     5 
                                   
                                 
                               
                             
                           
                           
                             
                               
                                 0 
                                 
                                   5 
                                   × 
                                   5 
                                 
                               
                             
                             
                               
                                 I 
                                 
                                   5 
                                   × 
                                   5 
                                 
                               
                             
                           
                         
                         ] 
                       
                       ⁢ 
                       
                         x 
                         k 
                       
                     
                     - 
                     
                       
                         [ 
                         
                             
                         
                         ⁢ 
                         
                           
                             
                               
                                 
                                   
                                     h 
                                     k 
                                     2 
                                   
                                   2 
                                 
                                 ⁢ 
                                 
                                   I 
                                   
                                     5 
                                     × 
                                     5 
                                   
                                 
                               
                             
                           
                           
                             
                               
                                 
                                   h 
                                   k 
                                 
                                 ⁢ 
                                 
                                   I 
                                   
                                     5 
                                     × 
                                     5 
                                   
                                 
                               
                             
                           
                         
                         ] 
                       
                       ⁢ 
                       
                         
                           
                             H 
                             _ 
                           
                           
                             - 
                             1 
                           
                         
                         ⁡ 
                         
                           ( 
                           ζ 
                           ) 
                         
                       
                       ⁢ 
                       
                         ( 
                         
                           
                             
                               
                                 V 
                                 _ 
                               
                               ⁡ 
                               
                                 ( 
                                 
                                   ζ 
                                   , 
                                   
                                     ζ 
                                     . 
                                   
                                 
                                 ) 
                               
                             
                             ⁢ 
                             
                               x 
                               2 
                             
                           
                           + 
                           
                             
                               
                                 
                                   H 
                                   _ 
                                 
                                 
                                   - 
                                   1 
                                 
                               
                               ⁡ 
                               
                                 ( 
                                 ζ 
                                 ) 
                               
                             
                             ⁢ 
                             
                               τ 
                               k 
                             
                           
                           + 
                           
                             
                               G 
                               _ 
                             
                             ⁡ 
                             
                               ( 
                               ζ 
                               ) 
                             
                           
                         
                         ) 
                       
                     
                   
                 
               
               
                 
                   ( 
                   58 
                   ) 
                 
               
             
           
         
       
     
     Regarding constraints modeling, the task of robotics simulation requires taking into consideration many constraints, such as the nominal values of kinematic and dynamic parameter (for example, the length of the link, velocities, accelerations, and also nominal torques that the actuators supported). These constraints are defined in joint space and in task space. 
     The robot constraints include nonholonomic constraints wherein the mobile robot cannot move in the lateral direction, i.e., it satisfies the conditions of pure rolling and non-slipping. Then, the three constraints for the mobile platform can be represented by equation (2). 
     Dynamic state equations consist of equation (55), which can be rewritten in the following formula:
 
 x   k+1   =f   d     k   ( x   k ,τ k   ,h   k )  (59)
 
     Limits on the intermediate lengths of links are expressed by equation (22), from which the limits of the angles are found utilizing the fact that each angle of the parallel manipulator is between 0.65 and 1.65 radian, and for the mobile platform it is between −10 radian and 10 radian. Singularity avoidance is performed as described above. 
     Regarding torque limits, another major issue for trajectory planning is not violating the control torque limits. In this research, we assume that the robot torques belong to a bounded set C⊂   N , as shown in the following formula:
 
 C={τ   k ε   N , such that: τ min ≦τ k ≦τ max   ,k= 0 , . . . , N− 1}  (60)
 
     With respect to sampling period limits, since the torque constraints bound indirectly the path traversal time, in order to achieve admissible solution to the optimal control problem, the overall robot traveling time T should not be too small. Also, in order to achieve system controllability, the sampling period must be smaller than the system smallest time mechanical constant between two control times. In this research, time mechanical constant and limits of sampling periods are assumed to be available previously. 
     Now, H is defined to be the sampling period:
 
 H={h   k ε   + , such that: h   min   ≦h   k   ≦h   max }  (61)
 
     Task and workspace constraints are basically geometric and kinematic, from which the size and shape of the manipulator workspace is determined. These constraints are expressed by imposing to the end effector (EE) to pass through a set of specified poses. These constraints represent equality constraints and are written for simplicity as:
 
 s   1   l ( x )=∥ p−p   l   ∥−T   PassThlp =0
 
 s   2   l ( x )=∥vect( R   T   R   l )∥− T   PassThl     R   =0 ,l= 1 , . . . L   (62)
 
The above inequality constraints are written in the following simplified forms:
 
 g   1 ( x )= q   min −Θ( x )≦0 ,g   2 ( x )=Θ( x )− q   Max ≦0
 
 g   3 (τ)=τ Min −τ≦0 ,g   4 (τ)=τ−τ Max≦ 0,  (63)
 
where q Min/Max  and τ Min/Max  are for (θ 1 , θ 2 , θ 3 ), all inequality constraints will be noted as g j (x, τ, h)≦0,j=1, . . . , 4, regardless if they depend only on state, control variables or both. Hence, we turn up with J=12 inequality constraints, 2 L equality constraints (imposed passages), and 6 equality constraints representing state dynamics equations.
 
     To validate the effectiveness of the established dynamic model for the MPM, the dynamic control in task space is implemented by resorting to a model-based controller. Since the number of coordinates in task space is less than that in joint space, the proposed MPM possesses self-motion with one degree of redundancy. In this research, a simple solution is presented to stabilize the redundant robotic system. 
     In the model-based controller design, the desired trajectories, velocities and accelerations (x d , {dot over (x)} d , {umlaut over (x)} d ) can be determined in advance, and the desired self-motions x nd , {dot over (x)} nd , and {umlaut over (x)} nd  can be selected to perform secondary tasks besides the one in task space. Here, the self motion is exploited to optimize the problem of minimizing {{dot over (q)} T , {umlaut over (q)}}, subject to {dot over (x)}=J{dot over (q)}. The model-based control system  300  is illustrated in the block diagram of  FIG. 3  and comprises position and rate control subsystems  302 ,  304 , and  306 , gain constants K D    310  and K P    312 , acceleration bias constant  311 , a matrix multiply unit  314  and an angular controller  316  in operable communication with a processor  308  on the robot. Let x Ed =[x d   T  x nd   T ] T , then the error system can be defined by:
 
 e=x   Ed   −x   E 0  (64)
 
The adopted model-based controller is expressed in terms of positive definite constant gain matrices:
 
τ= J   E   T   [  H   ( {umlaut over (x)}   Ed   +K   D   ė+K   P   e )+   V {dot over (x)}   E   +  G ]   (65)
 
where K D  and K P  are the positive definite constant gain matrices. The error equation can then be generated accordingly as:
 
 ë+K   D   e+K   P   e= 0.  (66)
 
     Regarding simulation results for model validation, the dynamic control algorithm is implemented in task space such that the moving platform can track a desired trajectory, and the simulations are carried out via Matlab and Simulink software. Two desired trajectories are selected such that no kinematic singularity is encountered. The two trajectories consisted of a linear locus and a parabola-like special locus. The heading angle is assigned as φ d =0 in the first curve, and 0.1 t in the second. The architectural parameters of the designed MPM are: a=0.2 m, b=0.2 m, e=0.16 m, u=0.12 m, d=0.4 m, h=0.2 m, r=0.08 m, l a =0.2 m and l b =0.1. The dynamic parameters are: m a =0.2 kg, m b =0.5 kg, m p =0.8 kg, m c =7.5 kg, I p =0.00034 kg·m 2 , I c =0.13982 kg·m 2 , and I f =0.00045 kg·m 2 . In the simulation, all parameters are supposed to be accurate enough. The actuated joint angles are initialized to be at home position. Additionally, the simulation time interval is selected as 10 seconds, and the gain matrices are selected as K D =diag {10} and K P =diag { 25 }. The simulation revealed that both the position and heading angular errors can be eliminated by the model-based controller employing the present method. Moreover, if proper gains are chosen, the initial errors can be decreased rapidly. 
     Two extra simulations are carried out to see the effect of choosing {dot over (q)} s . In the first simulation, {dot over (q)} s  is put equal to a vector of 0.001 s, and in the second simulation, to 0.0001. In each case, we calculate the condition number of the Jacobean matrix J and plot it with time. Simulation results revealed that the value of 4, affects the condition number of J, and that the condition number is increasing highly as time increases, while it becomes stable around 1.8, which indicates good behavior. It should be noticed that by combining a mobile platform with a parallel robot, the problem of stability may occur, since in some postures the external forces would cause the manipulator to topple. In addition, regarding accurate navigation of the MPM, the odometric error containing both systematic and nonsystematic components should be taken into account for practical applications. 
     In general, any cost function with a physical sense can be optimized, and in robotics, several criteria have been implemented to obtain control optimization problem. The cost function can be defined according to task and planning objectives. The general objective function for a robot controlled in discrete time can be written in the following formula (P1): 
                       E   d     =       F   ⁡     [     x   N     ]       +       ∑     k   =   0       N   -   1       ⁢     L   ⁡     [       x   k     ,     τ   k       ]             ,           (   67   )               
where F[x N ] is a cost associated to the final state, whereas the second term in the right-hand side of the equation is related to the instantaneous state and control input variables (i.e., at time t k =kh). The robot state and input vectors x k  and τ k  are related by the discrete dynamic model represented by equation (39).
 
     The Minimum Time Control of robotic systems corresponds to F=0, L=1. In the mentioned criterion, (P1) had been widely considered by several authors. This is of interest, considering production targets in industrial mass production processes. But, the major disadvantage of this control method is its Bang-Bang character, which produces non-smooth trajectories, which hastens the mechanical fatigue of the machine. The sampling periods are defined such that the overall robot traveling time is: 
                     T   =       ∑     k   =   0       N   -   1       ⁢     h   k         ,           (   68   )               
where h k  is the robot traveling time between two successive discrete configurations k and k+1, k=0, . . . , N−1.
 
     In a first approach to the minimum time control problem, we consider a fixed sampling period h and search for a minimum number N of discretizations of the trajectory. This is equivalent to bringing the robot from an initial configuration x s  to a final imposed one x T  within a minimum number N of steps. For highly nonlinear and coupled mechanical systems, such as the MPM, it is impractical, even by using symbolic calculations. 
     In a second approach to the minimum time control problem, we consider a fixed number of discretizations N and vary the sampling time h k . This means that the robot moves from an initial configuration x s  to a final imposed one x T  within a fixed number of steps N while varying the sampling period&#39;s h k . 
     The minimum energy control problem is formulated as the case, while minimizing electrical energy cost, the robot moves from a starting point x s  to a target point x T . Thus, the governing criterion is characterized by the relation:
 
 F= 0 and  L=Σ   k=0   N−1 τ k   T   Rτ   k   (69)
 
     Using this criterion, or in general, using quadratic criterion, such as kinetic-energy criterion, (L=Σ k=0   N−1 v k   T Rv k , v is the velocity vector), the obtained trajectory is smoother, as it steers away from discontinuous trajectories. 
     The redundancy resolution and singularity avoidance control problem is addressed as follows. Because of the redundancy robots, the Jacobian J is not a square matrix. The kinematic redundancy might be used to solve the inverse kinematics by optimizing a secondary criterion. 
     An objective functional for the considered problem is developed, taking into consideration the performance index as it relates to energy consumption, traveling time, and singularity avoidance. For time criterion, as shown in the preceding, there are two basic ways to perform optimization. The first one fixes the sampling period h and searches for a minimum number N of discretizations. The second one fixes the number of discretizations N and varies the sampling periods h k . In this research, the number of sampling periods from an initial feasible kinematic solution is estimated. Then the sampling periods and the actuator torques are considered as control variables. In continuous-time, the constrained optimal control problem can be stated as follows. 
     Choosing all admissible control sequences τ(t)εC and hεH, which cause the robot to move from an initial state x(t o )=x S  to a final state x(t T )=X T , find those that minimize the cost function E: 
                     E   =       min         τ   ⁡     (   t   )       ∈   C         t   o     ,       t   T     ∈   H           ⁢       ∫     t   o       t   T       ⁢     {       [         τ   ⁡     (   t   )       ⁢   U   ⁢           ⁢       τ   T     ⁡     (   t   )         +     t   1     +       1   2     ⁢       x   2     ⁡     (   t   )       ⁢     Qx   2   T       +     δω   ⁡     (       x   1     ⁡     (   t   )       )         ]     ⁢           ⁢     ⅆ   t       }           ,           (   70   )               
Subject to constraints (53)-(60), with C, H, U, Q, t and δ being, respectively, the set of admissible torques, the set of admissible sampling periods, electric energy, kinetic energy, and time weights, and a weight factor for singularity avoidance, the corresponding discrete-time optimal control problem consists of finding the optimal sequences (τ 0 , τ 2 , . . . , τ N−1 ) and (h 0 , h 2 , . . . , h N−1 ), allowing the robot to move from an initial state x 0 =x S  to a target state x N =x T , while minimizing the cost E d :
 
                       min         τ   ⁡     (   t   )       ∈   C         t   o     ,       t   T     ∈   H           ⁢     E   d       =     {       ∑     k   =   0       N   -   1       ⁢       [         τ   k     ⁢   U   ⁢           ⁢     τ   k   T       +   t   +       x     2   ⁢   k       ⁢     Qx     2   ⁢   k     T       +     δω   ⁡     (       x     1   ⁢   k       ⁡     (   t   )       )         ]     ⁢     h   k         }             (   71   )               
subject to:
 
 x   k+1   =f   d     k   ( x   k ,τ k   ,h   k ), k= 0 , . . . , N− 1,
 
 g   j ( x   k ,τ k   ,h   k )≦0 ,j= 1 , . . . , J,k= 0 , . . . , N− 1, and
 
 s   j ( x   k )=0 ,i= 1, . . . , 2 L,k= 0 , . . . , N.  
 
     In the Augmented Lagrangian Approach for solving the stated Minimum Time-Energy Singularity-Free Trajectory Planning (MTE-SF-TP) n constrained on-linear control problem, there are two basic approaches. These are dynamic programming, and variational calculus through the Maximum principle of Pontryagin. In the dynamic programming is used to find a global optimal control. The optimal feedback control is through Hamilton-Jacobi-Bellman partial differential equations (HJB-PDE). 
     For linear-quadratic regulator problems, the HJB-PDE can be solved analytically or numerically by solving either an algebraic or dynamic matrix Riccati equation. For a general case, however, the PDE can be solved numerically for very small state dimensions only. 
     Adding the inequality constraints on state and control variables makes the problem harder. In this research, we propose to use the second approach to solve this problem. The Augmented Lagrangian (AL) is used to solve the resulting nonlinear and non-convex constrained optimal control problem. Powell and Hestens originated independently the method of using the AL. The AL function transforms the constrained problem into a non-constrained one, where the degree of penalty for violating the constraints is regulated by penalty parameters. After that, several authors improved it. Moreover, AL might be convexified to some extent with a judicious choice of the penalty coefficient. This procedure had been previously implemented in several cases of robotic systems. The AL function transforming the constrained optimal control problem into an unconstrained one is written as: 
                       L   μ     ⁡     (     x   ,   τ   ,   h   ,   λ   ,   ρ   ,   σ     )       =         ∑     k   =   1     N     ⁢       [         τ   k   T     ⁢   U   ⁢           ⁢     τ   k       +   t   +       x     2   ⁢   k     T     ⁢     Qx     2   ⁢   k         +     δω   ⁡     (     x     1   ⁢   k       )         ]     ⁢     h   k         +       ∑     k   =   0       N   -   1       ⁢     {       λ     k   +   1     T     ⁡     (       x     k   +   1       -       f     d   k       ⁡     (       x   k     ,     τ   k     ,     h   k       )         )       }       +       ∑     k   =   0       N   -   1       ⁢       h   k     [               ∑     i   =   1       L   -   1       ⁢       ∑     i   =   1     2     ⁢       Ψ     μ   s       ⁡     (       σ   k   i     ,       s   i   l     ⁡     (     x   k     )         )           +                 ∑     j   =   1     J     ⁢       Φ     μ   g       ⁡     (       ρ   k   j     ,       g   j     ⁡     (       x   k     ,     τ   k     ,     h   k       )         )               ]       +                                 ⁢           ⁢       ∑             ⁢           ⁢       h   N     ⁢       Ψ     μ   s       ⁡     (       σ   k   i     ,     s   i   l     ,     (     x   k     )       )                       (   72   )               
where the function ƒ d     k   (x k , τ k ,h k ) is defined by the discrete state eq. (58) at the sampling time k, N is the total sampling number, λεR 12N  designates the ajoint (or co-state) obtained from the adjunct equations associated to state equations, ρ,σ are Lagrange multipliers with appropriate dimensions associated to equality and inequality constraints, and μ g ,μ s  are the corresponding penalty coefficients. The penalty functions adopted here combine penalty and dual methods. This allows relaxation of the inequality constraints as soon as they are satisfied. Typically, these penalty functions are defined by:
 
                         ψ     μ   s       ⁡     (     a   ,   b     )       =         (     a   +         μ   s     2     ⁢   b       )     T     ⁢   b       ,       and   ⁢           ⁢     ϕ     μ   g         =       1     2   ⁢     μ   g         ⁢     {              Max   ⁡     (     0   ,     a   +       μ   g     ⁢   b         )            2     -          a        2       }                 (   73   )               
where a and b refer, respectively, to Lagrange multipliers and the left-hand side of equality and inequality constraints.
 
     The requirements for the Karush-Kuhn-Tucker first order optimality necessary conditions are that there must exist some positive Lagrange multipliers (λ k ,ρ k ), unrestricted sign multipliers σ k , and finite positive penalty coefficients (μ g ,μ s ), for x k , τ k , h k , k=0, . . . , N for there to be a solution to the problem, such that: 
     
       
         
           
             
               
                 
                   
                     
                       
                         
                           ∂ 
                           
                             L 
                             μ 
                           
                         
                         
                           ∂ 
                           x 
                         
                       
                       = 
                       0 
                     
                     , 
                     
                       
                         
                           ∂ 
                           
                             L 
                             μ 
                           
                         
                         
                           ∂ 
                           τ 
                         
                       
                       = 
                       0 
                     
                     , 
                     
                       
                         
                           ∂ 
                           
                             L 
                             μ 
                           
                         
                         
                           ∂ 
                           h 
                         
                       
                       = 
                       0 
                     
                     , 
                     
                       
                         
                           ∂ 
                           
                             L 
                             μ 
                           
                         
                         
                           ∂ 
                           λ 
                         
                       
                       = 
                       0 
                     
                     , 
                     
                       
                         
                           ∂ 
                           
                             L 
                             μ 
                           
                         
                         
                           ∂ 
                           ρ 
                         
                       
                       = 
                       0 
                     
                     , 
                     
                       
                         
                           ∂ 
                           
                             L 
                             μ 
                           
                         
                         
                           ∂ 
                           σ 
                         
                       
                       = 
                       0 
                     
                     , 
                     
                       
 
                     
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     and 
                   
                   ⁢ 
                   
                     
 
                   
                   ⁢ 
                   
                       
                   
                   ⁢ 
                   
                     
                       
                         
                           ρ 
                           k 
                           T 
                         
                         ⁢ 
                         
                           g 
                           ⁡ 
                           
                             ( 
                             
                               x 
                               , 
                               τ 
                               , 
                               h 
                             
                             ) 
                           
                         
                       
                       = 
                       0 
                     
                     , 
                     
                       
 
                     
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       
                         
                           σ 
                           k 
                           T 
                         
                         ⁢ 
                         
                           s 
                           ⁡ 
                           
                             ( 
                             x 
                             ) 
                           
                         
                       
                       = 
                       0 
                     
                     , 
                     
                       
 
                     
                     ⁢ 
                     
                         
                     
                     ⁢ 
                     
                       
                         g 
                         ⁡ 
                         
                           ( 
                           
                             x 
                             , 
                             τ 
                             , 
                             h 
                           
                           ) 
                         
                       
                       ≤ 
                       0 
                     
                   
                 
               
               
                 
                   ( 
                   74 
                   ) 
                 
               
             
           
         
       
     
     Application of these conditions allows deriving the iterative formulas to solve the optimal control problem by adjusting control variables and Lagrange multipliers, as well as penalty coefficients and tolerances. However, establishing the necessary conditions entails problems of an intractable complexity, even by using symbolic calculation. 
     In the constrained linear-decoupled formulation, the major computational difficulty mentioned earlier cannot be solved by performing with the original nonlinear formulation. Instead, it is solved using a linear-decoupled formulation. The present method utilizes the theorem stating that, provided that the inertia matrix is invertible, then the control law in the Cartesian space is defined as:
 
 u=  H   (ζ) v+  V   (ζ,{dot over (ζ)}) x   2   +  G   (ζ)  (75)
 
     This leads the robot to have a linear and decoupled behavior with a dynamic equation:
 
 {dot over (x)}   2 =ν  (76)
 
where ν is an auxiliary input. This follows simply by substituting the proposed control law of equation (75) into the dynamic model equation (39) to obtain:
 
   H   (ζ) {dot over (x)}   2   =  H   (ζ)ν.  (77)
 
Since  H (ζ) is invertible, it follows that {dot over (x)} 2 =ν. This brings the robot to have the decoupled and linear behavior described by the following linear dynamic equation written in discrete form as:
 
 x   k+1   =F   dk   x   k   +B ( h   k )(ν k )= f   d     k     D ( x   k ,ν k   ,h   k )  (78)
 
where
 
                 f     d   k     D     ⁡     (       x   k     ,     v   k     ,     h   k       )       =         [           I     5   ×   5               h   k     ⁢     I     5   ×   5                   0     5   ×   5             I     5   ×   5             ]     ⁢     x   k       -       [               h   k   2     2     ⁢     I     5   ×   5                     h   k     ⁢     I     5   ×   5               ]     ⁢       v   k     .               
Notice that this formulation drastically reduces the computations by alleviating the calculation at each iteration of the inertia matrix inverse and its derivatives with respect to state variables, which results in ease of calculation of the co-states. The non-linearity is, however, transferred to the objective function. One problem of this formula, which is Euler&#39;s method, is less accuracy. In order to improve accuracy, and because the MPM structure contains highly nonlinear equations, as shown above, we use the Adams-Bashforth Formula, given by the following general formula:
 
                     y     i   +   1       =       y   i     +       h   2     ⁢     (       3   ⁢     f   i       -     f     i   -   1         )       +       5   12     ⁢     h   3     ⁢       f   i     ″                 (   79   )               
Applying the Adams-Bashforth Formula of eq. (79) to the dynamic equation (78) gives:
 
     
       
         
           
             
               
                 
                   
                     x 
                     
                       1 
                       
                         k 
                         + 
                         1 
                       
                     
                   
                   = 
                   
                     
                       x 
                       
                         1 
                         k 
                       
                     
                     + 
                     
                       1.5 
                       ⁢ 
                       
                         h 
                         k 
                       
                       ⁢ 
                       
                         x 
                         
                           2 
                           k 
                         
                       
                     
                     + 
                     
                       0.5 
                       ⁢ 
                       
                         h 
                         k 
                       
                       ⁢ 
                       
                         x 
                         
                           2 
                           
                             k 
                             - 
                             1 
                           
                         
                       
                     
                     + 
                     
                       
                         
                           5 
                           ⁢ 
                           
                             h 
                             k 
                             3 
                           
                         
                         12 
                       
                       ⁢ 
                       
                         v 
                         
                           2 
                           k 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   80 
                   ) 
                 
               
             
             
               
                 
                   
                     x 
                     
                       2 
                       
                         k 
                         + 
                         1 
                       
                     
                   
                   = 
                   
                     
                       x 
                       
                         2 
                         k 
                       
                     
                     + 
                     
                       1.5 
                       ⁢ 
                       
                         h 
                         k 
                       
                       ⁢ 
                       
                         v 
                         
                           2 
                           k 
                         
                       
                     
                     + 
                     
                       0.5 
                       ⁢ 
                       
                         h 
                         k 
                       
                       ⁢ 
                       
                         v 
                         
                           2 
                           
                             k 
                             - 
                             1 
                           
                         
                       
                     
                     + 
                     
                       
                         
                           5 
                           ⁢ 
                           
                             h 
                             k 
                             3 
                           
                         
                         12 
                       
                       ⁢ 
                       
                         
                           v 
                           . 
                         
                         
                           2 
                           k 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   81 
                   ) 
                 
               
             
           
         
       
     
     Since it is difficult to get the derivative of ν k , and to improve accuracy, the following formulas from numerical differentiation methods are used:
 
 {dot over (y)}   1 =( y   2   −y   1 )/ h   1   (82)
 
 {dot over (y)}   2 =( y   3   −y   1 )/2 h   2   (83)
 
 {dot over (y)}   k =(− y   k+2 +8 y   k+1 −8 y   k−1   +y   k−2 )/12 h   k   (84)
 
 {dot over (y)}   N−1 =( y   N   −y   N−2 )/2 h   2   (85)
 
 {dot over (y)}   N−1 =( y   N   −y   N−1 )/ h   2   (86)
 
     Now, the decoupled formulation transforms the discrete optimal control problem into finding optimal sequences of sampling periods and acceleration inputs h 0 , h 2 , . . . , h N−1 , ν 0 , ν 2 , ν N−1 , allowing the robot to move from an initial state x 0 =x S  to a final state x N =x T , while minimizing the cost function: 
                     E   d   D     =       Min       v   ∈   V       h   k         ⁢           ⁢     {         ∑     k   =   0       N   -   1       ⁢           ⁢             [           [           H   _     ⁡     (   ζ   )       ⁢   v     +         V   _     ⁡     (     ζ   ,     ζ   .       )       ⁢     x   2       +       G   _     ⁡     (   ζ   )         ]     T     ⁢     U   ⁡     [           H   _     ⁡     (   ζ   )       ⁢   v     +         V   _     ⁡     (     ζ   ,     ζ   .       )       ⁢     x   2       +         G   ⁢                 _     ⁢     (           ⁢   ζ   )         ]         +   t   +       x     2   ⁢   k     T     ⁢     Qx     2   ⁢   k         +     δω   ⁡     (     x     1   ⁢   k       )         ]     ⁢     h   k       }       ,                 (   87   )               
under the above mentioned constraints, which remain the same, except actuator torques, which become:
 
τ min   ≦  H   (ζ) v+  V   (ζ,{dot over (ζ)}) x   2   +  G   (ζ)≦τ max   (88)
 
Henceforth, inequality constraints g 3  and g 4  can be rewritten as:
 
 g   3   D ( x   k   ,v   k )=τ min   −[  H   (ζ) v+  V   (ζ,{dot over (ζ)}) x   2   +  G   (ζ)]≦0  (89)
 
 g   3   D ( x   k   ,v   k )=[   H   (ζ) v+  V   (ζ,{dot over (ζ)}) x   2   +  G   (ζ)]−τ max ≦0  (90)
 
Similar to the non-decoupled case, the decoupled problem might be written in the following form:
 
               Min       v   ∈   V       h   k         ⁢           ⁢     E   d   D           
subject to:
 
 x   k+1   =f   d     k     D ( x   k ,τ k   ,h   k ), k= 0 , . . . , N− 1,
 
 g   j   D ( x   k ,ν k   ,h   k )≦0 ,jε{ 1,2, . . . , J}, and
 
 S   j   D ( x   k )=0 ,iε{ 1,2 , . . . , I},k= 0 , . . . N.   (91)
 
     The augmented Lagrangian associated to the decoupled formulation (P) is as follows: 
                       L   μ   D     ⁡     (     x   ,   v   ,   h   ,   λ   ,   ρ   ,   σ     )       =         ∑     k   =   0       N   -   1       ⁢           ⁢     {       [           [           H   _     ⁡     (   ζ   )       ⁢   v     +         V   _     ⁡     (     ζ   ,     ζ   .       )       ⁢     x   2       +       G   _     ⁡     (   ζ   )         ]     T     ⁢     U   ⁡     [           H   _     ⁡     (   ζ   )       ⁢   v     +         V   _     ⁡     (     ζ   ,     ζ   .       )       ⁢     x   2       +       G   _     ⁡     (   ζ   )         ]         +   t   +       x     2   ⁢   k     T     ⁢     Qx     2   ⁢   k         +     δω   ⁡     (     x     1   ⁢   k       )         ]     ⁢     h   k       }       +       ∑     k   =   0       N   -   1       ⁢           ⁢     {       λ     k   +   1     T     ⁡     (       x     k   +   1       -       f     d   k       ⁡     (       x   k     ,     τ   k     ,     h   k       )         )       }       +       ∑     k   =   0       N   -   1       ⁢           ⁢       h   k     ⁢             [         ∑     l   =   1       L   -   1       ⁢           ⁢       ∑     i   =   1     2     ⁢           ⁢       ψ     μ   s       ⁡     (       σ   k   i     ,       s   i   Dl     ⁡     (     x   k     )         )           +       ∑     j   =   1     J     ⁢           ⁢       ϕ     μ     g   ⁢                 ⁡     (       ρ   k   j     ,       g   j   D     ⁡     (       x   k     ,     τ   k     ,     h   k       )         )           ]     +       ∑     i   =   1     2     ⁢           ⁢       h   N     ⁢       ψ     μ   s       ⁡     (       σ   N   i     ,       s   i     D   ⁢           ⁢   1       ⁡     (     x   k     )         )                             (   92   )               
where the function ƒ d     k   (x k , τ k , h k ) is defined by eq. (78) at time k, N is the total sampling number, and other parameters appearing in (86) are defined above.
 
     The first order Karush-Kuhn-Tucker optimality necessary conditions require that for x k , ν k , h k , k=0, . . . , N to be a solution to the problem (P), there must exist some positive Lagrange multipliers (λ k , ρ k ), unrestricted sign multipliers σ k , and finite positive penalty coefficients μ=(μ g ,μ s ) such that equations (74) are satisfied for the decoupled formulation. The co-states λ k  are determined by backward integration of the adjunct state equation yielding: 
                     λ     k   -   1       =         -   2     ⁢     h   k     ⁢       ∂     [           H   _     ⁡     (   ζ   )       ⁢   v     +         V   _     ⁡     (     ζ   ,     ζ   .       )       ⁢     x   2       +       G   _     ⁡     (   ζ   )         ]         ∂     x   k         ⁢     U   ⁡     [           H   _     ⁡     (   ζ   )       ⁢   v     +         V   _     ⁡     (     ζ   ,     ζ   .       )       ⁢     x   2       +       G   _     ⁡     (   ζ   )         ]         -     2   ⁢     Qx     2   ⁢   k       ⁢     h   k       -     δ   ⁢       ∇     x     1   ⁢   k         ⁢     ω   ⁡     (     x     1   ⁢   k       )           -       F   d   T     ⁢     λ   k       -       h   k     ⁡     [       ∑     l   =   1       L   -   1       ⁢           ⁢       ∑     i   =   1     2     ⁢           ⁢       ∇     x   k       ⁢       ψ     μ   s       ⁡     (       σ   k   i     ,       s   i     D   ⁢           ⁢   1       ⁡     (     x   k     )         )             ]       -       h   k     ⁡     [       ∑     j   =   1     J     ⁢           ⁢       ∇     x   k       ⁢       ϕ     μ   g       ⁡     (       ρ   k   j     ,       g   j   D     ⁡     (       x   k     ,     v   k     ,     h   k       )         )           ]                 (   93   )               
The gradient of the Lagrangian with respect to sampling period variables is:
 
                     λ     k   -   1       =         -   2     ⁢     h   k     ⁢       ∂     [           H   _     ⁡     (   ζ   )       ⁢   v     +         V   _     ⁡     (     ζ   ,     ζ   .       )       ⁢     x   2       +       G   _     ⁡     (   ζ   )         ]         ∂     x   k         ⁢     U   ⁡     [           H   _     ⁡     (   ζ   )       ⁢   v     +         V   _     ⁡     (     ζ   ,     ζ   .       )       ⁢     x   2       +       G   _     ⁡     (   ζ   )         ]         -     2   ⁢     Qx     2   ⁢   k       ⁢     h   k       -     δ   ⁢       ∇     x     1   ⁢   k         ⁢     ω   ⁡     (     x     1   ⁢   k       )           -       F   d   T     ⁢     λ   k       -       h   k     ⁡     [       ∑     l   =   1       L   -   1       ⁢           ⁢       ∑     i   =   1     2     ⁢           ⁢       ∇     x   k       ⁢       ψ     μ   s       ⁡     (       σ   k   i     ,       s   i     D   ⁢           ⁢   1       ⁡     (     x   k     )         )             ]       -       h   k     ⁡     [       ∑     j   =   1     J     ⁢           ⁢       ∇     x   k       ⁢       ϕ     μ   g       ⁡     (       ρ   k   j     ,       g   j   D     ⁡     (       x   k     ,     v   k     ,     h   k       )         )           ]                 (   94   )               
The gradient of the Lagrangian with respect to acceleration variables is:
 
                         ∇   vk     ⁢     L   μ   D       =       2   ⁢       H   _     ⁡     (   ζ   )       ⁢       U   T     ⁡     [           H   _     ⁡     (   ζ   )       ⁢   v     +         V   _     ⁡     (     ζ   ,     ζ   .       )       ⁢     x   2       +       G   _     ⁡     (   ζ   )         ]       ⁢     h   k       +       z   k   T     ⁢     λ   k       +       h   k     ⁡     [       ∑     j   =   1     J     ⁢           ⁢       ∇     v   k       ⁢       ϕ     μ   g       ⁡     (       ρ   k   j     ,       g   j   D     ⁡     (       x   k     ,     v   k     ,     h   k       )         )           ]           ⁢     
     ⁢           ⁢   where   ⁢     
     ⁢           ⁢         Z   k     =         [           I     5   ×   5               h   k     ⁢     I     5   ×   5                   0     5   ×   5             I     5   ×   5             ]     ⁢     x   k       -       [               h   k   2     2     ⁢     I     5   ×   5                     h   k     ⁢     I     5   ×   5               ]     ⁢     v   k           ,     
     ⁢           ⁢     k   =   0     ,   2   ,   …   ⁢           ,     N   -   1.               (   95   )               
In the previous equations, the quantities:
 
                 ∂     [           H   _     ⁡     (   ζ   )       ⁢   v     +         V   _     ⁡     (     ζ   ,     ζ   .       )       ⁢     x   2       +       G   _     ⁡     (   ζ   )         ]         ∂     x   k         ,     
     ⁢       ∇     x     1   ⁢   k         ⁢     ω   ⁡     (     x     1   ⁢   k       )         ,       ∇     x   k       ⁢     ϕ     μ   g         ,     
     ⁢   and                 ∇     v   k       ⁢     ϕ     μ   g             
are calculated using numerical differentiation formulas in equations 82-86.
 
     With respect to implementation issues, an initial solution involving a fast converging kinematic-feasible solution is defined. It is based on an optimal time trajectory parameterization. The initial time diseretizations are assumed to be an equidistant grid for convenience, i.e., 
     
       
         
           
             
               
                 
                   
                     
                       h 
                       k 
                     
                     = 
                     
                       
                         
                           t 
                           
                             k 
                             + 
                             1 
                           
                         
                         - 
                         
                           t 
                           k 
                         
                       
                       = 
                       
                         
                           
                             t 
                             f 
                           
                           - 
                           
                             t 
                             0 
                           
                         
                         N 
                       
                     
                   
                   , 
                   
                     
 
                   
                   ⁢ 
                   
                     k 
                     = 
                     1 
                   
                   , 
                   2 
                   , 
                   … 
                   ⁢ 
                   
                       
                   
                   , 
                   
                     N 
                     - 
                     1 
                   
                 
               
               
                 
                   ( 
                   96 
                   ) 
                 
               
             
           
         
       
     
     Upon this parameterized minimum time trajectory, a model for predictive planning is built in order to achieve a good initial solution for the AL. At the calculation of the inertia matrix and Coriolis and centrifugal dynamics components, we can use the approach developed initially for serial robots by Walker and Orin based on the application of a Newton-Euler model of the robot dynamics. This method is generally straightforward, and is suitable for the case of MPM robots. 
     Regarding search direction update, a limited-memory quasi-Newton-like method is used at each iteration of the optimization process to solve for the minimization step at the primal level of AL, since the problem is of large-scale type. Refer to the flowchart  400  of  FIGS. 4A-4B , which outlines the AL algorithm function and operation. 
     In the present method, a systematic procedure is used for solving the augmented Lagrangian implementation, as shown in  FIGS. 4A-4B . In this procedure, the first step is selecting robot parameters, task definition, (such as starting, intermediate and final poses), workspace limitations and simulation parameters. Then the kinematic unit defines a feasible solution satisfying initial and final poses. After that, the inner optimization loop solves for ALD minimization with respect to sampling periods and acceleration control variables to give the MPM dynamic state. 
     In the following step, this state is tested within against feasibility tolerances. The feasibility is done by testing the norms of all equality and inequality constraints against given tolerances. If the feasibility test fails, restart the inner optimization unit. Otherwise, if the feasibility test succeeds, i.e., the current values of the penalty are good in maintaining near-feasibility of iterations, a convergence test is made against optimal tolerances. If convergence holds, display optimal results and end the program. If there is non-convergence, go further to the dual part of ALD to test for constraints satisfaction, and update multipliers, penalty and tolerance parameters. 
     If the constraints are satisfied with respect to a first tolerance level (judged as good, although not optimal), then the multipliers are updated without decreasing penalty. If the constraints are violated with respect to a second tolerance level, then keep the multipliers unchanged and decrease the penalty to ensure that the next sub-problem will place more emphasis on reducing the constraints violations. In both cases, the tolerances are decreased to force the subsequent primal iterates to be increasingly accurate solutions of the primal problem. 
     Offline trajectory planning simulation involves using Matlab. The initial values of thetas were chosen as follows: θ L =0; θ r =0; θ 1 =1.3; θ 2 =0.8; θ 3 =1.4. The target values of thetas were: θ L =1; θ r =0.8; θ 1 =1.1; θ 2 =1.2; θ 3 =1.1. 
     A first case involved minimizing time, and a second case involved minimizing energy. Torque variations and position variations were recorded. Small but acceptable errors were observed. 
     Regarding the online trajectory planning, the result of the offline trajectory planning is used to run 50 different trajectories, each one contains 21 points along the trajectory. This gives 1050 samples, among which 950 are considered for training, whereas testing and validation datasets were each obtained using 100 entry samples. 
     The Adaptive Neural Fuzzy Inference System (ANFIS) module training system  500  is shown in  FIG. 5 . Training the ANFIS is an optimization task with the goal of finding a set of weights that minimizes an error measure. Observed error during the training was RMSE less than 0.1 in less than 10 epochs. It is noted that the configuration used for the learning is determined among infinitely many solutions that exist for each input. 
     It is to be understood that the present invention is not limited to the embodiments described above, but encompasses any and all embodiments within the scope of the following claims.