Abstract
In this paper, we study the trajectory planning problem for planar underactuated robot manipulators with two revolute joints in the absence of gravity. This problem is studied as an optimal control problem in which, given the dynamic model of a planar horizontal robot manipulator with two revolute joints one of which is not actuated, the initial state, and some specifications about the final state of the system, we find the available control input and the resulting trajectory that minimize the energy consumption during the motion. Our method consists in a numerical resolution of a reformulation of the optimal control problem as an unconstrained calculus of variations problem in which the dynamic equations of the mechanical system are regarded as constraints and treated using special derivative multipliers. We solve the resulting calculus of variations problem using a numerical approach based on the Euler-Lagrange necessary condition in integral form in which time is discretized and admissible variations for each variable are approximated using a linear combination of piecewise continuous basis functions of time. The use of the Euler-Lagrange necessary condition in integral form avoids the need for numerical corner conditions and the necessity of patching together solutions between corners.
1. Introduction
The class of underactuated manipulators includes robots with rigid links and unactuated joints, robots with rigid links and elastic transmission elements, and robots with flexible links. Whereas in the first case, underactuation is a consequence of design, in the other cases, it is the result of an accurate dynamic modeling of the system, in which the control inputs only have effect on the rigid-body motion. In any case, the number of available control inputs is strictly less than the number of the degrees of freedom of the robot. However, the control problem of different underactuated manipulators may have different levels of difficulty. For example, the absence of gravity significantly increases the complexity of the control problem.
In this paper we study the trajectory planning problem for planar horizontal underactuated robot manipulators with two revolute joints. The presence of two revolute joint in the mechanical system will be denoted by . We consider both possible models of planar horizontal underactuated robot manipulators, namely, the model in which only the shoulder joint is actuated, which will be denoted by , and the model in which only the elbow joint is actuated, which will be denoted by .
Underactuated robots are mechanical systems with second-order nonholonomic constraints because the dynamic equation of the unactuated part of the mechanical system is a second-order differential constraint which, in general, is nonintegrable. It is not possible to integrate even partially this second-order differential constraint in the dynamic model of the robot manipulator. However, in the presence of this second-order nonholonomic constraint, the system is controllable. On the contrary, in the dynamic model of the robot manipulator, this differential constraint is completely integrable. It can be converted into a holonomic constraint that makes the system not controllable. As a consequence, the trajectory planning problem has solutions only for particular initial and final states.
Planning of dynamically feasible trajectories, their asymptotic tracking, and the regulation to a desired equilibrium configuration are the main control problems for this class of mechanical systems. However, a general control theory for this class of mechanical systems has not been developed yet, and only solutions for particular robot models have been obtained. A review of the most significant works on control of underactuated robots with passive joints can be found in [1].
In this paper, the trajectory planning problem for planar horizontal underactuated robot manipulators is studied as an energy-optimal control problem. Given the initial and final states, we find the available control inputs and the corresponding trajectory that satisfy the dynamic equation of the robot manipulator and steer the system between initial and final states minimizing the energy consumption during the motion. This problem is referred to as boundary value problem. We also consider initial value problems in which the final state is not completely specified. Our numerical method can also tackle final value problems in which part of the initial state is not specified.
It is usually impossible to find analytical solutions to optimal control problems of robot manipulators, and, in general, numerical methods must be employed. To solve our optimal control problem, we apply a numerical method that falls into the class of indirect methods which are based on first-order necessary optimality conditions. More precisely, it is a variational approach in which the optimal control problem is transformed into an unconstrained calculus of variations problem by means of special derivative multipliers [2, Chapter VII].
Our method substantially differs from the usual indirect approaches in which the Euler-Lagrange differential equation is solved using a suitable numerical method. To compute the extremals, we use the Euler-Lagrange necessary condition in integral form plus transversality conditions to take into account the components of state and control variables and multipliers that are not specified at the endpoints [3, Chapter 6]. In this way, we avoid the loss of information induced by the use of the differential form of the same condition that implies numerical corner conditions and the necessity of patching together solutions between corners.
Moreover, in our method, the control inputs are derivatives of some components of the extended state vector which can be piecewise continuous functions, while the original state vector is supposed to be composed by piecewise smooth functions. Similarly, the multipliers which are the derivatives of other components of the extended state vector need only to be piecewise continuous. In our approach, time is discretized, and admissible variations for each variable are approximated by linear combinations of piecewise continuous basis functions of time. In this way, variations depend on the values of the coefficients at the mesh points. The conditions under which the objective functional is stationary with respect to all piecewise smooth variations that satisfy the boundary conditions are derived, and the set of nonlinear difference equations that must be satisfied by the coefficients is obtained. This set of equations is then solved using the Newton-Raphson method. This basic procedure can be modified to incorporate equality and inequality constraints by means of derivative multipliers and derivative excess variables.
In [4], the necessary conditions for optimal control are derived using the ideas of Lagrangian reduction that is reduction under a symmetry group. The techniques presented in this work are designed for Lagrangian mechanical holonomic and nonholonomic systems with symmetry. The key idea is to link the method of Lagrange multipliers with Lagrangian reduction as an alternative to the Pontryagin Maximum Principle and Poisson reduction. [5, Chap. 7] is devoted to optimal control of nonholonomic mechanical systems. The relationship between variational nonholonomic control systems and the classical Lagrange problem of optimal control is presented. Then, kinematic and dynamic optimal control problems are discussed whereas related work on integrable systems is studied in the Internet supplement of this book. In [6] an affine connection formulation is used to study an optimal control problem for a class of nonholonomic, underactuated mechanical systems. The class of nonholonomic systems studied in this paper are wheeled vehicle. The nonholonomic affine connection together with Lagrange multiplier method in the calculus of variations is used to derive the optimal necessary conditions.
The mechanical systems studied in this paper have similarities with the Pendubot and the Acrobot which are underactuated two-link robot manipulators that move in a vertical plane and therefore are subjected to gravity force. In the Pendubot, only the shoulder joint is actuated, whereas in the Acrobot, only the elbow joint is actuated,. The control objective is usually in both cases to drive the manipulator away from the straight-down position and steer it at the straight-up position. In [7], a unified strategy for motion control of underactuated two-link manipulators with gravity, such as the Acrobot and the Pendubot, is presented. First, a control law is employed to increase the energy and control the posture of the actuated link in the swing-up region. Finally, an optimal control law is designed for the attractive region using a linear approximation model of the system around the straight-up position. In [8], a general control methodology for swinging up and stabilizing underactuated two-link robots is presented. It is based on Euler-Lagrange dynamics, passivity analysis, and dynamic programming theory. In [9], two different approaches for feedforward control design are presented. The first approach is based on a coordinate transformation into the nonlinear input-output normal form, whereas the second approach uses servo constraints and results in a set of differential algebraic equations.
To the best knowledge of the authors, the optimal control of underactuated robot manipulators without gravity and without breaks has not been addressed.
This paper is organized as follows. In Section 2, the dynamic models of the two planar horizontal underactuated robot manipulators are described, and in Section 3, their control properties are discussed. The optimal control problem for these dynamic systems is stated in Section 4. In Section 5, a reformulation of the optimal control problems as a calculus of variations problem is presented, and in Section 6 the proposed numerical method to solve the resulting calculus of variations problem is described. In Section 7, the results of the application of this numerical method to several optimal control problems for planar horizontal underactuated robot manipulators are reported. Finally, Section 8 contains the conclusions.
2. Dynamic Model of Underactuated Manipulators
The general dynamic model of a robot manipulator is described by the following second-order differential equation where the first term of this equation, , represents the inertial forces due to acceleration at the joints and the second term, , represents the Coriolis and centrifugal forces. The third term, , is a simplified model of the friction in which only the viscous friction is considered. The term, , represents the potential forces such as elasticity and gravity. Matrix on the right-hand side maps the external forces/torques to forces/torques at the joints. Finally represents the forces/torques at the joints that are the control variables of the system.
We suppose that the links are rigid as well as the transmission elements and that the robot moves in a horizontal plane in such a way the gravity does not affect the dynamics of the manipulator. Finally, we do not take into account the effects of the friction, and we suppose that no external forces are acting on the mechanical system. Under these hypotheses, the dynamic model of the robotic system reduces to
A horizontal planar manipulator is composed of two homogeneous links and two revolute joints moving in a horizontal plane , as shown in Figure 1, where is the length of link , is the distance between joint and the mass center of link , is the mass of link , and is the barycentric inertia with respect to a vertical axis of link , for . In this case the two matrices and have the form where is the vector of configuration variables, being the angular position of link 1 with respect to the axis of the reference frame and the angular position of link 2 with respect to link 1 as illustrated in Figure 1. The vector is the vector of angular velocities, and is the vector of accelerations. The control inputs of the system are , where is the torque applied by the actuator at joint and is the torque applied by the actuator at joint . The parameters , , and in (3) have the following expressions:
A robot manipulator is said to be underactuated when the number of actuators is less than the degree of freedom of the mechanical system. The dynamic model (2) that does not consider the effects of gravity and friction can be rewritten for a robot manipulator underactuated by one control in the form [1] in which the state variables and correspond to the actuated and unactuated joints and is the available control input. The last equation of (5) describes the dynamics of the unactuated part of the mechanical system and has the form which is a second-order differential constraint without input variables.
Underactuated manipulators may be equipped with brakes at the passive joints. Hybrid optimal control strategies can be designed in this case [10, 11]. The presence of brakes will not be considered in this paper.
3. Control Properties of Underactuated RR Robot Manipulators
In this section, the main control properties of planar underactuated robot manipulators without the effects of the gravity will be described. For a more general description of the control properties of underactuated robot manipulators, see [1].
Optimal control approaches to trajectory planning assume that there exists a control input that steers the system between two specify states. Thus, controllability is the most important aspect to check before studying optimal control of a dynamic system. If in the trajectory planning problem the duration of the motion is not assigned, the existence of a finite-time solution for any state in a neighborhood of is equivalent for the robotic system to the property of local controllability at . If local controllability holds at any state, then the system is controllable and the trajectory planning problem is solvable for any pair of initial and final states.
For underactuated robot manipulators, controllability is related to integrability of the second-order nonholonomic constraints. The second-order differential constraint (6) may either be partially integrable to a first-order differential equation or completely integrable to a holonomic equation. Necessary and sufficient integrability conditions are given in [12, 13]. If (6) is not partially integrable it is possible to steer the system between equilibrium points. This occurs for planar underactuated robot manipulators without gravity which therefore are controllable. If (6) is completely integrable, to a holonomic constraint, the motion of the mechanical system is restricted to a -dimensional submanifold of the configuration space which depends on the initial configuration. This occurs for the dynamic equations of planar underactuated robot manipulators without the effects of the gravity [12]. For this robot model, the trajectory planning problem has solution only for particular initial and final states. Thus, when (6) is not partially or completely integrable, the mechanical system is controllable. However, several aspects of controllability can be studied which characterize this model of underactuated manipulator.
A dynamical system is linearly controllable at an equilibrium point if the linear approximation of the system around this point is controllable. Planar underactuated robot manipulators in the absence of gravity are not linearly controllable. On the contrary both planar underactuated and robot manipulators are linearly controllable in the presence of gravity.
A mechanical system is said to be small-time locally controllable () at if, for any neighborhood of and any time , the set of states that are reachable from within time , along trajectories contained in , includes a neighborhood of . Note that small-time local controllability is a stronger property than controllability [14]. Non- but controllable system must in general perform finite maneuvers in order to perform arbitrarily small changes of configuration. It has been proven in [15, 16] that planar underactuated robot manipulators in the absence of gravity are not . Both planar underactuated and robot manipulators in the presence of gravity are also not .
Second-order mechanical systems cannot be at states with nonzero velocity. Therefore, the weaker concept of small-time local configuration controllability has been introduced [17]. A system is said to be small-time local configuration controllable () at a configuration if, for any neighborhood of in the configuration space and any time , the set of configurations that are reachable (with some final velocity ) within , starting from and along a path in configuration space contained in , includes a neighborhood of . By definition, systems are also . Sufficient conditions for are given in [17]. It has been proven in [15, 16] that planar underactuated robot manipulators in the absence of gravity are not . Both planar underactuated and robot manipulators in the presence of gravity are also not .
A final question is to investigate if the trajectory planning problem for planar underactuated robot manipulators can be solved with algorithmic methods. A mechanical system is kinematically controllable () if every configuration is reachable by means of a sequence of kinematic motions, that is, feasible paths in the configuration space which may be followed with any arbitrary timing law [15, 16, 18]. Note that mechanical systems are also and that kinematic controllability does not imply small-time local controllability. If a mechanism is , the trajectory planning problem may be solved with algorithmic methods. Planar underactuated robot manipulators in the absence of gravity are not . Both planar underactuated and robot manipulators in the presence of gravity are not [15].
4. The Optimal Control Problem
Given the dynamic equation of an underactuated planar robot manipulator, an initial state, , and a final state, , the optimal control problem consists in finding the available control input, or , and the resulting trajectory with that steers the system between initial and final states satisfying the dynamic equation (5) and minimizing the objective functional where depending on which joint is actuated and and are the initial and final time values, respectively. This cost functional represents a measure of the energy consumed during the motion, since torque produced with an electromechanical actuator is approximately proportional to the current flow, and the rate of energy consumption is approximately equal to the square of this current.
If , the problem is called rest-to-rest trajectory planning problem. If the final or the initial states or part of them is not assigned, the problems are called initial value problem and final value problem, respectively. The final time may be fixed or not.
This problem is a particular case of an optimal control problem which can be stated in a more general form as follows. Minimize the integral subject to where is an -vector called the state vector, is an -vector called the control vector, the real-valued function is the objective functional, (9) is called the trajectory equation, and the conditions (14) are called the boundary conditions. The set is called the set of controls, with for every . We assume that , , , , , and are sufficiently smooth for our purpose. This will imply solutions such that is piecewise smooth, whereas is piecewise continuous [19].
5. Variational Reformulation of the Optimal Control Problem
A variational approach has been used to solve the more general optimal control problem stated in the previous section.
The classical calculus of variations problem is to minimize an integral of the form such that where the independent variable is assumed to be in the interval and the dependent variable is assumed to be an -vector continuous on with derivative . It is also assumed that is piecewise smooth, that is, there exists a finite set of points so that , is continuously differentiable on and that the respective left- and right-handed limits of exist. If is piecewise smooth, and satisfies the boundary conditions , , then is said to be an admissible arc. In words, this problem consists in finding, among all arcs connecting end points and , the one minimizing the integral (16).
The main optimality conditions are obtained by defining a variation , a set of functions and a functional where is a fixed real number and the variation is a piecewise smooth function with . Using a Taylor series expansion, it is easy to see that a necessary condition that is a relative minimum to is where denote the partial derivatives of evaluated along and the terms and are evaluated at .
Integrating (20) by parts for all admissible variations , another necessary condition for to give a relative minimum of the variational problem (16)-(17) is obtained, which is the following second-order differential equation: known as Euler-Lagrange condition. This equation must hold along except at a finite number of points [3, Section 2.1].
The extremals of (16)-(17) can be obtained by solving the Euler-Lagrange equation, but it only holds at points where the extremal is smooth. At points where has jumps, called corners, the Weierstrass-Erdmann corner conditions must be fulfilled [3, Section 2.3]. Since the location of the corners, their number, and the amplitudes of the jumps in are not known in advance, it is difficult to obtain a numerical method for a general problem using the Euler-Lagrange equation (21). One of the key aspects of our method is that the integral form of this condition holds for all and some , and therefore the Weierstrass-Erdmann corner conditions are not needed. Thus, an alternative way of computing the extremals can be based on this necessary condition in integral form.
Note that necessary condition requires that boundary values fulfill Euler-Lagrange equation. Thus, if some of the four values , , and are not explicitly given, alternate boundary conditions have to be provided. This is what transversality conditions do. Assume that , and are given but is free. In this case, the additional necessary transversality condition must hold.
The variational approach does not consider constraints. However, the optimal control problem has, at least, a first-order differential constraint (9) representing the dynamic equation of the system. Moreover, since the dynamic equation of a planar robot manipulator is a second-order differential equation, additional differential constraints will arise while rewriting it as a first-order differential equation. Therefore, the optimal control problem must be reformulated as an unconstrained calculus of variations problem in order to deal with differential and algebraic constraints as described in the following section.
Following [3, Chapter 5], we reformulate as an unconstrained calculus of variations problem the optimal control problem consisting in minimizing (8) subject to (9), (10), (11), (14), and (15). Notice that we omitted constraints (12) and (13) which need a special treatment.
For convenience, we change the independent variable from to and the dependent variable from to to be consistent with the notation of calculus of variations. Our reformulation is based on special derivative multipliers and a change of variables in which is the renamed state vector, is the renamed state vector, is the multiplier associated with (9), is the multiplier associated with constraint (10), is the multiplier associated with constraint (11), is the excess variable of constraint (11).Since are not unique without an extra condition, we initialize these variables by defining . Thus, our problem becomes where Since the values of , are unknown, transversality conditions are needed, having the form with and .
5.1. Holonomic Equality Constraints
In this section, we show how to deal with the equality constraint (12). For the sake of clarity, we consider the problem subject to which is related to the problem subject to In the above lines is an -vector, and are assumed to be differentiable in their arguments or with the needed smoothness. We also assume that . The boundary conditions of the problems are any combination of fixed boundary conditions for the components of with the possibility of leaving some of them unspecified.
If we reformulate problem (27)-(28) using the technique described in Section 5, we get the following Hamiltonian , with where is the multiplier. We have in this case which is singular. The singularity of is a difficulty we must avoid. Furthermore, even when it is not difficult to change from the constraint to the constraint by increasing the dimension of the independent variables, it is not easy to deal with the new associated boundary conditions. This is the reason that problem (27)-(28) is so difficult to solve.
It has been shown in [20] that problem (27)-(28) can be reformulated as an equivalent problem of the form (29)-(30). In particular, is a solution to (27)-(28) if and only if is a solution to subject to
If is a solution to (27)-(28), then and . On the other hand, if is a solution to (32)-(33), then for
6. Numerical Method
The numerical method used is based on the discretization of the unconstrained variational calculus problem stated in the previous section. In particular, the main underlying idea is obtaining a discretized solution solving (20) for all piecewise linear spline function variations instead of dealing with the Euler-Lagrange equation (21). Thus, this method uses no numerical corner conditions and avoids patching solutions to (21) between corners.
Let be a large positive integer, , and let be a partition of the interval , where for . Define the one-dimensional spline hat functions where . Define also the -dimensional, piecewise linear component functions where , is the sought numerical solution, and is a numerical variation. In particular, the constant vectors are to be determined by the algorithm developed by us, and the constant vectors are arbitrary. Thus, the discretized form of (20) is obtained in each subinterval . For the sake of clarity of exposition, we assume that . Note that in (20) is linear in so that a three-term relationship may be obtained at by choosing for . Thus, or In these equations and is the computed value of at . In the general case, when , the same result is obtained but and are column -vectors of functions with th component and , respectively. Similarly, is the -vector which is the average of the -vectors and .
By the same arguments that led to (38), which is the numerical equivalent of the transversality condition (23). For further details, see [3, Chapter 6].
It has been shown in [21] that with this method the global error has a priori global reduction ratio of . In practice, if the step size is halved, the error decreases by .
7. Implementation and Results
Several numerical experiments have been carried out for both and planar horizontal underactuated robot manipulators.
7.1. Planar Horizontal Underactuated Robot Manipulator
In this section, the optimal control problem of a planar horizontal underactuated is studied. In this robot model the second joint is not actuated; thus, . In this case, it is neither possible to integrate partially nor completely the nonholonomic constraint because the manipulator inertia matrix contains terms in (see [12]). Hence, the system is controllable. The numerical results of the application of our method for optimal control to a boundary value problem and to an initial value problem for this system will be described.
For a planar horizontal underactuated , (2) can be split into To express optimal control problems that involve this second-order differential constraints in the form of a basic optimal control problem, we have first to convert it into first-order differential constraints introducing the following change of variables: with the following additional relations Thus, the second-order differential equations (40) are converted into the first-order differential equations Relations (42), (43), and (44) are now the differential constraints of the optimal control problem, and the objective functional to minimize is
Then, we introduce the following new variables: such that where with is the multiplier associated with differential constraint (43), with is the multiplier associated with the differential constraint (44), and with and with are the multipliers associated with the additional equality constraints (42).
Thus, the unconstrained reformulation (24) of the problem is in this case where with initial conditions for both the boundary value problem and the initial value problem. The final conditions for the boundary value problem have the form whereas in the initial value problem, will be free for some .
The initial values of control variables and multipliers have been set to zero, whereas their final values have not been assigned in both optimal control problems. Therefore, transversality conditions are needed in both cases for the variables , , and they will be of the form with . Moreover, in the initial value problem, additional transversality conditions will be needed for each that is let free. We have used for both problems the following settings [kg m2], [m], [kg], whereas the initial and final times have been and , respectively.
7.1.1. Problem 1: Boundary Value Problem
The following initial and final conditions have been imposed on the state variables as follows: The initial values of control variable and of the multipliers have been set to zero, whereas their final values are left free. Figure 2 shows the sequence of configurations of the robot at times , . Since the configurations of the sequence overlap, it has been split into smaller sequences for a better visualization of the manipulator motion. Figure 3 depicts the corresponding control and state variables of the optimal solution of this boundary value problem obtained with a discretization of the time interval into subintervals. The value of the objective functional for this solution is [J].
| (a) , |
| (b) , |
| (c) , |
| (a) |
| (b) |
| (c) |
| (d) |
| (e) |
7.1.2. Problem 2: Initial Value Problem
An initial value problem has also been solved with the following initial and final conditions The initial values of control variable and of the multipliers have been set to zero, whereas their final values are left free. The only difference between these conditions and those of the boundary value problem described in Section 7.1.1 is that now .
Figures 4 and 5 depict the sequence of configurations the robot manipulator, and the corresponding control and state variables of the optimal solution of this initial value problem, respectively, obtained with a discretization of the time interval into subintervals. The value of the objective functional for this solution is [J]. This value is lower than the value of the objective functional of the solution of the boundary value problem described in Section 7.1.1 because now is , and the control system does not have to spend energy to stop it.
| (a) , |
| (b) , |
| (a) |
| (b) |
| (c) |
| (d) |
| (e) |
7.2. Planar Horizontal Underactuated Robot Manipulator
In this section, the optimal control problem of a planar horizontal underactuated robot manipulator is studied. In this robot model, the first joint is not actuated; thus, and (2) can be split into
As explained in [12], since gravity terms are all zero and does not intervene in the system inertia matrix, (56) can be partially integrated to Actually, constraint (56) is completely integrable giving rise to an holonomic constraint. The resulting holonomic constraint takes different forms depending on the value of which depends on the initial conditions. Therefore, two cases have been considered: (i) when the initial velocities and are both zero, (ii) when the initial velocity is nonzero.
7.2.1. Problem 3: Initial Value Problem with Zero Initial Velocities
An initial value problem has been solved with the following initial and final conditions: The initial values of the control variable and of the multipliers have been set to zero, whereas their final value is left free.
Since there is a holonomic constraint that relates the values of the angles and , without integrating (58), we are not able to find the value of consistent with . Therefore, no final conditions have been imposed on .
From the initial conditions of the problem, we obtain . Equation (58) with corresponds to the homogeneous differential constraint The differential is not exact. However, it becomes an exact differential if multiplied by the factor . This operation does not alter the differential equation (60). In this case, there does exist a function whose differential coincides with the expression . Due to the existence of this function, the integral of between two points depends only on these points and not on the integration path. Equation (60) rewritten in this form can be integrated by separating variables. The corresponding holonomic constraint has the following expression:
To express this optimal control problem in the form of a basic optimal control problem, we first have to convert (57) into a first-order differential model introducing the following change of variables: with the following additional relations: Thus, the optimal control problem is to minimize subject to the constraints and the additional constraints (64). To reformulate this optimal control problem as an unconstrained calculus of variations problem, let be such that where with is the multiplier associated with the holonomic constraint (66), with is the multipliers associated with the differential constraint (67), and with and with are the multipliers associated with the additional equality constraints (64).
Thus, the holonomic constraint of the problem can be rewritten as follows: Now, the technique described in Section 5.1 to deal with holonomic constraints can be applied to , and this holonomic constraint is replaced by From the initial conditions of the problem, the latter equation reduces to the equality , whereas the former takes the following form: The corresponding Hamiltonian is It is not difficult to check that matrix is singular. This is due to the fact that to handle our optimal control problem which involves second-order differential constraints we converted them into first-order differential constraints. Therefore, we apply again the technique of Section 5.1 obtaining the identity and the following constraint: The corresponding Hamiltonian is It is not difficult to check that matrix in this case is not singular since its determinant is Substituting the values of , , and , this expression becomes which is always positive for any real value . Figure 6 shows the sequence of configurations of the robot at times with and Figure 7 depicts control and state variables of the optimal solution obtained with a discretization of the interval into subintervals.
| (a) |
| (b) |
| (c) |
| (d) |
| (e) |
In particular, we get [rad]. To check the consistency of this result with the holonomic constraint (62), since [rad/s], we get from (58) that and using the initial condition [rad], we get from (62) that . Having established the value of the constant , we obtain from the same equation, for [rad], that [rad] which coincides with the value of obtained numerically.
7.2.2. Problem 4: Initial Value Problem with Nonzero Initial Velocity
Another initial value problem has been solved with the following conditions: The initial values of the multipliers have been set to zero whereas their final value is left free. Notice that no final conditions have been imposed on and . The same considerations done in previous section hold in this case, as well. The technique described in Section 5.1 must be applied twice leading to the differential constraint (74) and to the Hamiltonian (75). Figure 8 shows the sequence of configurations of the robot at times with , and Figure 9 depicts the control and state variables of the optimal solution obtained with a discretization of the interval into subintervals. In particular, we get that [rad] and [rad/s]. To check the consistency of the obtained value of with the holonomic constraint, consider (58). We can calculate the constant using the initial conditions of the problem obtaining Since , (58) corresponds in this case to the differential constraint Equation (79) can be rewritten in this form To check the obtained value of , is numerically integrated between and using the interpolated numerical optimal solution obtained for . We get that . This value is close to .
| (a) |
| (b) |
| (c) |
| (d) |
| (e) |
To check the consistency of the obtained value of with the constraint (58), using the computed value and the final conditions , of the problem, we obtain This value is very close to the value of obtained numerically.
7.3. Computational Issues
If the optimal control problem has variables and the time interval has been discretized into subintervals, the resulting set of difference equations (38) has equations and variables plus the equations and variables due to transversality conditions. Feasible solutions have been used as initial guesses of the algorithm.
The solution of the nonlinear system of difference equations (38) has been obtained using a damped Newton algorithm within a line search methodology implemented in Mathematica 7 under Mac OS X operating system (see [22, 23] for more details).
8. Conclusion
In this paper the trajectory planning problem for planar underactuated robot manipulators with two revolute joints without gravity has been studied. This problem is solved as an optimal control problem based on a numerical resolution of an unconstrained variational calculus reformulation of the optimal control problem in which the dynamic equation of the mechanical system is regarded as a constraint. It has been shown that this reformulation method based on special derivative multipliers is able to tackle both integrable and nonintegrable differential constraints of the dynamic models of underactuated planar horizontal robot manipulators with two revolute joints. This method can be seamlessly applied in the presence of additional constraints on the mechanical system.