Optimal trajectory planning for increased stability of mobile flexible manipulators undergoing large deflection

Abstract

In this article, a path planning algorithm for nonholonomic mobile flexible manipulators is presented that computes the robot trajectory by maximizing pose stability measures. Zero moment point criterion is used as a performance index for the system stability, which regards the mass moment of inertia of the mobile base. In dynamic analysis, an efficient model is employed to describe the treatment of flexible structure, in which both the geometric elastic nonlinearity and the foreshortening effects are considered. The optimization strategy is based on the indirect solution of the open-loop optimal control problem. Necessary optimality conditions in the form of Pontryagin’s minimum principle are established, which leads to a standard form of a two-point boundary value problem. A new objective function is proposed to improve stability for mobile flexible robot. In order to initially check the validity of the dynamic equations, the proposed model has been implemented and tested on a fixed-base flexible arm undergoing large deflection. A simulation study has been carried out to investigate further the validity and effectiveness of the mobile flexible manipulators on finding the optimal path between two points with stability consideration. The results clearly show the effect of flexibility and tip over stability on the mobile manipulators.

Introduction

Mobile manipulators that are required to have a long reach, fast motion, and reduced weight typically also possess significant structural flexibility. For example, mobile flexible manipulators have important application in the space station, manufacturing automation, nuclear contaminated environments, and many other areas. A common task for such mobile manipulators, referred to as a point-to-point maneuver, is to move the end-effector starting from the current configuration and stopping at a given final configuration. Furthermore, the effect of the manipulator, the payload movement, and moving on an uneven terrain may cause the base to turn over, particularly for mobile flexible manipulator when operating high speed with long arm. For such systems, to do an effective use of robotic systems, it is important to consider the path planning optimization of the system for increasing stability in point-to-point maneuvers, since it increases the applicability of robotic systems. However, the dynamic coupling effect between the manipulator and the mobile platform is challenging due to flexibility and kinematic constraints. An efficient model is employed to describe the treatment of flexible structure, in which both the geometric elastic nonlinearity and the foreshortening effects are considered. In this investigation, the optimization strategy is based on the indirect solution of the open-loop optimal control problem. Optimal control is a powerful tool which can be used in both open-loop and closed-loop strategies. In the open-loop optimal control, in contrast to the closed-loop ones, many difficulties like system nonlinearities and all types of constraints can be considered because of the off-line nature of the open-loop optimal control. The indirect solution method is based on Pontryagin’s minimum principle (PMP) which is a set of necessary optimality conditions.

Many researchers have studied the mobile manipulator’s problems during the last few years.^1–3 The dynamic model for links in most of these researches is often based on rigid or small deflection theory, but for applications like light-weight links, high-precision elements, or high speed maneuver, it is necessary to capture the deflection caused by nonlinear terms. Seraji presented a simple online approach for motion control of mobile manipulators using the combined Jacobian matrix. The proposed approach is extended to exploit the redundancy introduced by the base mobility to perform a set of user-specified additional tasks during the end-effector motion, which is equally applicable to holonomic and nonholonomic mobile robots. ¹ Yamamoto and Yun ² focused their research on the modeling and compensation of the dynamic interaction between the manipulator and the mobile platform of a mobile manipulator and developed a coordination algorithm based on the concept of a preferred operating region. Korayem et al. studied the effects of highly coupled dynamic interaction between the manipulator and vehicle. A new method using hierarchical optimal control for path planning and calculating maximum allowable dynamic load of wheeled mobile manipulator is presented. ³

Shaker and Ghosal considered nonlinear modeling of planar flexible manipulators with one and two revolute joints using the nonlinear finite element mathematical models. The motion equations of the systems are derived taking into account the nonlinear strain-displacement relationship. ⁴ Damaren and Sharf have presented and classified the different types of inertial and geometric nonlinearities in the dynamics equation for flexible multibody systems. They observed that for sufficiently fast maneuvers of the flexible-link manipulators, the ruthlessly linearized approximation is wholly inadequate. ⁵ Omar and Shabana developed an isoparametric shear-deformable two-dimensional beam element based on the absolute nodal coordinate formulation (ANCF) in which the elastic forces are determined using a general continuum mechanics approach. The use of the continuum approach leads to a simple expression for the elastic forces as compared to the use of local element frames. While the model accounts for the combined effects of shear deformation and rotary inertia, the finite element has zero Coriolis and centrifugal forces and leads to a constant mass. ⁶ Jiang et al. ⁷ presented a new motion planning approach, including reversal maneuvers, for car-like robots subject to nonholonomic constraints.

Several articles have tried to design the path planning problem for rigid and flexible manipulators. For instance, Wang et al. ⁸ had solved the optimal control problem with direct method using the B-Spline functions in order to determine the maximum payload of a rigid manipulator. Wilson et al. ⁹ formulated path planning of a flexible manipulator as a discrete time open-loop optimal control problem, the solution of which is done via discrete dynamic programming. Korayem and Nikoobin used the indirect solution of the optimal control problem to determine the dynamic load capacity of a flexible-link manipulator. The PMP has been used for path planning of the flexible-link manipulator. ¹⁰ Park considered the motion profiles of the joints as a Spline or polynomial functions. Then, parameters of the function have been obtained in order to reduce the residual vibration of flexible manipulators at the end of the motion. ¹¹

Some researchers have studied the stability of mobile manipulators. Papadopoulos and Rey have proposed a new tip over the stability measure, the Force–Angle stability measure, which is easily computed and sensitive to top-heaviness. The proposed measure has a simple geometric interpretation and is applicable to dynamic systems subjected to inertial loads and external forces. ¹² Li ¹³ has used the normal forces between the rear/front tires and ground for checking the static and dynamic stability of mobile manipulators. Moosavian and Alipour presented moment-height stability (MHS) measure to prevent overturning of a mobile manipulator. This measure is based on stabilizing and destabilizing moments exerted on the moving base which provides the system mobility. ¹⁴ The other approach is zero moment point (ZMP), which, for the first time, Huang et al. ¹⁵ employed for the tip over prevention of mobile manipulators. The valid stable region has been investigated for a mobile manipulator based on the ZMP criterion. However, the inertia effect of the rigid body has not been considered. Kim et al. ¹⁶ have proposed an online compensation algorithm to include the mass moment of inertia for original formulation of ZMP for the dynamic stability of the mobile manipulators.

In most of the previous works, flexibility and stability of the wheeled mobile manipulator in the path planning problem have not been considered. Hence, this article proposes a method for planning the trajectory of the nonholonomic mobile flexible manipulator for increasing stability and considering the effect of flexibility to achieve the specified point-to-point maneuver. First, the finite element method is used to derive dynamic equations of a mobile flexible manipulator. The strain energy is formulated in accordance with the slender beam theory, and various nonlinear terms are identified, in which both the geometric elastic nonlinearity and the foreshortening effects are considered. Then, the Hamiltonian function is formed and the necessary conditions for optimality are derived from an indirect solution of the open-loop optimal control problem by using the PMP. The obtained equations lead to a standard form of a boundary value problem which is solved by numerical techniques. Moreover, by applying the ZMP criterion as an index for the system stability, one can obtain optimal trajectory with attention to overturning stability constraint. Finally, in order to verify the effectiveness of the proposed approach, several experiments on a fixed-base flexible arm have been carried out and several simulation studies on a nonholonomic wheeled mobile manipulator are performed and optimal paths with increasing stability and minimum effort are obtained. The obtained simulation results demonstrate the accuracy and merits of the proposed method.

Kinematic and dynamic model of the mobile flexible manipulator

In this section, for the sake of modeling and analysis, a mobile flexible manipulator comprising a manipulator flexible arm mounted on a nonholonomic mobile base is considered, as shown in Figure 1. The considered system consists of a 5-degree of freedom (DOF) mobile base and two (n_i  + 1)-DOF serial manipulator arms, where $n_i$ is the total DOF of the number of elements used to describe the deflection of each link. Manipulators’ joints are revolute with a single DOF. However, the developed methodology can be applied equally well to systems with n-link manipulators. The motion of the system has to be decomposed into the motion of the flexible manipulator and the motion of the base. The unidirectional platform shown in Figure 2 is a typical example of a nonholonomic wheeled mobile robot (WMR) which has two rear driving wheels and two caster wheels. The two driving wheels are powered by direct current (DC) motors and have the same wheel radius, r. Point $P_b$ is the origin of WMR axis, which is located at the intersection of the longitudinal x-axis and the lateral y-axis. $L_0$ and $b$ are length and width of WMR body, respectively. The origin of the inertial frame $\{X,Y\}$ is shown as $O$ and as such allows the pose of the WMR to be completely specified through the following vector of generalized coordinates with respect to $\{X,Y\}$ , $q_b=[\begin{array}{ccccc}\hfill X_b\hfill & \hfill Y_b\hfill & \hfill \phi \hfill & \hfill {\theta }_r\hfill & \hfill {\theta }_l\hfill \end{array}]$ , where $X_b$ and $Y_b$ are the coordinates of the center of mass. The orientation of the WMR frame from the inertial frame is denoted as $\phi$ . ${\theta }_r$ and ${\theta }_l$ are the angular displacements of right and left driving wheel, respectively. Due to the nonholonomic nature of the system, the constraint equation obeys the ideal no-slip condition. The rolling and the knife edge constraint equations for this system can be found in Yamamoto and Yun. ² In the next paragraph, these constraints will be explained.

Figure 1.

Schematic view of a mobile flexible manipulator.

Figure 2.

Nonholonomic wheeled mobile robot platform.

The analysis of the flexible link can be modeled by slender elastic beams. In this investigation, a more efficient computationally model is employed, in which both the geometric elastic nonlinearity and the foreshortening effects are considered. The model takes into account a distinction between the longitudinal displacement due to axial deformation, denoted as $s$ , and the longitudinal displacement can occur due to the foreshortening effect, denoted by $u_{\mathrm{fs}}$ . The longitudinal displacement caused by transverse deflection of the neutral axis of the beam can be expressed as ¹⁷

$$u_{\mathrm{fs}}=-\frac{1}{2}\underset{0}{\overset{x}{\int }}[{\left(\frac{\partial v}{\partial x}\right)}^2+{\left(\frac{\partial w}{\partial x}\right)}^2]\mathrm{dx}$$

(1)

The assumed field of displacements for $\mathbf{u}$ can be written as

$$\mathbf{u}=\left[\begin{array}{c}\hfill u\hfill \\ \multicolumn{1}{c}{v}\\ \multicolumn{1}{c}{w}\end{array}\right]=\left[\begin{array}{c}\hfill s+u_{\mathrm{fs}}\hfill \\ \multicolumn{1}{c}{v}\\ \multicolumn{1}{c}{w}\end{array}\right]$$

(2)

The general expression of the strain energy is written in terms of $s$ , $v$ , and $w$ , as below ¹⁸

$$\begin{array}{c}\multicolumn{1}{c}{U=\frac{\mathrm{EA}}{2}\underset{0}{\overset{l}{\int }}{\left(\frac{\partial s}{\partial x}\right)}^2\mathrm{dx}+\frac{\mathrm{EI}}{2}\underset{0}{\overset{l}{\int }}{\left(\frac{{\partial }^{2}v}{\partial x^2}\right)}^2\mathrm{dx}}\\ \multicolumn{1}{c}{+\frac{\mathrm{EI}}{2}\underset{0}{\overset{l}{\int }}{\left(\frac{{\partial }^{2}w}{\partial x^2}\right)}^2\mathrm{dx}=\frac{1}{2}{q}_j^T{K}_j{q}_j}\end{array}$$

(3)

where $E$ , $A$ , $I$ , and $l$ denote the Young’s modulus, cross-sectional area, moment of inertia of the cross section, and length, respectively. This formulation has caused the nonlinear inertia terms and a constant stiffness matrix in the motion equations as follows

$${\mathbf{K}}_{\mathbf{j}}=\frac{\mathrm{EI}}{l^3}\left[\begin{array}{cccccccccc}\hfill A{l}^2/I\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill -A{l}^2/I\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill \\ \multicolumn{1}{c}{}& \hfill 12\hfill & \hfill 0\hfill & \hfill 6l\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill -12\hfill & \hfill 0\hfill & \hfill 6l\hfill & \hfill 0\hfill \\ \multicolumn{1}{c}{}& \hfill \hfill & \hfill 12\hfill & \hfill 0\hfill & \hfill 6l\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill -12\hfill & \hfill 0\hfill & \hfill 6l\hfill \\ \multicolumn{1}{c}{}& \hfill \hfill & \hfill \hfill & \hfill 4l^2\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill -6l\hfill & \hfill 0\hfill & \hfill 2l^2\hfill & \hfill 0\hfill \\ \multicolumn{1}{c}{}& \hfill \hfill & \hfill \hfill & \hfill \hfill & \hfill 4l^2\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill -6l\hfill & \hfill 0\hfill & \hfill 2l^2\hfill \\ \multicolumn{1}{c}{}& \hfill \hfill & \hfill \hfill & \hfill \hfill & \hfill \hfill & \hfill A{l}^2/I\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill \\ \multicolumn{1}{c}{}& \hfill \hfill & \hfill \hfill & \hfill \hfill & \hfill \hfill & \hfill \hfill & \hfill 12\hfill & \hfill 0\hfill & \hfill -6l\hfill & \hfill 0\hfill \\ \multicolumn{1}{c}{}& \hfill \hfill & \hfill \mathrm{Sym}.\hfill & \hfill \hfill & \hfill \hfill & \hfill \hfill & \hfill \hfill & \hfill 12\hfill & \hfill 0\hfill & \hfill -6l\hfill \\ \multicolumn{1}{c}{}& \hfill \hfill & \hfill \hfill & \hfill \hfill & \hfill \hfill & \hfill \hfill & \hfill \hfill & \hfill \hfill & \hfill 4l^2\hfill & \hfill 0\hfill \\ \multicolumn{1}{c}{}& \hfill \hfill & \hfill \hfill & \hfill \hfill & \hfill \hfill & \hfill \hfill & \hfill \hfill & \hfill \hfill & \hfill \hfill & \hfill 4l^2\hfill \end{array}\right]$$

(4)

The Lagrangian method is utilized to formulate the dynamic equations governing the motion of the mobile flexible manipulator systems. In order to derive dynamic equations, the kinetic energy and the potential energy are computed for the entire system. The kinetic energy for the overall system is obtained by computing the kinetic energy for each element ij and then summing over all the elements. The potential energy of the manipulator is obtained by computing the strain energy for each element ij due to elasticity and gravity of any link. After calculation of these energies, by applying the Lagrangian multipliers procedure and performing some algebraic manipulations, the compact form of the governing equations of two-link flexible mobile manipulator can be obtained from

$$\left[\begin{array}{ccc}\hfill M_{\mathrm{bb}}\hfill & \hfill M_{\mathrm{bm}}\hfill & \hfill M_{\mathrm{bf}}\hfill \\ \multicolumn{1}{c}{M_{\mathrm{mb}}}& \hfill M_{\mathrm{mm}}\hfill & \hfill M_{\mathrm{mf}}\hfill \\ \multicolumn{1}{c}{M_{\mathrm{fb}}}& \hfill M_{\mathrm{fm}}\hfill & \hfill M_{\mathrm{ff}}\hfill \end{array}\right]\left[\begin{array}{c}\hfill {\stackrel{··}{q}}_b\hfill \\ \multicolumn{1}{c}{{\stackrel{··}{q}}_m}\\ \multicolumn{1}{c}{{\stackrel{··}{q}}_f}\end{array}\right]+\left[\begin{array}{c}\hfill C_b(q_b,q_m,q_f,{\stackrel{·}{q}}_b,{\stackrel{·}{q}}_m,{\stackrel{·}{q}}_f)\hfill \\ \multicolumn{1}{c}{C_m(q_b,q_m,q_f,{\stackrel{·}{q}}_b,{\stackrel{·}{q}}_m,{\stackrel{·}{q}}_f)}\\ \multicolumn{1}{c}{C_f(q_b,q_m,q_f,{\stackrel{·}{q}}_b,{\stackrel{·}{q}}_m,{\stackrel{·}{q}}_f)}\end{array}\right]+\left[\begin{array}{c}\hfill G_b(q_b,q_m,q_f)\hfill \\ \multicolumn{1}{c}{G_m(q_b,q_m,q_f)}\\ \multicolumn{1}{c}{G_f(q_b,q_m,q_f)}\end{array}\right]=\left[\begin{array}{c}\hfill F_b-A^T(q)\lambda \hfill \\ \multicolumn{1}{c}{F_m}\\ \multicolumn{1}{c}{0}\end{array}\right]$$

(5)

where $\left[M\right]$ is the nonlinear mass matrix, $\left\{C\right\}$ is the vector of Coriolis and centrifugal forces, $\left\{G\right\}$ describes the gravity effects, and $\left[A\right]$ denotes nonholonomic constraints. The generalized coordinates, $\left\{q\right\}$ , and the generalized force, $\left\{F\right\}$ , are the following vectors

$$\begin{array}{c}\multicolumn{1}{c}{q=\left[q_b{q}_m{q}_f\right]=[X_f{Y}_f\phi {\theta }_r{\theta }_l{\theta }_1{\theta }_2{q}_{f1}\dots q_{\mathrm{fn}}]}\end{array}$$

(6)

$$F=\left[F_b{F}_{m}0\right]=[000{\tau }_{\mathrm{wr}}{\tau }_{\mathrm{wl}}{\tau }_1{\tau }_{2}0\dots 0]$$

(7)

The nonholonomy in mobile robotics causes some difficulties in modeling and motion planning, but it reduces the number of driving motors needed for the motion of the platform, and a nonholonomic mobile platform is more cost-efficient. There are three constraints in motion of the nonholonomic mobile platform: the first one is that the platform must move in the direction of the axis of symmetry and the other two constraints are the rolling constraints not allowing them to slip.

The nonholonomic constraints to which the platform is subjected are given as

$${\stackrel{·}{Y}}_f\cos \phi -{\stackrel{·}{X}}_f\sin \phi -d\stackrel{·}{\phi }=0$$

$${\stackrel{·}{Y}}_f\sin \phi +{\stackrel{·}{X}}_f\cos \phi +b\stackrel{·}{\phi }=r{\stackrel{·}{\theta }}_r$$

$${\stackrel{·}{Y}}_f\sin \phi +{\stackrel{·}{X}}_f\cos \phi -b\stackrel{·}{\phi }=r{\stackrel{·}{\theta }}_l$$

(8)

The three constraints can be written in the compact form

$$A(q)\stackrel{·}{q}=0$$

(9)

and

$$A=\left[\begin{array}{cccccccccc}\hfill -\sin \phi \hfill & \hfill \cos \phi \hfill & \hfill -d\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill \hfill & \hfill 0\hfill \\ \multicolumn{1}{c}{-\cos \phi }& \hfill -\sin \phi \hfill & \hfill -b\hfill & \hfill r\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill \dots \hfill & \hfill 0\hfill \\ \multicolumn{1}{c}{-\cos \phi }& \hfill -\sin \phi \hfill & \hfill b\hfill & \hfill 0\hfill & \hfill r\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill \hfill & \hfill 0\hfill \end{array}\right]$$

(10)

Where r is the radius of the driving wheel, b is the distance of two wheels, and d is the distance between front and rear wheels.

By defining the $B(q)$ matrix, which is the null space of $A(q)$ matrix, the Lagrange multipliers can be eliminated

$$A(q)B(q)=0$$

(11)

One choice of $B(q)$ is as follows

$$B=\left[\begin{array}{ccccccc}\hfill (r/2b)(b\cos \phi -d\sin \phi )\hfill & \hfill (r/2b)(b\cos \phi +d\sin \phi )\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill \hfill & \hfill 0\hfill \\ \multicolumn{1}{c}{(r/2b)(b\sin \phi +d\cos \phi )}& \hfill (r/2b)(b\sin \phi -d\cos \phi )\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill \hfill & \hfill 0\hfill \\ \multicolumn{1}{c}{(r/2b)}& \hfill -(r/2b)\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill \hfill & \hfill 0\hfill \\ \multicolumn{1}{c}{1}& \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill \dots \hfill & \hfill 0\hfill \\ \multicolumn{1}{c}{0}& \hfill 1\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill \hfill & \hfill 0\hfill \\ \multicolumn{1}{c}{0}& \hfill 0\hfill & \hfill 1\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill \hfill & \hfill 0\hfill \\ \multicolumn{1}{c}{0}& \hfill 0\hfill & \hfill 0\hfill & \hfill 1\hfill & \hfill 0\hfill & \hfill \hfill & \hfill 0\hfill \\ \multicolumn{1}{c}{0}& \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 1\hfill & \hfill \hfill & \hfill 0\hfill \\ \multicolumn{1}{c}{}& \hfill ⋮\hfill & \hfill \hfill & \hfill \hfill & \hfill \hfill & \hfill \ddots \hfill & \hfill 0\hfill \\ \multicolumn{1}{c}{0}& \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 1\hfill \end{array}\right]$$

(12)

As well, $\stackrel{·}{q}$ can be expressed as follows

$$\stackrel{·}{q}=B(q)\upsilon$$

(13)

where

$$\upsilon ={\left[\begin{array}{ccccccc}\hfill {\stackrel{·}{\theta }}_r\hfill & \hfill {\stackrel{·}{\theta }}_l\hfill & \hfill {\stackrel{·}{\theta }}_1\hfill & \hfill {\stackrel{·}{\theta }}_2\hfill & \hfill {\stackrel{·}{q}}_{f1}\hfill & \hfill \dots \hfill & \hfill {\stackrel{·}{q}}_{\mathrm{fn}}\hfill \end{array}\right]}^T$$

(14)

By differentiating equation (13)

$$\stackrel{··}{q}=B(q)\stackrel{·}{\upsilon }+\stackrel{·}{B}(q)\upsilon$$

(15)

After performing some algebraic manipulations, the dynamic equation of the mobile flexible manipulator is

$$B^T(q).M.(B(q)\stackrel{·}{\upsilon }+\stackrel{·}{B}(q)\upsilon )+B^T(q).(C+G)=B^T(q).F$$

(16)

Finally, the dynamic equations in the state space form considered in this article are as follows

$$\stackrel{·}{x}=\left[\begin{array}{c}\hfill B\upsilon \hfill \\ \multicolumn{1}{c}{{\left(B^T\mathrm{MB}\right)}^{-1}(-B^{T}M\stackrel{·}{B}\upsilon -B^{T}C)}\end{array}\right]+\left[\begin{array}{c}\hfill 0\hfill \\ \multicolumn{1}{c}{{\left(B^T\mathrm{MB}\right)}^{-1}}\end{array}\right]F$$

(17)

By using these equations, the optimal trajectory planning problem can be formulated.

Optimization strategy

Path planning in case of mobile flexible manipulator is complex due to the flexibility of the manipulator arm. In this section, an indirect solution of optimal control problem is applied for the off-line global trajectory planning of mobile flexible manipulator. In comparison with other methods, the open-loop optimal control approach provides a powerful tool for designers to create various optimal paths via defining the proper performance measure.

The purpose of the optimal control problem is to determine the control $u(t)$ that minimizes the performance index $J(u)$ . The general expression which minimizes the cost functional means that ¹⁰

$$\begin{array}{c}\multicolumn{1}{c}{\mathrm{MinimizeJ}=\underset{t_o}{\overset{t_f}{\int }}L(X(t),U(t),t)\mathrm{dt}=\frac{1}{2}\Vert X_1{\Vert }_{W_P}^2}\\ \multicolumn{1}{c}{+\frac{1}{2}\Vert X_2{\Vert }_{W_V}^2+\frac{1}{2}\Vert U{\Vert }_R^2}\end{array}$$

(18)

Here, the integrand $L(.)$ is a smooth, differentiable function in the arguments, $X(t)$ and $U(t)$ denote the state space form of the generalized coordinate and the joint torque, respectively. $\Vert X{\Vert }_K^2=X^{\mathrm{T}}\mathrm{KX}$ is the generalized squared norm; $W_P$ , $W_V$ are symmetric, positive semi-definite $(k\times k)$ weighting matrix; and $R$ is the symmetric, positive definite $(k\times k)$ matrix. Therefore, various optimal trajectories with different specifications can be obtained via changing the penalty matrices values $W_P$ , $W_V$ , and $R$ .

In order to minimize the objective function subjected to the nonlinear dynamic equations, the well-known PMP is used, and by introducing the costate vector $\psi$ , the Hamiltonian function of the system can be defined as

$$H(X,U,\psi ,t)=L(X,U)+{\psi }^{\mathrm{T}}\stackrel{·}{X}$$

(19)

The PMP then implies that a necessary condition for a local minimum is that $H$ be minimized with respect to $u(t)$ at all times. If it is assumed that the set of admissible inputs is bounded $U_i^{-}\le u_i^{*}\le U_i^{+}$ , this condition is equivalent to

$$\stackrel{·}{X}=\frac{\partial H}{\partial \psi }$$

$$\stackrel{·}{\psi }=\frac{-\partial H}{\partial X}$$

$$0=\frac{\partial H}{\partial U}$$

(20)

The optimal trajectory is then obtained by solving the 2n differential equations. The set of dynamic equations, the governing optimal control problem, and the boundary conditions lead to a standard form of a two-point boundary value problem (TPBVP). The collocation method is one of the basic ways of solving TPBVP

$$\{\begin{array}{c}\hfill {\stackrel{·}{\mathrm{x}}}^{*}(\mathrm{t})=\frac{\partial \mathrm{H}}{\partial \mathrm{p}}({\mathrm{x}}^{*}(\mathrm{t}),{\mathrm{u}}^{*}(\mathrm{t}),{\mathrm{p}}^{*}(\mathrm{t}),\mathrm{t})\hfill \\ \multicolumn{1}{c}{{\stackrel{·}{\mathrm{p}}}^{*}(\mathrm{t})=-\frac{\partial \mathrm{H}}{\partial \mathrm{x}}({\mathrm{x}}^{*}(\mathrm{t}),{\mathrm{u}}^{*}(\mathrm{t}),{\mathrm{p}}^{*}(\mathrm{t}),\mathrm{t})}\\ \multicolumn{1}{c}{\mathrm{H}({\mathrm{x}}^{*}(\mathrm{t}),{\mathrm{u}}^{*}(\mathrm{t}),{\mathrm{p}}^{*}(\mathrm{t}),\mathrm{t})\le \mathrm{H}({\mathrm{x}}^{*}(\mathrm{t}),\mathrm{u}(\mathrm{t}),{\mathrm{p}}^{*}(\mathrm{t}),\mathrm{t})}\end{array}$$

(21)

In this study, bvp4c command in MATLAB^® which is based on the collocation method is used to solve the obtained problem. The details of the numerical technique used in MATLAB to solve the TPBVP are given by Shampine et al. ¹⁹ The method iterates on the initial values of the costate until the final boundary conditions are satisfied by the following desired accuracy

$$\begin{array}{c}\multicolumn{1}{c}{h(X(t_f),t_f)=\frac{1}{2}\Vert X_1(t_f)-X_{1f}{\Vert }_{W_p}^2+\frac{1}{2}\Vert X_2(t_f)}\\ \multicolumn{1}{c}{-X_{2f}{\Vert }_{W_V}^2\le \epsilon }\end{array}$$

(22)

Now, by using the solution of obtained TPBVP, an algorithm is presented in Figure 3 in order to find the optimal paths

Figure 3.

Schematic diagram of the stable region.

Formulation of stability constraint

In order for mobile flexible manipulators to successfully carry out tasks, especially in carrying heavy loads on different trajectories, the concepts of the stability degree and the valid stable region become very important to avoid tipping over. Therefore, estimation and evaluation of dynamic stability with an appropriate criterion throughout the motion of such system is a fundamental necessity.

The stability margins could be enlarged by making a bigger platform; it also has a direct relation with the position of casters, but when the size of the platform must be smaller, the stability is critically argued. However, concepts of the stability degree and the valid stable region can be presented based on the ZMP criterion. ¹⁶ ZMP is defined as that point on the ground at which the net moment of the inertial forces and the gravity forces has no component along the horizontal axes. Each component of ZMP can be presented as ¹⁶

$$x_{\mathrm{zmp}}=\frac{{\displaystyle \sum _i}{m}_i({\stackrel{··}{z}}_i+g)x_i-{\displaystyle \sum _i}{m}_i{\stackrel{··}{x}}_i{z}_i-{\displaystyle \sum _i}{\left(T_y\right)}_i}{{\displaystyle \sum _i}{m}_i({\stackrel{··}{z}}_i+g)}$$

$$y_{\mathrm{zmp}}=\frac{{\displaystyle \sum _i}{m}_i({\stackrel{··}{z}}_i+g)y_i-{\displaystyle \sum _i}{m}_i{\stackrel{··}{y}}_i{z}_i-{\displaystyle \sum _i}{\left(T_x\right)}_i}{{\displaystyle \sum _i}{m}_i({\stackrel{··}{z}}_i+g)}$$

(23)

where the index i denotes parts of the robot and $(x_i,y_i,z_i)$ shows the coordinates of the center of mass for each part of the system. When ZMP exists within the domain of the stability margin, the contact between the ground and the robot wheels is stable, $P_{\mathrm{ZMP}}(x_{\mathrm{zmp}},y_{\mathrm{zmp}})\in S$ . Figure 3 shows the stability area (S) for $P_{\mathrm{ZMP}}$ , where $P_{\mathrm{ZMP}}$ denotes a position of ZMP, and S denotes a domain of the support surface.

In order for the mobile flexible manipulator to be always stable, ZMP must be within the stable region. The stable region is a quadrangle, and it is expressed with the position relation between the wheels and the casters (see Figure 3). Here, MSP is the Most Stable Point defined as the center of the contact polygon between the platform and the ground. The distance from MSP, base position $P_b(x_b,y_b)$ , to $\mathrm{ZMP}(x_{\mathrm{zmp}},y_{\mathrm{zmp}})$ is defined as $d_{\mathrm{ZMP}}$ which is expressed as follows

$$d_{\mathrm{zmp}}=\sqrt{{(x_{\mathrm{zmp}}-x_b)}^2+{(y_{\mathrm{zmp}}-y_b)}^2}$$

(24)

Therefore, the objective function for increasing stability can be defined as

$$\begin{array}{c}\multicolumn{1}{c}{J(u)=\underset{t_0}{\overset{t_f}{\int }}L(X,U,t)\mathrm{dt}=\frac{1}{2}\Vert X_1{\Vert }_{W_1}^2+\frac{1}{2}\Vert X_2{\Vert }_{W_2}^2}\\ \multicolumn{1}{c}{+\frac{1}{2}\Vert U{\Vert }_R^2+\frac{1}{2}\Vert d_{\mathrm{zmp}}{\Vert }_{W_{\mathrm{ZMP}}}^2}\end{array}$$

(25)

Simulation and experimental results

In this section, numerical and experimental results are presented to show the validity and effectiveness of the proposed approach. In order to initially check the validity of the dynamic equations and the response of the system, simulation is performed on a single-link very flexible arm as shown in Figure 4. One end (base) of the flexible link made of fiberglass is attached to a Harmonic Drive mini servo DC motor which has a reduction relation n = 50. At the other end (tip), there is a disk mass that can freely spin on its vertical axis by means of a bearing in order to prevent the apparition of torques at the tip. The physical characteristics of the link and the tip mass are specified in Table 1. The tip load of the link is a disk floating over an air table which allows us to cancel the effects of gravity and makes the friction of the movement almost inappreciable. The system sensor is integrated by an encoder embedded in the motor and a camera-based system for three-dimensional (3D) position measurement. The first one allows for knowing the motor angle with a precision of 0.7 × 10⁻⁴ rad. The second one consists of a set of three cameras that gives the tip position and the base position by means of a pair of spherical markers which reflect the infrared light. The precision of this system is 0.3 mm. The motor angular trajectory prescribed for this maneuver is shown in Figure 5. The maneuver of the motor is 1 rad with constant velocity 1 rad/s that it can lead to produce very large deflections. The obtained tip position in Cartesian is compared with the experimental results in Figures 6 and 7. The results are in good agreement with the experimental responses.

Figure 4.

The very flexible arm setup on the air table.

Table 1.

Parameters used for simulation.

Parameter	Value (manipulator)	Unit
Length of links	$L=1$	m
Density	$\rho =1761.7$	kg/m3
Diameter	$D=3\times {10}^{-3}$	m
Young’s modulus of material	$E=40\times {10}^9$	N/m2
Tip mass	$m_t=0.04376$	kg

Figure 5.

The motor and tip angular positions.

Figure 6.

Displacement of the tip in X direction.

Figure 7.

Displacement of the tip in Y direction.

The proposed nonlinear modeling method is also compared to obtained results by using the classical finite element method ⁵ and ANCF model. ⁶ Figure 8 shows the absolute error of the radius tip between nonlinear models with experiment. It is evident that the proposed nonlinear model is accurate than the other one.

Figure 8.

Tip radius error nonlinear models and experimental response.

A simulation study has been carried out to investigate further the validity and effectiveness of the mobile flexible manipulators on finding the optimal path between two points with stability consideration. A two-link planar manipulator is considered which is mounted on a differentially driven mobile base at point $F$ on the main axis of the base (Figure 1). In this investigation, the motion planning problem is to find the optimal trajectory with the minimum effort and increasing stability. The path with the minimum effort is a path in which the minimum torque is exerted by each motor in point-to-point motion. Therefore, in this case, penalty matrices can be considered as $R=\mathrm{diag}(1)$ , $W_p=W_V=[0]$ , and $W_{\mathrm{ZMP}}=[1]$ . This cost function is typical of systems that need to conserve energy and increase stability during a particular operation. The parameters of the mobile flexible manipulator are given in Table 2. The system is initially at rest, thus the mobile base is initially at the point ( $x_F=0.75m$ , $y_F=0.2m$ , $\phi =0^{}\circ$ ) and moves to final position ( $x_F=1.2m$ , $y_F=0.35m$ , $\phi ={15}^{}\circ$ ). The initial conditions of the manipulator are ${\theta }_1(0)=1.5\mathrm{rad}$ , ${\theta }_2(0)=2\mathrm{rad}$ , ${\stackrel{·}{\theta }}_1(0)=0$ , ${\stackrel{·}{\theta }}_2(0)=0$ (point A in Figure 9), and the final conditions are ${\theta }_1(t_f)=0.6\mathrm{rad}$ , ${\theta }_2(t_f)=0.02\mathrm{rad}$ , ${\stackrel{·}{\theta }}_1(t_f)=0$ , ${\stackrel{·}{\theta }}_2(t_f)=0$ (point B in Figure 9) during the overall time $t_f=2s$ (see Figure 9) and also the remaining boundary conditions are equal to zero.

Table 2.

Simulation of parameters.

Parameter	Value (base)	Value (manipulator)	Unit
Length of links	$L_0=0.4$	$L_1=L_2=0.5$	m
Mass	$m_b=55$	$m_1=m_2=3,m_p=0.5$	kg
Cross-sectional area	–	$A_1=A_2=4\times {10}^{-8}$	m2
Moment of inertia	$I_b=6.609$	$I_1=I_2=2.5\times {10}^{-9}$	m4
Young’s modulus of material	–	$E_1=E_2=2\times {10}^{10}$	N/m2

Figure 9.

The optimal paths between two points via minimum effort and increasing stability.

A comparative study is carried out between the rigid model and flexible models (linear and nonlinear models) as shown in Figure 10. The optimal angular positions of link and wheels, corresponding to the minimum effort and increased stability, are shown in Figures 11 and 12. The simulation results presented in Figures 13 and 14 show the varying torques of the manipulator actuators. The actuator torque constraint is based on the typical torque–speed characteristics of DC motors. It can be seen that the flexibility of link will increase the oscillation of torque curves. Variation of the ZMP points during optimal path was compared to the optimal trajectory with only minimum effort in the same conditions as shown in Figure 15. As expected, the optimum trajectory with increased stability and the minimum effort is more stable than the optimal trajectory with considering just minimum effort.

Figure 10.

The optimal paths for different models.

Figure 11.

The optimal angular positions of the first and second joints.

Figure 12.

The optimal angular positions of the right and left wheels.

Figure 13.

Minimum effort of the first motor within upper and lower acceptable boundaries.

Figure 14.

Minimum effort of the second actuator.

Figure 15.

Variation of the ZMP points.

Conclusion

The main objective of this investigation is to determine the trajectory optimization to improve the stability of the mobile flexible manipulators in point-to-point maneuver. The effect of the dynamic interaction between the flexible manipulator and the mobile platform is considered to characterize the motion of a nonholonomic mobile manipulator with the compliant link capable of large deflection, in which both the geometric elastic nonlinearity and the foreshortening effects are considered. This model leads to a constant stiffness matrix and makes the formulation particularly efficient in computational terms and numerically more stable than alternative geometrically nonlinear formulations based on lower order terms. The PMP is used to obtain the optimality conditions, which leads to a standard form of a TPBVP. A new objective function based on ZMP criterion is defined to increase stability during point-to-point maneuvers. The dynamic model has been implemented and tested on a single-link very flexible arm, and in order to verify the effectiveness of the presented method, several simulation studies on a nonholonomic wheeled mobile manipulator are carried out and optimal paths with the minimum effort and increasing stability are obtained. The numerical results indicate the effect of employing trajectory optimization in the performance improvement of the mobile flexible manipulator. Moreover, in this method, designer can compromise between different objectives by considering the proper penalty matrices, and it yields to choose the proper trajectory among the various paths. The obtained results illustrate the power and efficiency of the model to overcome the high nonlinearly nature of the large deflection and optimization problem which with other methods may be very difficult or impossible.