This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
Transcript
Hindawi Publishing CorporationAbstract and Applied AnalysisVolume 2013 Article ID 476094 16 pageshttpdxdoiorg1011552013476094
Research ArticleEnergy-Optimal Trajectory Planning for Planar UnderactuatedRR Robot Manipulators in the Absence of Gravity
John Gregory1 Alberto Olivares2 and Ernesto Staffetti2
1 Department of Mathematics Southern Illinois University Carbondale Carbondale IL 62901 USA2Department of Signal and Communication Theory Universidad Rey Juan Carlos Fuenlabrada 28943 Madrid Spain
Correspondence should be addressed to Ernesto Staffetti ernestostaffettiurjces
Received 30 January 2013 Revised 19 April 2013 Accepted 22 April 2013
Academic Editor Qun Lin
Copyright copy 2013 John Gregory et al This is an open access article distributed under the Creative Commons Attribution Licensewhich permits unrestricted use distribution and reproduction in any medium provided the original work is properly cited
In this paper we study the trajectory planning problem for planar underactuated robot manipulators with two revolute joints in theabsence of gravityThis problem is studied as an optimal control problem in which given the dynamic model of a planar horizontalrobot manipulator with two revolute joints one of which is not actuated the initial state and some specifications about the finalstate of the system we find the available control input and the resulting trajectory that minimize the energy consumption duringthe motion Our method consists in a numerical resolution of a reformulation of the optimal control problem as an unconstrainedcalculus of variations problem in which the dynamic equations of the mechanical system are regarded as constraints and treatedusing special derivative multipliers We solve the resulting calculus of variations problem using a numerical approach based on theEuler-Lagrange necessary condition in integral form in which time is discretized and admissible variations for each variable areapproximated using a linear combination of piecewise continuous basis functions of timeThe use of the Euler-Lagrange necessarycondition in integral form avoids the need for numerical corner conditions and the necessity of patching together solutions betweencorners
1 Introduction
The class of underactuatedmanipulators includes robots withrigid links and unactuated joints robots with rigid links andelastic transmission elements and robots with flexible linksWhereas in the first case underactuation is a consequenceof design in the other cases it is the result of an accuratedynamic modeling of the system in which the control inputsonly have effect on the rigid-body motion In any case thenumber of available control inputs is strictly less than thenumber of the degrees of freedom of the robot Howeverthe control problem of different underactuated manipulatorsmay have different levels of difficulty For example theabsence of gravity significantly increases the complexity ofthe control problem
In this paper we study the trajectory planning problemfor planar horizontal underactuated robot manipulators withtwo revolute joints The presence of two revolute joint inthe mechanical system will be denoted by 119877119877 We considerboth possible models of planar horizontal underactuated 119877119877robot manipulators namely the model in which only the
shoulder joint is actuated which will be denoted by 119877119877 andthe model in which only the elbow joint is actuated whichwill be denoted by 119877119877
Underactuated robots are mechanical systems with sec-ond-order nonholonomic constraints because the dynamicequation of the unactuated part of the mechanical systemis a second-order differential constraint which in general isnonintegrable It is not possible to integrate even partially thissecond-order differential constraint in the dynamic modelof the 119877119877 robot manipulator However in the presence ofthis second-order nonholonomic constraint the system iscontrollable On the contrary in the dynamicmodel of the119877119877robot manipulator this differential constraint is completelyintegrable It can be converted into a holonomic constraintthatmakes the system not controllable As a consequence thetrajectory planning problem has solutions only for particularinitial and final states
Planning of dynamically feasible trajectories their asy-mptotic tracking and the regulation to a desired equilibriumconfiguration are the main control problems for this class
2 Abstract and Applied Analysis
of mechanical systems However a general control theoryfor this class of mechanical systems has not been developedyet and only solutions for particular robot models have beenobtained A review of the most significant works on controlof underactuated robots with passive joints can be found in[1]
In this paper the trajectory planning problem for planarhorizontal underactuated 119877119877 robot manipulators is studiedas an energy-optimal control problem Given the initial andfinal states we find the available control inputs and thecorresponding trajectory that satisfy the dynamic equationof the robot manipulator and steer the system between initialand final states minimizing the energy consumption duringthe motion This problem is referred to as boundary valueproblemWe also consider initial value problems inwhich thefinal state is not completely specified Our numerical methodcan also tackle final value problems inwhich part of the initialstate is not specified
It is usually impossible to find analytical solutions to opti-mal control problems of robot manipulators and in generalnumerical methods must be employed To solve our optimalcontrol problem we apply a numerical method that fallsinto the class of indirect methods which are based on first-order necessary optimality conditions More precisely it is avariational approach in which the optimal control problemis transformed into an unconstrained calculus of variationsproblem by means of special derivative multipliers [2 Chap-ter VII]
Our method substantially differs from the usual indirectapproaches in which the Euler-Lagrange differential equationis solved using a suitable numerical method To compute theextremals we use the Euler-Lagrange necessary conditionin integral form plus transversality conditions to take intoaccount the components of state and control variables andmultipliers that are not specified at the endpoints [3 Chapter6] In thiswaywe avoid the loss of information induced by theuse of the differential form of the same condition that impliesnumerical corner conditions and the necessity of patchingtogether solutions between corners
Moreover in our method the control inputs are deriva-tives of some components of the extended state vector whichcan be piecewise continuous functions while the originalstate vector is supposed to be composed by piecewise smoothfunctions Similarly the multipliers which are the derivativesof other components of the extended state vector need only tobe piecewise continuous In our approach time is discretizedand admissible variations for each variable are approximatedby linear combinations of piecewise continuous basis func-tions of time In this way variations depend on the valuesof the coefficients at the mesh points The conditions underwhich the objective functional is stationary with respect toall piecewise smooth variations that satisfy the boundaryconditions are derived and the set of nonlinear differenceequations thatmust be satisfied by the coefficients is obtainedThis set of equations is then solved using the Newton-Raphson method This basic procedure can be modified toincorporate equality and inequality constraints by means ofderivative multipliers and derivative excess variables
In [4] the necessary conditions for optimal control arederived using the ideas of Lagrangian reduction that is reduc-tion under a symmetry group The techniques presented inthis work are designed for Lagrangianmechanical holonomicand nonholonomic systems with symmetry The key idea isto link the method of Lagrange multipliers with Lagrangianreduction as an alternative to the Pontryagin MaximumPrinciple and Poisson reduction [5 Chap 7] is devoted tooptimal control of nonholonomic mechanical systems Therelationship between variational nonholonomic control sys-tems and the classical Lagrange problem of optimal controlis presented Then kinematic and dynamic optimal controlproblems are discussed whereas related work on integrablesystems is studied in the Internet supplement of this bookIn [6] an affine connection formulation is used to studyan optimal control problem for a class of nonholonomicunderactuated mechanical systems The class of nonholo-nomic systems studied in this paper are wheeled vehicleThe nonholonomic affine connection together with Lagrangemultiplier method in the calculus of variations is used toderive the optimal necessary conditions
The mechanical systems studied in this paper havesimilarities with the Pendubot and the Acrobot which areunderactuated two-link 119877119877 robot manipulators that movein a vertical plane and therefore are subjected to gravityforce In the Pendubot only the shoulder joint is actuatedwhereas in the Acrobot only the elbow joint is actuatedThe control objective is usually in both cases to drive themanipulator away from the straight-down position and steerit at the straight-up position In [7] a unified strategy formotion control of underactuated two-link manipulators withgravity such as the Acrobot and the Pendubot is presentedFirst a control law is employed to increase the energyand control the posture of the actuated link in the swing-up region Finally an optimal control law is designed forthe attractive region using a linear approximation modelof the system around the straight-up position In [8] ageneral control methodology for swinging up and stabilizingunderactuated two-link robots is presented It is based onEuler-Lagrange dynamics passivity analysis and dynamicprogramming theory In [9] two different approaches forfeedforward control design are presented The first approachis based on a coordinate transformation into the nonlinearinput-output normal formwhereas the second approach usesservo constraints and results in a set of differential algebraicequations
To the best knowledge of the authors the optimal controlof underactuated 119877119877 robot manipulators without gravity andwithout breaks has not been addressed
This paper is organized as follows In Section 2 thedynamic models of the two planar horizontal underactuated119877119877 robot manipulators are described and in Section 3their control properties are discussed The optimal controlproblem for these dynamic systems is stated in Section 4 InSection 5 a reformulation of the optimal control problems asa calculus of variations problem is presented and in Section 6the proposed numerical method to solve the resulting calcu-lus of variations problem is described In Section 7 the resultsof the application of this numericalmethod to several optimal
Abstract and Applied Analysis 3
Link 1
Link 2
Joint 1
Joint 2
119910
119909
1205792
1205791
1199032
1199031
1198972
1198971
Figure 1 An underactuated two-link robot manipulator that movesin a horizontal plane in which only one of the joints is actuated
control problems for planar horizontal underactuated 119877119877
robot manipulators are reported Finally Section 8 containsthe conclusions
2 Dynamic Model of UnderactuatedManipulators
The general dynamic model of a robot manipulator isdescribed by the following second-order differential equation
where the first term of this equation 119861(120579) 120579 represents theinertial forces due to acceleration at the joints and the secondterm 119862(120579 120579) 120579 represents the Coriolis and centrifugal forcesThe third term 119865 120579 is a simplified model of the frictionin which only the viscous friction is considered The term119890(120579) represents the potential forces such as elasticity andgravity Matrix119866(120579) on the right-hand side maps the externalforcestorques 119908 to forcestorques at the joints Finally 119906represents the forcestorques at the joints that are the controlvariables of the system
We suppose that the links are rigid as well as the trans-mission elements and that the robot moves in a horizontalplane in such a way the gravity does not affect the dynamicsof the manipulator Finally we do not take into account theeffects of the friction and we suppose that no external forcesare acting on themechanical systemUnder these hypothesesthe dynamic model of the robotic system reduces to
A horizontal planar 119877119877 manipulator is composed of twohomogeneous links and two revolute joints moving in ahorizontal plane 119909 119910 as shown in Figure 1 where 119897
119894is the
length of link 119894 119903119894is the distance between joint 119894 and the
mass center of link 119894 119898119894is the mass of link 119894 and 119868
119911119894is the
barycentric inertia with respect to a vertical axis 119911 of link 119894
for 119894 = 1 2 In this case the two matrices 119861(120579) and 119862(120579 120579)have the form
119861 (120579) = [120572 + 2120573 cos 120579
2120575 + 120573 cos 120579
2
120575 + 120573 cos 1205792
120575]
119862 (120579 120579) = [minus120573 sin 120579
21205792minus120573 sin 120579
2( 1205791+ 1205792)
120573 sin 12057921205791
0]
(3)
where 120579 = (1205791 1205792)119879 is the vector of configuration variables
being 1205791the angular position of link 1 with respect to the 119909
axis of the reference frame 119909 119910 and 1205792the angular position
of link 2 with respect to link 1 as illustrated in Figure 1 Thevector 120579 = ( 120579
1 1205792)119879 is the vector of angular velocities and
120579 = ( 1205791 1205792)119879 is the vector of accelerations The control inputs
of the system are 119906 = (1199061 1199062)119879 where 119906
1is the torque applied
by the actuator at joint 1 and 1199062is the torque applied by the
actuator at joint 2 The parameters 120572 120573 and 120575 in (3) have thefollowing expressions
A robot manipulator is said to be underactuated whenthe number of actuators is less than the degree of freedom ofthe mechanical systemThe dynamic model (2) that does notconsider the effects of gravity and friction can be rewrittenfor a 119877119877 robot manipulator underactuated by one control inthe form [1]
[119861119886119886 (120579) 119861
119906119886 (120579)
119861119906119886 (120579) 119861
119906119906 (120579)](
120579119886
120579119906
) + [119862119886(120579 120579)
119862119906(120579 120579)
] = (119906119886
0) (5)
in which the state variables 120579119886and 120579
119906correspond to the
actuated and unactuated joints and 119906119886is the available control
input The last equation of (5) describes the dynamics of theunactuated part of the mechanical system and has the form
119861119906119886 (120579)
120579119886+ 119861119906119906 (120579)
120579119906+ 119862119906(120579 120579) = 0 (6)
which is a second-order differential constraint without inputvariables
Underactuated manipulators may be equipped withbrakes at the passive joints Hybrid optimal control strategiescan be designed in this case [10 11] The presence of brakeswill not be considered in this paper
3 Control Properties of Underactuated RRRobot Manipulators
In this section the main control properties of planar under-actuated 119877119877 robot manipulators without the effects of thegravity will be described For a more general description ofthe control properties of underactuated robot manipulatorssee [1]
4 Abstract and Applied Analysis
Optimal control approaches to trajectory planning ass-ume that there exists a control input that steers the systembetween two specify states Thus controllability is the mostimportant aspect to check before studying optimal control ofa dynamic system If in the trajectory planning problem theduration of the motion 119879 is not assigned the existence of afinite-time solution for any state (120579
119865 120579119865) in a neighborhood
of (120579119868 120579119868) is equivalent for the robotic system to the property
of local controllability at (120579119868 120579119868) If local controllability holds
at any state then the system is controllable and the trajectoryplanning problem is solvable for any pair of initial and finalstates
For underactuated119877119877 robotmanipulators controllabilityis related to integrability of the second-order nonholonomicconstraints The second-order differential constraint (6) mayeither be partially integrable to a first-order differentialequation or completely integrable to a holonomic equationNecessary and sufficient integrability conditions are givenin [12 13] If (6) is not partially integrable it is possible tosteer the system between equilibrium points This occursfor planar underactuated 119877119877 robot manipulators withoutgravity which therefore are controllable If (6) is completelyintegrable to a holonomic constraint the motion of themechanical system is restricted to a 1-dimensional sub-manifold of the configuration space which depends on theinitial configuration This occurs for the dynamic equationsof planar underactuated 119877119877 robot manipulators withoutthe effects of the gravity [12] For this robot model thetrajectory planning problem has solution only for particularinitial and final states Thus when (6) is not partially orcompletely integrable the mechanical system is controllableHowever several aspects of controllability can be studiedwhich characterize this model of underactuatedmanipulator
A dynamical system is linearly controllable at an equi-librium point if the linear approximation of the systemaround this point is controllable Planar underactuated 119877119877robot manipulators in the absence of gravity are not linearlycontrollable On the contrary both planar underactuated 119877119877and 119877119877 robot manipulators are linearly controllable in thepresence of gravity
A mechanical system is said to be small-time locallycontrollable (STLC) at 119909
119868= (120579119868 120579119868) if for any neighborhood
V119909of 119909119868and any time 119879 gt 0 the set RV119909 119879
(119909119868) of states
that are reachable from 119909119868within time 119879 along trajectories
contained in V includes a neighborhood of 119909119868 Note that
small-time local controllability is a stronger property thancontrollability [14] Non-STLC but controllable system mustin general perform finite maneuvers in order to performarbitrarily small changes of configuration It has been provenin [15 16] that planar underactuated 119877119877 robot manipulatorsin the absence of gravity are not STLC Both planar under-actuated 119877119877 and 119877119877 robot manipulators in the presence ofgravity are also not STLC
Second-order mechanical systems cannot be STLC atstates with nonzero velocity Therefore the weaker conceptof small-time local configuration controllability has beenintroduced [17] A system is said to be small-time local
configuration controllable (STLCC) at a configuration 120579119868if
for any neighborhood V120579of 120579119868in the configuration space
and any time 119879 gt 0 the set RV120579119879(120579119868) of configurations that
are reachable (with some final velocity 120579) within 119879 startingfrom (120579
119868 0) and along a path in configuration space contained
in V120579 includes a neighborhood of 120579
119868 By definition STLC
systems are also STLCC Sufficient conditions for STLCC
are given in [17] It has been proven in [15 16] that planarunderactuated 119877119877 robot manipulators in the absence ofgravity are not STLCC Both planar underactuated 119877119877 and119877119877 robot manipulators in the presence of gravity are also notSTLCC
A final question is to investigate if the trajectory planningproblem for119877119877planar underactuated robotmanipulators canbe solved with algorithmic methods A mechanical systemis kinematically controllable (KC) if every configuration isreachable by means of a sequence of kinematic motions thatis feasible paths in the configuration space which may befollowed with any arbitrary timing law [15 16 18] Note thatKC mechanical systems are also STLCC and that kinematiccontrollability does not imply small-time local controllabilityIf amechanism isKC the trajectory planning problemmaybesolved with algorithmic methods Planar underactuated 119877119877robotmanipulators in the absence of gravity are notKC Bothplanar underactuated 119877119877 and 119877119877 robot manipulators in thepresence of gravity are not KC [15]
4 The Optimal Control Problem
Given the dynamic equation of an underactuated planar119877119877 robot manipulator an initial state (120579
119868 120579119868) and a final
state (120579119865 120579119865) the optimal control problem consists in finding
the available control input 1199061(119905) or 119906
2(119905) and the resulting
trajectory with 119905 isin [119905119868 119905119865] that steers the system between
initial and final states satisfying the dynamic equation (5) andminimizing the objective functional
119869 = int
119905119865
119905119868
1199062
119894119889119905 (7)
where 119894 = 1 2 depending on which joint is actuated and119905119868and 119905119865are the initial and final time values respectively
This cost functional represents a measure of the energyconsumed during the motion since torque produced withan electromechanical actuator is approximately proportionalto the current flow and the rate of energy consumption isapproximately equal to the square of this current
If 120579119868= 120579119865= 0 the problem is called rest-to-rest trajectory
planning problem If the final or the initial states or part ofthem is not assigned the problems are called initial valueproblem and final value problem respectively The final time119905119865may be fixed or notThis problem is a particular case of an optimal control
problem which can be stated in a more general form asfollows Minimize the integral
where 119909(119905) = (1199091(119905) 1199092(119905) 119909
119899(119905))119879 is an 119899-vector called
the state vector 119906(119905) = (1199061(119905) 1199062(119905) 119906
119898(119905))119879 is an 119898-
vector called the control vector the real-valued function119869(119909 119906) is the objective functional (9) is called the trajectoryequation and the conditions (14) are called the boundaryconditions The set 119880 sub R119898 is called the set of controls with119906(119905) isin 119880 for every 119905 isin [119905
119868 119905119865] We assume that 119891 119892 ℎ 119897 119901
and 119902 are sufficiently smooth for our purposeThis will implysolutions such that 119909(119905) is piecewise smooth whereas 119906(119905) ispiecewise continuous [19]
5 Variational Reformulation of the OptimalControl Problem
A variational approach has been used to solve the more gen-eral optimal control problem stated in the previous section
The classical calculus of variations problem is tominimizean integral of the form
where the independent 119909 variable is assumed to be in theinterval [119886 119887] and the dependent variable 119910 = 119910(119909) = (119910
1(119909)
1199102(119909) 119910
119899(119909))119879 is assumed to be an 119899-vector continuous on
[119886 119887] with derivative 1199101015840 = 1199101015840(119909) = (11991010158401(119909) 1199101015840
2(119909) 119910
1015840
119899(119909))119879
It is also assumed that 119910 is piecewise smooth that is thereexists a finite set of points 119886
1 1198862 119886
119896so that 119886 le 119886
1lt 1198862lt
sdot sdot sdot lt 119886119896le 119887 119910(119909) is continuously differentiable on (119886
119897 119886119897+1)
and that the respective left- and right-handed limits of 1199101015840(119909)exist If 119910(119909) is piecewise smooth and satisfies the boundaryconditions 119910(119886) = 119860 119910(119887) = 119861 then 119910(119909) is said to be anadmissible arc In words this problem consists in findingamong all arcs connecting end points (119886 119860) and (119887 119861) theone minimizing the integral (16)
The main optimality conditions are obtained by defininga variation 119911(119909) a set of functions
where 120575 gt 0 is a fixed real number and the variation 119911(119909) isa piecewise smooth function with 119911(119886) = 119911(119887) = 0 Usinga Taylor series expansion it is easy to see that a necessarycondition that 0 is a relative minimum to 119865 is
where 119891119910 1198911199101015840 denote the partial derivatives of 119891 evaluated
along (119909 119910(119909) 1199101015840(119909)) and the terms 119911 and 1199111015840 are evaluated at119909
Integrating (20) by parts for all admissible variations 119911(119909)another necessary condition for 119910 = 119910(119909) to give a relativeminimum of the variational problem (16)-(17) is obtainedwhich is the following second-order differential equation
known as Euler-Lagrange conditionThis equationmust holdalong (119909 119910(119909) 1199101015840(119909)) except at a finite number of points [3Section 21]
The extremals of (16)-(17) can be obtained by solving theEuler-Lagrange equation but it only holds at points where theextremal 119910lowast(119909) is smooth At points where 119910lowast1015840(119909) has jumpscalled corners the Weierstrass-Erdmann corner conditionsmust be fulfilled [3 Section 23] Since the location of thecorners their number and the amplitudes of the jumps in1199101015840lowast(119909) 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 ourmethodis that the integral form of this condition
holds for all 119909 isin [119886 119887] and some 119888 and therefore theWeierstrass-Erdmann corner conditions are not neededThus an alternative way of computing the extremals can bebased on this necessary condition in integral form
Note that necessary condition requires that boundaryvalues fulfill Euler-Lagrange equation Thus if some of thefour values 119886 119910(119886) 119887 and 119910(119887) are not explicitly givenalternate boundary conditions have to be provided This iswhat transversality conditions do Assume that 119886 119910(119886) and 119887are given but119910(119887) is free In this case the additional necessarytransversality condition
1198911199101015840 (119887 119910
lowast(119887) 119910
1015840lowast(119887)) = 0 (23)
must holdThe variational approach does not consider constraints
However the optimal control problem has at least a first-order differential constraint (9) representing the dynamicequation of the system Moreover since the dynamic equa-tion of a planar 119877119877 robot manipulator is a second-order
6 Abstract and Applied Analysis
differential equation additional differential constraints willarise while rewriting it as a first-order differential equationTherefore the optimal control problemmust be reformulatedas an unconstrained calculus of variations problem in orderto deal with differential and algebraic constraints as describedin the following section
Following [3 Chapter 5] we reformulate as an uncon-strained calculus of variations problem the optimal controlproblem consisting inminimizing (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 variablefrom 119905 to 119909 and the dependent variable from 119909 to 119910 to beconsistent with the notation of calculus of variations Ourreformulation is based on special derivative multipliers anda change of variables in which
1199101(119909) = 119910(119909) is the renamed state vector
1199101015840
2(119909) = 119906(119909) is the renamed state vector
1199101015840
3(119909) is the multiplier associated with (9)
1199101015840
4(119909) is themultiplier associated with constraint (10)
1199101015840
5(119909) is the multiplier associated with constraint (11)
1199101015840
6(119909) is the excess variable of constraint (11)
Since 1199102(119909) 119910
6(119909) are not unique without an extra condi-
tion we initialize these variables by defining 119910119894(119909119868) = 0 119894 =
1 6 Thus our problem becomes
min 119868 (Y) = int119909119865
119909119868
119865 (119909YY1015840) 119889119909 (24)
where
Y = (1199101 1199102 1199103 1199104 1199105 1199106)119879
119865 = 119891 (119909 1199101 1199101015840
2) + 1199101015840119879
3(1199101015840
1minus 119892 (119909 119910
1 1199101015840
2))
+ 1199101015840119879
4ℎ (119909 119910
1 1199101015840
2) + 1199101015840119879
5(119897 (119909 119910
1 1199101015840
2) + 11991010158402
6)
(25)
Since the values of 119910119894(119909119865) 119894 = 2 6 are unknown
transversality conditions are needed having the form
In the above lines 119910(119909) is an 119899-vector and120595 120593 120601 are assumedto be differentiable in their arguments or with the neededsmoothness We also assume that 120595
11991010158401199101015840 gt 0 The boundary
conditions of the problems are any combination of fixedboundary conditions for the components of 119910 with thepossibility of leaving some of them unspecified
If we reformulate problem (27)-(28) using the techniquedescribed in Section 5 we get the following HamiltonianΨ(119909 119884 119884
1015840) = 120595(119909 119910
1 1199101015840
1) + 1199101015840
2120593(119909 119910
1) with 119910
1(119909) = 119910(119909)
where 11991010158402is the multiplier We have in this case
Ψ11988410158401198841015840 = [
1205951199101015840
11199101015840
1
0
0 0] (31)
which is singular The singularity of Ψ11988410158401198841015840 is a difficulty we
must avoid Furthermore even when it is not difficult tochange from the120593 constraint to the120601 constraint by increasingthe dimension of the independent variables it is not easy todeal with the new associated boundary conditionsThis is thereason that problem (27)-(28) is so difficult to solve
It has been shown in [20] that problem (27)-(28) can bereformulated as an equivalent problem of the form (29)-(30)In particular 119910(119909) is a solution to (27)-(28) if and only if 119910(119909)is a solution to
The numerical method used is based on the discretizationof the unconstrained variational calculus problem stated inthe previous section In particular the main underlying ideais obtaining a discretized solution 119910
ℎ(119909) solving (20) for all
piecewise linear spline function variations 119911(119909) instead of
Abstract and Applied Analysis 7
dealing with the Euler-Lagrange equation (21) Thus thismethod uses no numerical corner conditions and avoidspatching solutions to (21) between corners
Let 119873 be a large positive integer ℎ = (119887 minus 119886)119873 and let120587 = (119886 = 119886
0lt 1198861lt sdot sdot sdot lt 119886
119873= 119887) be a partition of the
interval [119886 119887] where 119886119896= 119886 + 119896ℎ for 119896 = 0 1 119873 Define
the one-dimensional spline hat functions
119908119896 (119909) =
119909 minus 119886119896minus1
ℎif 119886119896minus1
lt 119909 lt 119886119896
119886119896+1
minus 119909
ℎif 119886119896lt 119909 lt 119886
119896+1
0 otherwise
(35)
where 119896 = 1 2 119873 minus 1 Define also the 119898-dimensionalpiecewise linear component functions
119910ℎ (119909) =
119873
sum
119896=0
119882119896 (119909) 119862119896 119911
ℎ (119909) =
119873
sum
119896=0
119882119896 (119909)119863119896 (36)
where 119882119896(119909) = 119908
119896(119909)119868119898times119898
119910ℎ(119909) is the sought numerical
solution and 119911ℎ(119909) is a numerical variation In particular the
constant vectors 119862119896are to be determined by the algorithm
developed by us and the constant vectors 119863119896are arbitrary
Thus the discretized form of (20) is obtained in eachsubinterval [119886
119896minus1 119886119896+1] For the sake of clarity of exposition
we assume that119898 = 1 Note that 1198681015840(119910 119911) in (20) is linear in 119911so that a three-term relationship may be obtained at 119909 = 119886
119896
by choosing 119911(119909) = 119908119896(119909) for 119896 = 1 2 119873 minus 1 Thus
In these equations 119886lowast119896= (119886119896+ 119886119896+1)2 and 119910
119896= 119910ℎ(119886119896) is
the computed value of 119910ℎ(119909) at 119886
119896 In the general case when
119898 gt 1 the same result is obtained but 1198911199101015840 and 119891
119910are
column 119898-vectors of functions with 119894th component 1198911199101198941015840 and
119891119910119894 respectively Similarly (119910
119896+119910119896minus1)2 is the119898-vector which
is the average of the119898-vectors 119910ℎ(119886119896) and 119910
ℎ(119886119896minus1)
By the same arguments that led to (38)
1198911199101015840 (119886lowast
119873minus1119910119873+ 119910119873minus1
2119910119873minus 119910119873minus1
ℎ)
+ℎ
2119891119910(119886lowast
119896minus1119910119873+ 119910119873minus1
2119910119873minus 119910119873minus1
ℎ) = 0
(39)
which is the numerical equivalent of the transversality condi-tion (23) For further details see [3 Chapter 6]
It has been shown in [21] that with this method the globalerror has a priori global reduction ratio of119874(ℎ2) In practiceif the step size ℎ is halved the error decreases by 4
7 Implementation and Results
Several numerical experiments have been carried out forboth 119877119877 and 119877119877 planar horizontal underactuated robotmanipulators
71 Planar Horizontal Underactuated 119877119877 Robot ManipulatorIn this section the optimal control problem of a planar hor-izontal underactuated 119877119877 is studied In this robot model thesecond joint is not actuated thus 119906 = (119906
1 0)119879 In this case
it is neither possible to integrate partially nor completely thenonholonomic constraint because the manipulator inertiamatrix contains terms in 120579
2(see [12]) Hence the system is
controllable The numerical results of the application of ourmethod for optimal control to a boundary value problem andto an initial value problem for this system will be described
For a planar horizontal underactuated119877119877 (2) can be splitinto
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792
minus 120573 sin 1205792(2 12057911205792+ 1205792
2) = 1199061
(120575 + 120573 cos 1205792) 1205791+ 120575 1205792+ 120573 sin 120579
21205792
1= 0
(40)
To express optimal control problems that involve this second-order differential constraints in the form of a basic optimal
8 Abstract and Applied Analysis
control problem we have first to convert it into first-orderdifferential constraints introducing the following change ofvariables
1199091= 1205791 119909
3= 1205791
1199092= 1205792 119909
4= 1205792
(41)
with the following additional relations
1199091015840
1= 1199093
1199091015840
2= 1199094
(42)
Thus the second-order differential equations (40) are con-verted into the first-order differential equations
(120572 + 2120573 cos1199092) 1199091015840
3+ (120575 + 120573 cos119909
2) 1199091015840
4
minus 120573 sin1199092(211990931199094+ 1199092
4) = 1199061
(43)
(120575 + 120573 cos1199092) 1199091015840
3+ 1205751199091015840
4+ 120573 sin119909
21199092
3= 0 (44)
Relations (42) (43) and (44) are now the differential con-straints of the optimal control problem and the objectivefunctional to minimize is
119869 = int
119905119865
119905119868
1199062
1119889119905 (45)
Then we introduce the following new variables
X = [
[
1198831
sdot sdot sdot
1198839
]
]
(46)
such that
119883119894= 119909119894 119894 = 1 4 (47)
1198831015840
5= 1199061 119883
5(119905119868) = 0 (48)
where 11988310158406with 119883
6(119905119868) = 0 is the multiplier associated with
whereas in the initial value problem 119883119894(119905119865) will be free for
some 119894The initial values of control variables and multipliers
have been set to zero whereas their final values have notbeen assigned in both optimal control problems Thereforetransversality conditions are needed in both cases for thevariables119883
119894(119905119865) 119894 = 5 10 and they will be of the form
Figure 2 Sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 corresponding to the optimal solutionof problem 1 a boundary value problem for the planar 119877119877 robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad]
1205791(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads] obtained with a
discretization of [119905119868 119905119865] into 64 subintervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control and
state variables are represented in Figure 3
The initial values of control variable and of the multipliershave been set to zero whereas their final values are left freeFigure 2 shows the sequence of configurations of the robot attimes 119905 = 11989632 119896 = 0 1 32 Since the configurations ofthe sequence overlap it has been split into smaller sequencesfor a better visualization of the manipulator motion Figure 3depicts the corresponding control and state variables of theoptimal solution of this boundary value problem obtainedwith a discretization of the time interval [119905
119868 119905119865] into 64
subintervals The value of the objective functional for thissolution is 345185 [J]
712 Problem 2 Initial Value Problem An initial valueproblem has also been solved with the following initial andfinal conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) =
120587
2[rad]
1205792(119905119868) = minus
120587
2[rad] 120579
2(119905119865) = minus
120587
2[rad]
1205791(119905119868) = 0 [rads] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(55)
The initial values of control variable and of the multipliershave been set to zero whereas their final values are left freeThe only difference between these conditions and those of theboundary value problem described in Section 711 is that now1205791(119905119865) = free
Figures 4 and 5 depict the sequence of configurations the119877119877 robot manipulator and the corresponding control andstate variables of the optimal solution of this initial valueproblem respectively obtained with a discretization of thetime interval [119905
119868 119905119865] into 64 subintervals The value of the
objective functional for this solution is 56472 [J]This value islower than the value of the objective functional of the solutionof the boundary value problem described in Section 711because now is 120579
1(119905119865) = free and the control system does
not have to spend energy to stop it
72 Planar Horizontal Underactuated 119877119877 Robot ManipulatorIn this section the optimal control problem of a planarhorizontal underactuated 119877119877 robot manipulator is studiedIn this robot model the first joint is not actuated thus 119906 =
(0 1199062)119879 and (2) can be split into
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792
minus 120573 sin 1205792(2 12057911205792+ 1205792
2) = 0
(56)
(120575 + 120573 cos 1205792) 1205791+ 120575 1205792+ 120573 sin 120579
21205792
1= 1199062 (57)
As explained in [12] since gravity terms are all zero and1205791does not intervene in the system inertia matrix (56) can
be partially integrated to
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792+ 1198881= 0 (58)
Actually constraint (56) is completely integrable giving rise toan holonomic constraintThe resulting holonomic constrainttakes different forms depending on the value of 119888
1which
depends on the initial conditions Therefore two cases havebeen considered
(i) when the initial velocities 1205791(119905119868) and 120579
2(119905119868) are both
zero(ii) when the initial velocity 120579
1(119905119868) is nonzero
721 Problem 3 Initial Value Problem with Zero Initial Veloc-ities An initial value problem has been solved with the fol-lowing initial and final conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rad] 120579
2(119905119865) = 120587 [rad]
1205791(119905119868) = 0 [rads] 120579
1(119905119865) = 0 [rads]
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(59)
10 Abstract and Applied Analysis
10 20 30 40 50 60
05
1
15
2
25
(a) 1205791
10 20 30 40 50 60minus05
minus1
minus15
minus2
minus25
minus3
(b) 1205792
10 20 30 40 50 60
minus10
minus5
5
10
(c) 1205791
10 20 30 40 50 60
minus10
minus5
5
10
15
(d) 1205792
10 20 30 40 50 60
minus300
minus200
minus100
100
200
300
(e) 1199061
Figure 3 Control and state variables of the optimal solution of problem 1 a boundary value problem for the planar 119877119877 robot manipulatorwith boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 2
Figure 4 Sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 corresponding to the optimal solutionof problem 2 an initial value problem for an 119877119877 robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) =
0 [rads] 1205792(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained with a discretization of
[119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control and state variables are
represented in Figure 5
Abstract and Applied Analysis 11
10 20 30 40 50 60
05
1
15
2
25
(a) 1205791
10 20 30 40 50 60
minus25
minus2
(b) 1205792
10 20 30 40 50 60
minus4
minus2
2
4
6
8
10
(c) 1205791
minus4
minus6
minus210 20 30 40 50 60
2
4
(d) 1205792
10 20 30 40 50 60minus50
50
100
150
(e) 1199061
Figure 5 Control and state variables of the optimal solution of problem 2 an initial value problem for a119877119877 robotmanipulator with boundaryconditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = free
and 1205792(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of configurations of the
robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 4
The initial values of the control variable and of themultipliershave been set to zero whereas their final value is left free
Since there is a holonomic constraint that relates thevalues of the angles 120579
1and 120579
2 without integrating (58) we
are not able to find the value of 1205791(119905119865) consistent with 120579
1(119905119868)
Therefore no final conditions have been imposed on 1205791
From the initial conditions of the problem we obtain 1198881=
0 Equation (58) with 1198881= 0 corresponds to the homogeneous
differential constraint
119889120577 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792= 0 (60)
The differential 119889120577 is not exact However it becomes an exactdifferential if multiplied by the factor 1(120572 + 2120573 cos 120579
2) This
operation does not alter the differential equation (60) Inthis case there does exist a function 120577 whose differentialcoincides with the expression 119889120577(120572 + 2120573 cos 120579
2) Due to
the existence of this function the integral of 119889120577 between
two points depends only on these points and not on theintegration path Equation (60) rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2)
120572 + 2120573 cos 1205792
1198891205792
(61)
can be integrated by separating variables The correspondingholonomic constraint has the following expression
1205791= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1205792
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1205792
2+ 1198882
(62)
To express this optimal control problem in the form ofa basic optimal control problem we first have to convert (57)
12 Abstract and Applied Analysis
into a first-order differential model introducing the followingchange of variables
1199091= 1205791 119909
3= 1205791
1199092= 1205792 119909
4= 1205792
(63)
with the following additional relations
1199091015840
1= 1199093
1199091015840
2= 1199094
(64)
Thus the optimal control problem is to minimize
int
1
0
1199062
2119889119905 (65)
subject to the constraints1199091
= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1199092
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1199092
2+ 1198882
(66)
(120575 + 120573 cos 1205792) 1199091015840
3+ 1205751199091015840
4+ 120573 sin119909
21199092
3= 1199062 (67)
and the additional constraints (64) To reformulate thisoptimal control problem as an unconstrained calculus ofvariations problem let X be
X = [
[
1198831
sdot sdot sdot
1198839
]
]
(68)
such that119883119894= 119909119894 119894 = 1 4
1198831015840
5= 1199061 119883
5(119905119868) = 0
(69)
where 11988310158406with 119883
6(119905119868) = 0 is the multiplier associated with
the holonomic constraint (66) 11988310158407with 119883
7(119905119868) = 0 is the
multipliers associated with the differential constraint (67)and 1198831015840
8with 119883
8(119905119868) = 0 and 1198831015840
9with 119883
9(119905119868) = 0 are the
multipliers associatedwith the additional equality constraints(64)
Thus the holonomic constraint of the problem can berewritten as follows
120593 (119905X)
= 1198831minus((120572 minus 2120575)
times tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1198832
2)]]
]
)
times (radic41205732 minus 1205722)
minus1
+1198832
2= 0
(70)
Now the technique described in Section 51 to deal withholonomic constraints can be applied to 120593(119905X) and thisholonomic constraint is replaced by
120593119905+ 1205931198831198831015840= 0
120593 (0 119883 (0)) = 0
(71)
From the initial conditions of the problem the latter equationreduces to the equality 0 = 0 whereas the former takes thefollowing form
(120572 + 2120573 cos (1198832))1198833+ (120575 + 120573 cos (119883
2))1198834= 0 (72)
The corresponding Hamiltonian is
1198661= 11988310158402
5+ 1198831015840
6((120572 + 2120573 cos (119883
2))1198833
+ (120575 + 120573 cos (1198832))1198834)
+1198831015840
7(120573 sin (119883
2)1198832
3+ (120575 + 120573 cos (119883
2))1198831015840
3
+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(73)
It is not difficult to check that matrix 1198661X1015840X1015840
is singularThis is due to the fact that to handle our optimal controlproblemwhich involves second-order differential constraintswe converted them into first-order differential constraintsTherefore we apply again the technique of Section 51obtaining the identity 0 = 0 and the following constraint
minus 120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4= 0
(74)
The corresponding Hamiltonian is
1198662= 11988310158402
5+ 1198831015840
6
times (minus120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4)
+ 1198831015840
7(120573 sin (119883
2)1198832
3+(120575+120573 cos (119883
2))1198831015840
3+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(75)
It is not difficult to check that matrix 1198662X1015840X1015840
in this case is notsingular since its determinant is
Substituting the values of120572120573 and 120575 this expression becomesdet(119866
2X1015840X1015840) = 1128(43 minus 2 cos(2119883
2))2 which is always
positive for any real value1198832 Figure 6 shows the sequence of
configurations of the robot at times 11989632with 119896 = 0 1 32and Figure 7 depicts control and state variables of the optimal
Abstract and Applied Analysis 13
Figure 6 Sequence of configurations of the robot manipulator attimes 11989632 with 119896 = 0 1 32 corresponding to the optimal sol-ution of problem 3 an initial value problem for an underactuated119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) =
0 [rad] 1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads]
1205791(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2
(119905119865) = 0 [rads] obtained with a discretization of [119905
119868 119905119865] into 64
subintervals The initial and final times are 119905119868= 0 and 119905
119865=
1 [s] respectively The corresponding control and state variables arerepresented in Figure 7
solution obtained with a discretization of the interval [119905119868 119905119865]
into 64 subintervalsIn particular we get 120579
1(119905119865) = minus110248 [rad] To check the
consistency of this result with the holonomic constraint (62)since 120579
1(119905119868) = 1205792(119905119868) = 0 [rads] we get from (58) that 119888
1= 0
and using the initial condition 1205791(119905119868) = 1205792(119905119868) = 0 [rad] we
get from (62) that 1198882= 0 Having established the value of the
constant 1198882 we obtain from the same equation for 120579
2(119905119865) =
120587 [rad] that 1205791(119905119865) = minus110248 [rad] which coincides with
the value of 1205791(119905119865) obtained numerically
722 Problem 4 Initial Value Problem with Nonzero InitialVelocity 120579
1 Another initial value problem has been solved
with the following conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rad] 120579
2(119905119865) = 120587 [rad]
1205791(119905119868) = 5 [rads] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(77)
The initial values of the multipliers have been set to zerowhereas their final value is left free Notice that no finalconditions have been imposed on 120579
1and 120579
1 The same
considerations done in previous section hold in this caseas well The technique described in Section 51 must beapplied twice leading to the differential constraint (74) andto the Hamiltonian (75) Figure 8 shows the sequence ofconfigurations of the robot at times 11989632with 119896 = 0 1 32and Figure 9 depicts the control and state variables ofthe optimal solution obtained with a discretization of theinterval [119905
119868 119905119865] into 64 subintervals In particular we get that
1205791(119905119865) = 617172 [rad] and 120579
1(119905119865) = 900163 [rads] To
check the consistency of the obtained value of 1205791(119905119865) with
the holonomic constraint consider (58) We can calculate
the constant 1198881using the initial conditions of the problem
obtaining
1198881= minus 120579
1(119905119868) (120572 + 2120573 cos 120579
2(119905119868))
minus 1205792(119905119868) (120575 + 120573 cos 120579
2(119905119868)) = minus225
(78)
Since 1198881= 0 (58) corresponds in this case to the differential
constraint
119889120578 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792+ 1198881119889119905 = 0
(79)
Equation (79) can be rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2 (119905))
120572 + 2120573 cos 1205792 (119905)
1198891205792minus
1198881
120572 + 2120573 cos 1205792 (119905)
119889119905 (80)
To check the obtained value of 1205791(119905119865) 1198891205791is numerically
integrated between 1205791(119905119868) and 120579
1(119905119865) using the interpolated
numerical optimal solution obtained for 1205792(119905) We get that
1205791(119905119865) = 618705 This value is close to 617172
To check the consistency of the obtained value of 1205791(119905119865)
with the constraint (58) using the computed value 1198881= minus225
and the final conditions 1205792(119905119865) = 0 120579
2(119905119865) = 120587 of the problem
we obtain
1205791(119905119865) =
minus (120575 + 120573 cos 1205792(119905119865))
120572 + 2120573 cos 1205792(119905119865)
1205792(119905119865)
minus1198881
120572 + 2120573 cos 1205792(119905119865)= 9
(81)
This value is very close to the value of 1205791(119905119865) obtained
numerically
73 Computational Issues If the optimal control problem has119898 variables and the time interval [119905
119868 119905119865] has been discretized
into 119873 subintervals the resulting set of difference equations(38) has119898times(119873minus1) equations and119898times(119873minus1) variables plusthe equations and variables due to transversality conditionsFeasible solutions have been used as initial guesses of thealgorithm
The solution of the nonlinear system of difference equa-tions (38) has been obtained using a damped Newtonalgorithm within a line search methodology implementedin Mathematica 7 under Mac OS X operating system (see[22 23] for more details)
8 Conclusion
In this paper the trajectory planning problem for planarunderactuated robot manipulators with two revolute jointswithout gravity has been studied This problem is solved asan optimal control problem based on a numerical resolutionof an unconstrained variational calculus reformulation of theoptimal control problem in which the dynamic equation ofthe mechanical system is regarded as a constraint It hasbeen shown that this reformulation method based on special
14 Abstract and Applied Analysis
10 20 30 40 50 60
minus1
minus08
minus06
minus04
minus02
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
minus15
minus1
minus05
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
(d) 1205792
10 20 30 40 50 60
minus15
minus10
5
10
15
minus5
(e) 1199062
Figure 7 Control and state variables of the optimal solution of problem 3 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 119906
2(119905119868) = 0Nm 120579
1(119905119865) = free
1205792(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads]The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectivelyThe corresponding
sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 6
Figure 8 Sequence of configurations of the robot manipulator at times 11989632 with 119896 = 0 1 32 corresponding to the optimal solutionof problem 4 an initial value problem for an underactuated 119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad]
1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained
with a discretization of [119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control
and state variables are represented in Figure 9
Abstract and Applied Analysis 15
10 20 30 40 50 60
1
2
3
4
5
6
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
4
5
6
7
8
9
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
6
(d) 1205792
10 20 30 40 50 60minus5
5
10
15
20
(e) 1199062
Figure 9 Control and state variables of the optimal solution of problem 4 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 8
derivative multipliers is able to tackle both integrable andnonintegrable differential constraints of the dynamic modelsof underactuated planar horizontal robot manipulators withtwo revolute joints This method can be seamlessly appliedin the presence of additional constraints on the mechanicalsystem
References
[1] A De Luca S Iannitti R Mattone and G Oriolo ldquoUnderactu-ated manipulators control properties and techniquesrdquoMachineIntelligence and Robotic Control vol 4 no 3 pp 113ndash125 2002
[2] G A Bliss Lectures on the Calculus of Variations University ofChicago Press Chicago Ill USA 1946
[3] J Gregory and C Lin Constrained Optimization in the Calculusof Variations and Optimal Control theory Chapman amp Hall1996
[4] W-S Koon and J E Marsden ldquoOptimal control for holonomicand nonholonomic mechanical systems with symmetry andLagrangian reductionrdquo SIAM Journal on Control and Optimiza-tion vol 35 no 3 pp 901ndash929 1997
[5] A M Bloch Nonholonomic Mechanics and Control SpringerNew York NY USA 2003
[6] I I Hussein and A M Bloch ldquoOptimal control of underactu-ated nonholonomic mechanical systemsrdquo IEEE Transactions onAutomatic Control vol 53 no 3 pp 668ndash682 2008
[7] X Z Lai J H She S X Yang andMWu ldquoComprehensive uni-fied control strategy for underactuated two-link manipulatorsrdquoIEEE Transactions on Systems Man and Cybernetics B vol 39no 2 pp 389ndash398 2009
[8] J P Ordaz-Oliver O J Santos-Sanchez and V Lopez-MoralesldquoToward a generalized sub-optimal control method of underac-tuated systemsrdquo Optimal Control Applications amp Methods vol33 no 3 pp 338ndash351 2012
16 Abstract and Applied Analysis
[9] R Seifried ldquoTwo approaches for feedforward control andoptimal design of underactuatedmultibody systemsrdquoMultibodySystem Dynamics vol 27 no 1 pp 75ndash93 2012
[10] M Buss O von Stryk R Bulirsch and G Schmidt ldquoTowardshybrid optimal controlrdquo atmdashAutomatisierungstechnik vol 48no 9 pp 448ndash459 2000
[11] M Buss M Glocker M Hardt O von Stryk R Bulirsch andG Schmidt ldquoNonlinear hybrid dynamical systems modelingoptimal control and applicationsrdquo in Modelling Analysis andDesign of Hybrid Systems S Engell G Frehse and E SchniederEds vol 279 of Lecture Notes in Control and InformationScience pp 331ndash335 Springer 2002
[12] G Oriolo and Y Nakamura ldquoControl of mechanical systemswith second-order nonholonomic constraints underactuatedmanipulatorsrdquo in Proceedings of the 30th IEEE Conference onDecision and Control pp 2398ndash2403 December 1991
[13] T J Tarn M Zhang and A Serrani ldquoNew integrability condi-tions for differential constraintsrdquo Systems and Control Lettersvol 49 no 5 pp 335ndash345 2003
[14] H J Sussmann ldquoA general theorem on local controllabilityrdquoSIAM Journal on Control and Optimization vol 25 no 1 pp158ndash194 1987
[15] F Bullo A D Lewis and K M Lynch ldquoControllable kinematicreductions for mechanical systems concepts computationaltools and examplesrdquo in Proceedings of International Symposiumon Mathematical Theory of Networks and Systems 2002
[16] F Bullo and A D Lewis ldquoLow-order controllability and kine-matic reductions for affine connection control systemsrdquo SIAMJournal on Control andOptimization vol 44 no 3 pp 885ndash9082006
[17] A D Lewis and R MMurray ldquoConfiguration controllability ofsimple mechanical control systemsrdquo SIAM Journal on Controland Optimization vol 35 no 3 pp 766ndash790 1997
[18] F Bullo and K M Lynch ldquoKinematic controllability for decou-pled trajectory planning in underactuatedmechanical systemsrdquoIEEE Transactions on Robotics and Automation vol 17 no 4 pp402ndash412 2001
[19] M R Hestenes Calculus of Variations and Optimcl ControlTheory John Wiley amp Sons 1966
[20] J Gregory ldquoA new systematic method for efficiently solvingholonomic (and nonholonomic) constraint problemsrdquo Analysisand Applications vol 8 no 1 pp 85ndash98 2010
[21] J Gregory and R S Wang ldquoDiscrete variable methods forthe m-dependent variable nonlinear extremal problem in thecalculus of variationsrdquo SIAM Journal onNumerical Analysis vol27 no 2 pp 470ndash487 1990
[22] Wolfram Research 2012[23] J J More and D J Thuente ldquoLine search algorithms with guar-
anteed sufficient decreaserdquo ACM Transactions on MathematicalSoftware vol 20 no 3 pp 286ndash307 1994
of mechanical systems However a general control theoryfor this class of mechanical systems has not been developedyet and only solutions for particular robot models have beenobtained A review of the most significant works on controlof underactuated robots with passive joints can be found in[1]
In this paper the trajectory planning problem for planarhorizontal underactuated 119877119877 robot manipulators is studiedas an energy-optimal control problem Given the initial andfinal states we find the available control inputs and thecorresponding trajectory that satisfy the dynamic equationof the robot manipulator and steer the system between initialand final states minimizing the energy consumption duringthe motion This problem is referred to as boundary valueproblemWe also consider initial value problems inwhich thefinal state is not completely specified Our numerical methodcan also tackle final value problems inwhich part of the initialstate is not specified
It is usually impossible to find analytical solutions to opti-mal control problems of robot manipulators and in generalnumerical methods must be employed To solve our optimalcontrol problem we apply a numerical method that fallsinto the class of indirect methods which are based on first-order necessary optimality conditions More precisely it is avariational approach in which the optimal control problemis transformed into an unconstrained calculus of variationsproblem by means of special derivative multipliers [2 Chap-ter VII]
Our method substantially differs from the usual indirectapproaches in which the Euler-Lagrange differential equationis solved using a suitable numerical method To compute theextremals we use the Euler-Lagrange necessary conditionin integral form plus transversality conditions to take intoaccount the components of state and control variables andmultipliers that are not specified at the endpoints [3 Chapter6] In thiswaywe avoid the loss of information induced by theuse of the differential form of the same condition that impliesnumerical corner conditions and the necessity of patchingtogether solutions between corners
Moreover in our method the control inputs are deriva-tives of some components of the extended state vector whichcan be piecewise continuous functions while the originalstate vector is supposed to be composed by piecewise smoothfunctions Similarly the multipliers which are the derivativesof other components of the extended state vector need only tobe piecewise continuous In our approach time is discretizedand admissible variations for each variable are approximatedby linear combinations of piecewise continuous basis func-tions of time In this way variations depend on the valuesof the coefficients at the mesh points The conditions underwhich the objective functional is stationary with respect toall piecewise smooth variations that satisfy the boundaryconditions are derived and the set of nonlinear differenceequations thatmust be satisfied by the coefficients is obtainedThis set of equations is then solved using the Newton-Raphson method This basic procedure can be modified toincorporate equality and inequality constraints by means ofderivative multipliers and derivative excess variables
In [4] the necessary conditions for optimal control arederived using the ideas of Lagrangian reduction that is reduc-tion under a symmetry group The techniques presented inthis work are designed for Lagrangianmechanical holonomicand nonholonomic systems with symmetry The key idea isto link the method of Lagrange multipliers with Lagrangianreduction as an alternative to the Pontryagin MaximumPrinciple and Poisson reduction [5 Chap 7] is devoted tooptimal control of nonholonomic mechanical systems Therelationship between variational nonholonomic control sys-tems and the classical Lagrange problem of optimal controlis presented Then kinematic and dynamic optimal controlproblems are discussed whereas related work on integrablesystems is studied in the Internet supplement of this bookIn [6] an affine connection formulation is used to studyan optimal control problem for a class of nonholonomicunderactuated mechanical systems The class of nonholo-nomic systems studied in this paper are wheeled vehicleThe nonholonomic affine connection together with Lagrangemultiplier method in the calculus of variations is used toderive the optimal necessary conditions
The mechanical systems studied in this paper havesimilarities with the Pendubot and the Acrobot which areunderactuated two-link 119877119877 robot manipulators that movein a vertical plane and therefore are subjected to gravityforce In the Pendubot only the shoulder joint is actuatedwhereas in the Acrobot only the elbow joint is actuatedThe control objective is usually in both cases to drive themanipulator away from the straight-down position and steerit at the straight-up position In [7] a unified strategy formotion control of underactuated two-link manipulators withgravity such as the Acrobot and the Pendubot is presentedFirst a control law is employed to increase the energyand control the posture of the actuated link in the swing-up region Finally an optimal control law is designed forthe attractive region using a linear approximation modelof the system around the straight-up position In [8] ageneral control methodology for swinging up and stabilizingunderactuated two-link robots is presented It is based onEuler-Lagrange dynamics passivity analysis and dynamicprogramming theory In [9] two different approaches forfeedforward control design are presented The first approachis based on a coordinate transformation into the nonlinearinput-output normal formwhereas the second approach usesservo constraints and results in a set of differential algebraicequations
To the best knowledge of the authors the optimal controlof underactuated 119877119877 robot manipulators without gravity andwithout breaks has not been addressed
This paper is organized as follows In Section 2 thedynamic models of the two planar horizontal underactuated119877119877 robot manipulators are described and in Section 3their control properties are discussed The optimal controlproblem for these dynamic systems is stated in Section 4 InSection 5 a reformulation of the optimal control problems asa calculus of variations problem is presented and in Section 6the proposed numerical method to solve the resulting calcu-lus of variations problem is described In Section 7 the resultsof the application of this numericalmethod to several optimal
Abstract and Applied Analysis 3
Link 1
Link 2
Joint 1
Joint 2
119910
119909
1205792
1205791
1199032
1199031
1198972
1198971
Figure 1 An underactuated two-link robot manipulator that movesin a horizontal plane in which only one of the joints is actuated
control problems for planar horizontal underactuated 119877119877
robot manipulators are reported Finally Section 8 containsthe conclusions
2 Dynamic Model of UnderactuatedManipulators
The general dynamic model of a robot manipulator isdescribed by the following second-order differential equation
where the first term of this equation 119861(120579) 120579 represents theinertial forces due to acceleration at the joints and the secondterm 119862(120579 120579) 120579 represents the Coriolis and centrifugal forcesThe third term 119865 120579 is a simplified model of the frictionin which only the viscous friction is considered The term119890(120579) represents the potential forces such as elasticity andgravity Matrix119866(120579) on the right-hand side maps the externalforcestorques 119908 to forcestorques at the joints Finally 119906represents the forcestorques at the joints that are the controlvariables of the system
We suppose that the links are rigid as well as the trans-mission elements and that the robot moves in a horizontalplane in such a way the gravity does not affect the dynamicsof the manipulator Finally we do not take into account theeffects of the friction and we suppose that no external forcesare acting on themechanical systemUnder these hypothesesthe dynamic model of the robotic system reduces to
A horizontal planar 119877119877 manipulator is composed of twohomogeneous links and two revolute joints moving in ahorizontal plane 119909 119910 as shown in Figure 1 where 119897
119894is the
length of link 119894 119903119894is the distance between joint 119894 and the
mass center of link 119894 119898119894is the mass of link 119894 and 119868
119911119894is the
barycentric inertia with respect to a vertical axis 119911 of link 119894
for 119894 = 1 2 In this case the two matrices 119861(120579) and 119862(120579 120579)have the form
119861 (120579) = [120572 + 2120573 cos 120579
2120575 + 120573 cos 120579
2
120575 + 120573 cos 1205792
120575]
119862 (120579 120579) = [minus120573 sin 120579
21205792minus120573 sin 120579
2( 1205791+ 1205792)
120573 sin 12057921205791
0]
(3)
where 120579 = (1205791 1205792)119879 is the vector of configuration variables
being 1205791the angular position of link 1 with respect to the 119909
axis of the reference frame 119909 119910 and 1205792the angular position
of link 2 with respect to link 1 as illustrated in Figure 1 Thevector 120579 = ( 120579
1 1205792)119879 is the vector of angular velocities and
120579 = ( 1205791 1205792)119879 is the vector of accelerations The control inputs
of the system are 119906 = (1199061 1199062)119879 where 119906
1is the torque applied
by the actuator at joint 1 and 1199062is the torque applied by the
actuator at joint 2 The parameters 120572 120573 and 120575 in (3) have thefollowing expressions
A robot manipulator is said to be underactuated whenthe number of actuators is less than the degree of freedom ofthe mechanical systemThe dynamic model (2) that does notconsider the effects of gravity and friction can be rewrittenfor a 119877119877 robot manipulator underactuated by one control inthe form [1]
[119861119886119886 (120579) 119861
119906119886 (120579)
119861119906119886 (120579) 119861
119906119906 (120579)](
120579119886
120579119906
) + [119862119886(120579 120579)
119862119906(120579 120579)
] = (119906119886
0) (5)
in which the state variables 120579119886and 120579
119906correspond to the
actuated and unactuated joints and 119906119886is the available control
input The last equation of (5) describes the dynamics of theunactuated part of the mechanical system and has the form
119861119906119886 (120579)
120579119886+ 119861119906119906 (120579)
120579119906+ 119862119906(120579 120579) = 0 (6)
which is a second-order differential constraint without inputvariables
Underactuated manipulators may be equipped withbrakes at the passive joints Hybrid optimal control strategiescan be designed in this case [10 11] The presence of brakeswill not be considered in this paper
3 Control Properties of Underactuated RRRobot Manipulators
In this section the main control properties of planar under-actuated 119877119877 robot manipulators without the effects of thegravity will be described For a more general description ofthe control properties of underactuated robot manipulatorssee [1]
4 Abstract and Applied Analysis
Optimal control approaches to trajectory planning ass-ume that there exists a control input that steers the systembetween two specify states Thus controllability is the mostimportant aspect to check before studying optimal control ofa dynamic system If in the trajectory planning problem theduration of the motion 119879 is not assigned the existence of afinite-time solution for any state (120579
119865 120579119865) in a neighborhood
of (120579119868 120579119868) is equivalent for the robotic system to the property
of local controllability at (120579119868 120579119868) If local controllability holds
at any state then the system is controllable and the trajectoryplanning problem is solvable for any pair of initial and finalstates
For underactuated119877119877 robotmanipulators controllabilityis related to integrability of the second-order nonholonomicconstraints The second-order differential constraint (6) mayeither be partially integrable to a first-order differentialequation or completely integrable to a holonomic equationNecessary and sufficient integrability conditions are givenin [12 13] If (6) is not partially integrable it is possible tosteer the system between equilibrium points This occursfor planar underactuated 119877119877 robot manipulators withoutgravity which therefore are controllable If (6) is completelyintegrable to a holonomic constraint the motion of themechanical system is restricted to a 1-dimensional sub-manifold of the configuration space which depends on theinitial configuration This occurs for the dynamic equationsof planar underactuated 119877119877 robot manipulators withoutthe effects of the gravity [12] For this robot model thetrajectory planning problem has solution only for particularinitial and final states Thus when (6) is not partially orcompletely integrable the mechanical system is controllableHowever several aspects of controllability can be studiedwhich characterize this model of underactuatedmanipulator
A dynamical system is linearly controllable at an equi-librium point if the linear approximation of the systemaround this point is controllable Planar underactuated 119877119877robot manipulators in the absence of gravity are not linearlycontrollable On the contrary both planar underactuated 119877119877and 119877119877 robot manipulators are linearly controllable in thepresence of gravity
A mechanical system is said to be small-time locallycontrollable (STLC) at 119909
119868= (120579119868 120579119868) if for any neighborhood
V119909of 119909119868and any time 119879 gt 0 the set RV119909 119879
(119909119868) of states
that are reachable from 119909119868within time 119879 along trajectories
contained in V includes a neighborhood of 119909119868 Note that
small-time local controllability is a stronger property thancontrollability [14] Non-STLC but controllable system mustin general perform finite maneuvers in order to performarbitrarily small changes of configuration It has been provenin [15 16] that planar underactuated 119877119877 robot manipulatorsin the absence of gravity are not STLC Both planar under-actuated 119877119877 and 119877119877 robot manipulators in the presence ofgravity are also not STLC
Second-order mechanical systems cannot be STLC atstates with nonzero velocity Therefore the weaker conceptof small-time local configuration controllability has beenintroduced [17] A system is said to be small-time local
configuration controllable (STLCC) at a configuration 120579119868if
for any neighborhood V120579of 120579119868in the configuration space
and any time 119879 gt 0 the set RV120579119879(120579119868) of configurations that
are reachable (with some final velocity 120579) within 119879 startingfrom (120579
119868 0) and along a path in configuration space contained
in V120579 includes a neighborhood of 120579
119868 By definition STLC
systems are also STLCC Sufficient conditions for STLCC
are given in [17] It has been proven in [15 16] that planarunderactuated 119877119877 robot manipulators in the absence ofgravity are not STLCC Both planar underactuated 119877119877 and119877119877 robot manipulators in the presence of gravity are also notSTLCC
A final question is to investigate if the trajectory planningproblem for119877119877planar underactuated robotmanipulators canbe solved with algorithmic methods A mechanical systemis kinematically controllable (KC) if every configuration isreachable by means of a sequence of kinematic motions thatis feasible paths in the configuration space which may befollowed with any arbitrary timing law [15 16 18] Note thatKC mechanical systems are also STLCC and that kinematiccontrollability does not imply small-time local controllabilityIf amechanism isKC the trajectory planning problemmaybesolved with algorithmic methods Planar underactuated 119877119877robotmanipulators in the absence of gravity are notKC Bothplanar underactuated 119877119877 and 119877119877 robot manipulators in thepresence of gravity are not KC [15]
4 The Optimal Control Problem
Given the dynamic equation of an underactuated planar119877119877 robot manipulator an initial state (120579
119868 120579119868) and a final
state (120579119865 120579119865) the optimal control problem consists in finding
the available control input 1199061(119905) or 119906
2(119905) and the resulting
trajectory with 119905 isin [119905119868 119905119865] that steers the system between
initial and final states satisfying the dynamic equation (5) andminimizing the objective functional
119869 = int
119905119865
119905119868
1199062
119894119889119905 (7)
where 119894 = 1 2 depending on which joint is actuated and119905119868and 119905119865are the initial and final time values respectively
This cost functional represents a measure of the energyconsumed during the motion since torque produced withan electromechanical actuator is approximately proportionalto the current flow and the rate of energy consumption isapproximately equal to the square of this current
If 120579119868= 120579119865= 0 the problem is called rest-to-rest trajectory
planning problem If the final or the initial states or part ofthem is not assigned the problems are called initial valueproblem and final value problem respectively The final time119905119865may be fixed or notThis problem is a particular case of an optimal control
problem which can be stated in a more general form asfollows Minimize the integral
where 119909(119905) = (1199091(119905) 1199092(119905) 119909
119899(119905))119879 is an 119899-vector called
the state vector 119906(119905) = (1199061(119905) 1199062(119905) 119906
119898(119905))119879 is an 119898-
vector called the control vector the real-valued function119869(119909 119906) is the objective functional (9) is called the trajectoryequation and the conditions (14) are called the boundaryconditions The set 119880 sub R119898 is called the set of controls with119906(119905) isin 119880 for every 119905 isin [119905
119868 119905119865] We assume that 119891 119892 ℎ 119897 119901
and 119902 are sufficiently smooth for our purposeThis will implysolutions such that 119909(119905) is piecewise smooth whereas 119906(119905) ispiecewise continuous [19]
5 Variational Reformulation of the OptimalControl Problem
A variational approach has been used to solve the more gen-eral optimal control problem stated in the previous section
The classical calculus of variations problem is tominimizean integral of the form
where the independent 119909 variable is assumed to be in theinterval [119886 119887] and the dependent variable 119910 = 119910(119909) = (119910
1(119909)
1199102(119909) 119910
119899(119909))119879 is assumed to be an 119899-vector continuous on
[119886 119887] with derivative 1199101015840 = 1199101015840(119909) = (11991010158401(119909) 1199101015840
2(119909) 119910
1015840
119899(119909))119879
It is also assumed that 119910 is piecewise smooth that is thereexists a finite set of points 119886
1 1198862 119886
119896so that 119886 le 119886
1lt 1198862lt
sdot sdot sdot lt 119886119896le 119887 119910(119909) is continuously differentiable on (119886
119897 119886119897+1)
and that the respective left- and right-handed limits of 1199101015840(119909)exist If 119910(119909) is piecewise smooth and satisfies the boundaryconditions 119910(119886) = 119860 119910(119887) = 119861 then 119910(119909) is said to be anadmissible arc In words this problem consists in findingamong all arcs connecting end points (119886 119860) and (119887 119861) theone minimizing the integral (16)
The main optimality conditions are obtained by defininga variation 119911(119909) a set of functions
where 120575 gt 0 is a fixed real number and the variation 119911(119909) isa piecewise smooth function with 119911(119886) = 119911(119887) = 0 Usinga Taylor series expansion it is easy to see that a necessarycondition that 0 is a relative minimum to 119865 is
where 119891119910 1198911199101015840 denote the partial derivatives of 119891 evaluated
along (119909 119910(119909) 1199101015840(119909)) and the terms 119911 and 1199111015840 are evaluated at119909
Integrating (20) by parts for all admissible variations 119911(119909)another necessary condition for 119910 = 119910(119909) to give a relativeminimum of the variational problem (16)-(17) is obtainedwhich is the following second-order differential equation
known as Euler-Lagrange conditionThis equationmust holdalong (119909 119910(119909) 1199101015840(119909)) except at a finite number of points [3Section 21]
The extremals of (16)-(17) can be obtained by solving theEuler-Lagrange equation but it only holds at points where theextremal 119910lowast(119909) is smooth At points where 119910lowast1015840(119909) has jumpscalled corners the Weierstrass-Erdmann corner conditionsmust be fulfilled [3 Section 23] Since the location of thecorners their number and the amplitudes of the jumps in1199101015840lowast(119909) 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 ourmethodis that the integral form of this condition
holds for all 119909 isin [119886 119887] and some 119888 and therefore theWeierstrass-Erdmann corner conditions are not neededThus an alternative way of computing the extremals can bebased on this necessary condition in integral form
Note that necessary condition requires that boundaryvalues fulfill Euler-Lagrange equation Thus if some of thefour values 119886 119910(119886) 119887 and 119910(119887) are not explicitly givenalternate boundary conditions have to be provided This iswhat transversality conditions do Assume that 119886 119910(119886) and 119887are given but119910(119887) is free In this case the additional necessarytransversality condition
1198911199101015840 (119887 119910
lowast(119887) 119910
1015840lowast(119887)) = 0 (23)
must holdThe variational approach does not consider constraints
However the optimal control problem has at least a first-order differential constraint (9) representing the dynamicequation of the system Moreover since the dynamic equa-tion of a planar 119877119877 robot manipulator is a second-order
6 Abstract and Applied Analysis
differential equation additional differential constraints willarise while rewriting it as a first-order differential equationTherefore the optimal control problemmust be reformulatedas an unconstrained calculus of variations problem in orderto deal with differential and algebraic constraints as describedin the following section
Following [3 Chapter 5] we reformulate as an uncon-strained calculus of variations problem the optimal controlproblem consisting inminimizing (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 variablefrom 119905 to 119909 and the dependent variable from 119909 to 119910 to beconsistent with the notation of calculus of variations Ourreformulation is based on special derivative multipliers anda change of variables in which
1199101(119909) = 119910(119909) is the renamed state vector
1199101015840
2(119909) = 119906(119909) is the renamed state vector
1199101015840
3(119909) is the multiplier associated with (9)
1199101015840
4(119909) is themultiplier associated with constraint (10)
1199101015840
5(119909) is the multiplier associated with constraint (11)
1199101015840
6(119909) is the excess variable of constraint (11)
Since 1199102(119909) 119910
6(119909) are not unique without an extra condi-
tion we initialize these variables by defining 119910119894(119909119868) = 0 119894 =
1 6 Thus our problem becomes
min 119868 (Y) = int119909119865
119909119868
119865 (119909YY1015840) 119889119909 (24)
where
Y = (1199101 1199102 1199103 1199104 1199105 1199106)119879
119865 = 119891 (119909 1199101 1199101015840
2) + 1199101015840119879
3(1199101015840
1minus 119892 (119909 119910
1 1199101015840
2))
+ 1199101015840119879
4ℎ (119909 119910
1 1199101015840
2) + 1199101015840119879
5(119897 (119909 119910
1 1199101015840
2) + 11991010158402
6)
(25)
Since the values of 119910119894(119909119865) 119894 = 2 6 are unknown
transversality conditions are needed having the form
In the above lines 119910(119909) is an 119899-vector and120595 120593 120601 are assumedto be differentiable in their arguments or with the neededsmoothness We also assume that 120595
11991010158401199101015840 gt 0 The boundary
conditions of the problems are any combination of fixedboundary conditions for the components of 119910 with thepossibility of leaving some of them unspecified
If we reformulate problem (27)-(28) using the techniquedescribed in Section 5 we get the following HamiltonianΨ(119909 119884 119884
1015840) = 120595(119909 119910
1 1199101015840
1) + 1199101015840
2120593(119909 119910
1) with 119910
1(119909) = 119910(119909)
where 11991010158402is the multiplier We have in this case
Ψ11988410158401198841015840 = [
1205951199101015840
11199101015840
1
0
0 0] (31)
which is singular The singularity of Ψ11988410158401198841015840 is a difficulty we
must avoid Furthermore even when it is not difficult tochange from the120593 constraint to the120601 constraint by increasingthe dimension of the independent variables it is not easy todeal with the new associated boundary conditionsThis is thereason that problem (27)-(28) is so difficult to solve
It has been shown in [20] that problem (27)-(28) can bereformulated as an equivalent problem of the form (29)-(30)In particular 119910(119909) is a solution to (27)-(28) if and only if 119910(119909)is a solution to
The numerical method used is based on the discretizationof the unconstrained variational calculus problem stated inthe previous section In particular the main underlying ideais obtaining a discretized solution 119910
ℎ(119909) solving (20) for all
piecewise linear spline function variations 119911(119909) instead of
Abstract and Applied Analysis 7
dealing with the Euler-Lagrange equation (21) Thus thismethod uses no numerical corner conditions and avoidspatching solutions to (21) between corners
Let 119873 be a large positive integer ℎ = (119887 minus 119886)119873 and let120587 = (119886 = 119886
0lt 1198861lt sdot sdot sdot lt 119886
119873= 119887) be a partition of the
interval [119886 119887] where 119886119896= 119886 + 119896ℎ for 119896 = 0 1 119873 Define
the one-dimensional spline hat functions
119908119896 (119909) =
119909 minus 119886119896minus1
ℎif 119886119896minus1
lt 119909 lt 119886119896
119886119896+1
minus 119909
ℎif 119886119896lt 119909 lt 119886
119896+1
0 otherwise
(35)
where 119896 = 1 2 119873 minus 1 Define also the 119898-dimensionalpiecewise linear component functions
119910ℎ (119909) =
119873
sum
119896=0
119882119896 (119909) 119862119896 119911
ℎ (119909) =
119873
sum
119896=0
119882119896 (119909)119863119896 (36)
where 119882119896(119909) = 119908
119896(119909)119868119898times119898
119910ℎ(119909) is the sought numerical
solution and 119911ℎ(119909) is a numerical variation In particular the
constant vectors 119862119896are to be determined by the algorithm
developed by us and the constant vectors 119863119896are arbitrary
Thus the discretized form of (20) is obtained in eachsubinterval [119886
119896minus1 119886119896+1] For the sake of clarity of exposition
we assume that119898 = 1 Note that 1198681015840(119910 119911) in (20) is linear in 119911so that a three-term relationship may be obtained at 119909 = 119886
119896
by choosing 119911(119909) = 119908119896(119909) for 119896 = 1 2 119873 minus 1 Thus
In these equations 119886lowast119896= (119886119896+ 119886119896+1)2 and 119910
119896= 119910ℎ(119886119896) is
the computed value of 119910ℎ(119909) at 119886
119896 In the general case when
119898 gt 1 the same result is obtained but 1198911199101015840 and 119891
119910are
column 119898-vectors of functions with 119894th component 1198911199101198941015840 and
119891119910119894 respectively Similarly (119910
119896+119910119896minus1)2 is the119898-vector which
is the average of the119898-vectors 119910ℎ(119886119896) and 119910
ℎ(119886119896minus1)
By the same arguments that led to (38)
1198911199101015840 (119886lowast
119873minus1119910119873+ 119910119873minus1
2119910119873minus 119910119873minus1
ℎ)
+ℎ
2119891119910(119886lowast
119896minus1119910119873+ 119910119873minus1
2119910119873minus 119910119873minus1
ℎ) = 0
(39)
which is the numerical equivalent of the transversality condi-tion (23) For further details see [3 Chapter 6]
It has been shown in [21] that with this method the globalerror has a priori global reduction ratio of119874(ℎ2) In practiceif the step size ℎ is halved the error decreases by 4
7 Implementation and Results
Several numerical experiments have been carried out forboth 119877119877 and 119877119877 planar horizontal underactuated robotmanipulators
71 Planar Horizontal Underactuated 119877119877 Robot ManipulatorIn this section the optimal control problem of a planar hor-izontal underactuated 119877119877 is studied In this robot model thesecond joint is not actuated thus 119906 = (119906
1 0)119879 In this case
it is neither possible to integrate partially nor completely thenonholonomic constraint because the manipulator inertiamatrix contains terms in 120579
2(see [12]) Hence the system is
controllable The numerical results of the application of ourmethod for optimal control to a boundary value problem andto an initial value problem for this system will be described
For a planar horizontal underactuated119877119877 (2) can be splitinto
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792
minus 120573 sin 1205792(2 12057911205792+ 1205792
2) = 1199061
(120575 + 120573 cos 1205792) 1205791+ 120575 1205792+ 120573 sin 120579
21205792
1= 0
(40)
To express optimal control problems that involve this second-order differential constraints in the form of a basic optimal
8 Abstract and Applied Analysis
control problem we have first to convert it into first-orderdifferential constraints introducing the following change ofvariables
1199091= 1205791 119909
3= 1205791
1199092= 1205792 119909
4= 1205792
(41)
with the following additional relations
1199091015840
1= 1199093
1199091015840
2= 1199094
(42)
Thus the second-order differential equations (40) are con-verted into the first-order differential equations
(120572 + 2120573 cos1199092) 1199091015840
3+ (120575 + 120573 cos119909
2) 1199091015840
4
minus 120573 sin1199092(211990931199094+ 1199092
4) = 1199061
(43)
(120575 + 120573 cos1199092) 1199091015840
3+ 1205751199091015840
4+ 120573 sin119909
21199092
3= 0 (44)
Relations (42) (43) and (44) are now the differential con-straints of the optimal control problem and the objectivefunctional to minimize is
119869 = int
119905119865
119905119868
1199062
1119889119905 (45)
Then we introduce the following new variables
X = [
[
1198831
sdot sdot sdot
1198839
]
]
(46)
such that
119883119894= 119909119894 119894 = 1 4 (47)
1198831015840
5= 1199061 119883
5(119905119868) = 0 (48)
where 11988310158406with 119883
6(119905119868) = 0 is the multiplier associated with
whereas in the initial value problem 119883119894(119905119865) will be free for
some 119894The initial values of control variables and multipliers
have been set to zero whereas their final values have notbeen assigned in both optimal control problems Thereforetransversality conditions are needed in both cases for thevariables119883
119894(119905119865) 119894 = 5 10 and they will be of the form
Figure 2 Sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 corresponding to the optimal solutionof problem 1 a boundary value problem for the planar 119877119877 robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad]
1205791(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads] obtained with a
discretization of [119905119868 119905119865] into 64 subintervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control and
state variables are represented in Figure 3
The initial values of control variable and of the multipliershave been set to zero whereas their final values are left freeFigure 2 shows the sequence of configurations of the robot attimes 119905 = 11989632 119896 = 0 1 32 Since the configurations ofthe sequence overlap it has been split into smaller sequencesfor a better visualization of the manipulator motion Figure 3depicts the corresponding control and state variables of theoptimal solution of this boundary value problem obtainedwith a discretization of the time interval [119905
119868 119905119865] into 64
subintervals The value of the objective functional for thissolution is 345185 [J]
712 Problem 2 Initial Value Problem An initial valueproblem has also been solved with the following initial andfinal conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) =
120587
2[rad]
1205792(119905119868) = minus
120587
2[rad] 120579
2(119905119865) = minus
120587
2[rad]
1205791(119905119868) = 0 [rads] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(55)
The initial values of control variable and of the multipliershave been set to zero whereas their final values are left freeThe only difference between these conditions and those of theboundary value problem described in Section 711 is that now1205791(119905119865) = free
Figures 4 and 5 depict the sequence of configurations the119877119877 robot manipulator and the corresponding control andstate variables of the optimal solution of this initial valueproblem respectively obtained with a discretization of thetime interval [119905
119868 119905119865] into 64 subintervals The value of the
objective functional for this solution is 56472 [J]This value islower than the value of the objective functional of the solutionof the boundary value problem described in Section 711because now is 120579
1(119905119865) = free and the control system does
not have to spend energy to stop it
72 Planar Horizontal Underactuated 119877119877 Robot ManipulatorIn this section the optimal control problem of a planarhorizontal underactuated 119877119877 robot manipulator is studiedIn this robot model the first joint is not actuated thus 119906 =
(0 1199062)119879 and (2) can be split into
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792
minus 120573 sin 1205792(2 12057911205792+ 1205792
2) = 0
(56)
(120575 + 120573 cos 1205792) 1205791+ 120575 1205792+ 120573 sin 120579
21205792
1= 1199062 (57)
As explained in [12] since gravity terms are all zero and1205791does not intervene in the system inertia matrix (56) can
be partially integrated to
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792+ 1198881= 0 (58)
Actually constraint (56) is completely integrable giving rise toan holonomic constraintThe resulting holonomic constrainttakes different forms depending on the value of 119888
1which
depends on the initial conditions Therefore two cases havebeen considered
(i) when the initial velocities 1205791(119905119868) and 120579
2(119905119868) are both
zero(ii) when the initial velocity 120579
1(119905119868) is nonzero
721 Problem 3 Initial Value Problem with Zero Initial Veloc-ities An initial value problem has been solved with the fol-lowing initial and final conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rad] 120579
2(119905119865) = 120587 [rad]
1205791(119905119868) = 0 [rads] 120579
1(119905119865) = 0 [rads]
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(59)
10 Abstract and Applied Analysis
10 20 30 40 50 60
05
1
15
2
25
(a) 1205791
10 20 30 40 50 60minus05
minus1
minus15
minus2
minus25
minus3
(b) 1205792
10 20 30 40 50 60
minus10
minus5
5
10
(c) 1205791
10 20 30 40 50 60
minus10
minus5
5
10
15
(d) 1205792
10 20 30 40 50 60
minus300
minus200
minus100
100
200
300
(e) 1199061
Figure 3 Control and state variables of the optimal solution of problem 1 a boundary value problem for the planar 119877119877 robot manipulatorwith boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 2
Figure 4 Sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 corresponding to the optimal solutionof problem 2 an initial value problem for an 119877119877 robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) =
0 [rads] 1205792(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained with a discretization of
[119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control and state variables are
represented in Figure 5
Abstract and Applied Analysis 11
10 20 30 40 50 60
05
1
15
2
25
(a) 1205791
10 20 30 40 50 60
minus25
minus2
(b) 1205792
10 20 30 40 50 60
minus4
minus2
2
4
6
8
10
(c) 1205791
minus4
minus6
minus210 20 30 40 50 60
2
4
(d) 1205792
10 20 30 40 50 60minus50
50
100
150
(e) 1199061
Figure 5 Control and state variables of the optimal solution of problem 2 an initial value problem for a119877119877 robotmanipulator with boundaryconditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = free
and 1205792(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of configurations of the
robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 4
The initial values of the control variable and of themultipliershave been set to zero whereas their final value is left free
Since there is a holonomic constraint that relates thevalues of the angles 120579
1and 120579
2 without integrating (58) we
are not able to find the value of 1205791(119905119865) consistent with 120579
1(119905119868)
Therefore no final conditions have been imposed on 1205791
From the initial conditions of the problem we obtain 1198881=
0 Equation (58) with 1198881= 0 corresponds to the homogeneous
differential constraint
119889120577 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792= 0 (60)
The differential 119889120577 is not exact However it becomes an exactdifferential if multiplied by the factor 1(120572 + 2120573 cos 120579
2) This
operation does not alter the differential equation (60) Inthis case there does exist a function 120577 whose differentialcoincides with the expression 119889120577(120572 + 2120573 cos 120579
2) Due to
the existence of this function the integral of 119889120577 between
two points depends only on these points and not on theintegration path Equation (60) rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2)
120572 + 2120573 cos 1205792
1198891205792
(61)
can be integrated by separating variables The correspondingholonomic constraint has the following expression
1205791= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1205792
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1205792
2+ 1198882
(62)
To express this optimal control problem in the form ofa basic optimal control problem we first have to convert (57)
12 Abstract and Applied Analysis
into a first-order differential model introducing the followingchange of variables
1199091= 1205791 119909
3= 1205791
1199092= 1205792 119909
4= 1205792
(63)
with the following additional relations
1199091015840
1= 1199093
1199091015840
2= 1199094
(64)
Thus the optimal control problem is to minimize
int
1
0
1199062
2119889119905 (65)
subject to the constraints1199091
= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1199092
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1199092
2+ 1198882
(66)
(120575 + 120573 cos 1205792) 1199091015840
3+ 1205751199091015840
4+ 120573 sin119909
21199092
3= 1199062 (67)
and the additional constraints (64) To reformulate thisoptimal control problem as an unconstrained calculus ofvariations problem let X be
X = [
[
1198831
sdot sdot sdot
1198839
]
]
(68)
such that119883119894= 119909119894 119894 = 1 4
1198831015840
5= 1199061 119883
5(119905119868) = 0
(69)
where 11988310158406with 119883
6(119905119868) = 0 is the multiplier associated with
the holonomic constraint (66) 11988310158407with 119883
7(119905119868) = 0 is the
multipliers associated with the differential constraint (67)and 1198831015840
8with 119883
8(119905119868) = 0 and 1198831015840
9with 119883
9(119905119868) = 0 are the
multipliers associatedwith the additional equality constraints(64)
Thus the holonomic constraint of the problem can berewritten as follows
120593 (119905X)
= 1198831minus((120572 minus 2120575)
times tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1198832
2)]]
]
)
times (radic41205732 minus 1205722)
minus1
+1198832
2= 0
(70)
Now the technique described in Section 51 to deal withholonomic constraints can be applied to 120593(119905X) and thisholonomic constraint is replaced by
120593119905+ 1205931198831198831015840= 0
120593 (0 119883 (0)) = 0
(71)
From the initial conditions of the problem the latter equationreduces to the equality 0 = 0 whereas the former takes thefollowing form
(120572 + 2120573 cos (1198832))1198833+ (120575 + 120573 cos (119883
2))1198834= 0 (72)
The corresponding Hamiltonian is
1198661= 11988310158402
5+ 1198831015840
6((120572 + 2120573 cos (119883
2))1198833
+ (120575 + 120573 cos (1198832))1198834)
+1198831015840
7(120573 sin (119883
2)1198832
3+ (120575 + 120573 cos (119883
2))1198831015840
3
+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(73)
It is not difficult to check that matrix 1198661X1015840X1015840
is singularThis is due to the fact that to handle our optimal controlproblemwhich involves second-order differential constraintswe converted them into first-order differential constraintsTherefore we apply again the technique of Section 51obtaining the identity 0 = 0 and the following constraint
minus 120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4= 0
(74)
The corresponding Hamiltonian is
1198662= 11988310158402
5+ 1198831015840
6
times (minus120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4)
+ 1198831015840
7(120573 sin (119883
2)1198832
3+(120575+120573 cos (119883
2))1198831015840
3+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(75)
It is not difficult to check that matrix 1198662X1015840X1015840
in this case is notsingular since its determinant is
Substituting the values of120572120573 and 120575 this expression becomesdet(119866
2X1015840X1015840) = 1128(43 minus 2 cos(2119883
2))2 which is always
positive for any real value1198832 Figure 6 shows the sequence of
configurations of the robot at times 11989632with 119896 = 0 1 32and Figure 7 depicts control and state variables of the optimal
Abstract and Applied Analysis 13
Figure 6 Sequence of configurations of the robot manipulator attimes 11989632 with 119896 = 0 1 32 corresponding to the optimal sol-ution of problem 3 an initial value problem for an underactuated119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) =
0 [rad] 1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads]
1205791(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2
(119905119865) = 0 [rads] obtained with a discretization of [119905
119868 119905119865] into 64
subintervals The initial and final times are 119905119868= 0 and 119905
119865=
1 [s] respectively The corresponding control and state variables arerepresented in Figure 7
solution obtained with a discretization of the interval [119905119868 119905119865]
into 64 subintervalsIn particular we get 120579
1(119905119865) = minus110248 [rad] To check the
consistency of this result with the holonomic constraint (62)since 120579
1(119905119868) = 1205792(119905119868) = 0 [rads] we get from (58) that 119888
1= 0
and using the initial condition 1205791(119905119868) = 1205792(119905119868) = 0 [rad] we
get from (62) that 1198882= 0 Having established the value of the
constant 1198882 we obtain from the same equation for 120579
2(119905119865) =
120587 [rad] that 1205791(119905119865) = minus110248 [rad] which coincides with
the value of 1205791(119905119865) obtained numerically
722 Problem 4 Initial Value Problem with Nonzero InitialVelocity 120579
1 Another initial value problem has been solved
with the following conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rad] 120579
2(119905119865) = 120587 [rad]
1205791(119905119868) = 5 [rads] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(77)
The initial values of the multipliers have been set to zerowhereas their final value is left free Notice that no finalconditions have been imposed on 120579
1and 120579
1 The same
considerations done in previous section hold in this caseas well The technique described in Section 51 must beapplied twice leading to the differential constraint (74) andto the Hamiltonian (75) Figure 8 shows the sequence ofconfigurations of the robot at times 11989632with 119896 = 0 1 32and Figure 9 depicts the control and state variables ofthe optimal solution obtained with a discretization of theinterval [119905
119868 119905119865] into 64 subintervals In particular we get that
1205791(119905119865) = 617172 [rad] and 120579
1(119905119865) = 900163 [rads] To
check the consistency of the obtained value of 1205791(119905119865) with
the holonomic constraint consider (58) We can calculate
the constant 1198881using the initial conditions of the problem
obtaining
1198881= minus 120579
1(119905119868) (120572 + 2120573 cos 120579
2(119905119868))
minus 1205792(119905119868) (120575 + 120573 cos 120579
2(119905119868)) = minus225
(78)
Since 1198881= 0 (58) corresponds in this case to the differential
constraint
119889120578 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792+ 1198881119889119905 = 0
(79)
Equation (79) can be rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2 (119905))
120572 + 2120573 cos 1205792 (119905)
1198891205792minus
1198881
120572 + 2120573 cos 1205792 (119905)
119889119905 (80)
To check the obtained value of 1205791(119905119865) 1198891205791is numerically
integrated between 1205791(119905119868) and 120579
1(119905119865) using the interpolated
numerical optimal solution obtained for 1205792(119905) We get that
1205791(119905119865) = 618705 This value is close to 617172
To check the consistency of the obtained value of 1205791(119905119865)
with the constraint (58) using the computed value 1198881= minus225
and the final conditions 1205792(119905119865) = 0 120579
2(119905119865) = 120587 of the problem
we obtain
1205791(119905119865) =
minus (120575 + 120573 cos 1205792(119905119865))
120572 + 2120573 cos 1205792(119905119865)
1205792(119905119865)
minus1198881
120572 + 2120573 cos 1205792(119905119865)= 9
(81)
This value is very close to the value of 1205791(119905119865) obtained
numerically
73 Computational Issues If the optimal control problem has119898 variables and the time interval [119905
119868 119905119865] has been discretized
into 119873 subintervals the resulting set of difference equations(38) has119898times(119873minus1) equations and119898times(119873minus1) variables plusthe equations and variables due to transversality conditionsFeasible solutions have been used as initial guesses of thealgorithm
The solution of the nonlinear system of difference equa-tions (38) has been obtained using a damped Newtonalgorithm within a line search methodology implementedin Mathematica 7 under Mac OS X operating system (see[22 23] for more details)
8 Conclusion
In this paper the trajectory planning problem for planarunderactuated robot manipulators with two revolute jointswithout gravity has been studied This problem is solved asan optimal control problem based on a numerical resolutionof an unconstrained variational calculus reformulation of theoptimal control problem in which the dynamic equation ofthe mechanical system is regarded as a constraint It hasbeen shown that this reformulation method based on special
14 Abstract and Applied Analysis
10 20 30 40 50 60
minus1
minus08
minus06
minus04
minus02
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
minus15
minus1
minus05
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
(d) 1205792
10 20 30 40 50 60
minus15
minus10
5
10
15
minus5
(e) 1199062
Figure 7 Control and state variables of the optimal solution of problem 3 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 119906
2(119905119868) = 0Nm 120579
1(119905119865) = free
1205792(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads]The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectivelyThe corresponding
sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 6
Figure 8 Sequence of configurations of the robot manipulator at times 11989632 with 119896 = 0 1 32 corresponding to the optimal solutionof problem 4 an initial value problem for an underactuated 119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad]
1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained
with a discretization of [119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control
and state variables are represented in Figure 9
Abstract and Applied Analysis 15
10 20 30 40 50 60
1
2
3
4
5
6
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
4
5
6
7
8
9
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
6
(d) 1205792
10 20 30 40 50 60minus5
5
10
15
20
(e) 1199062
Figure 9 Control and state variables of the optimal solution of problem 4 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 8
derivative multipliers is able to tackle both integrable andnonintegrable differential constraints of the dynamic modelsof underactuated planar horizontal robot manipulators withtwo revolute joints This method can be seamlessly appliedin the presence of additional constraints on the mechanicalsystem
References
[1] A De Luca S Iannitti R Mattone and G Oriolo ldquoUnderactu-ated manipulators control properties and techniquesrdquoMachineIntelligence and Robotic Control vol 4 no 3 pp 113ndash125 2002
[2] G A Bliss Lectures on the Calculus of Variations University ofChicago Press Chicago Ill USA 1946
[3] J Gregory and C Lin Constrained Optimization in the Calculusof Variations and Optimal Control theory Chapman amp Hall1996
[4] W-S Koon and J E Marsden ldquoOptimal control for holonomicand nonholonomic mechanical systems with symmetry andLagrangian reductionrdquo SIAM Journal on Control and Optimiza-tion vol 35 no 3 pp 901ndash929 1997
[5] A M Bloch Nonholonomic Mechanics and Control SpringerNew York NY USA 2003
[6] I I Hussein and A M Bloch ldquoOptimal control of underactu-ated nonholonomic mechanical systemsrdquo IEEE Transactions onAutomatic Control vol 53 no 3 pp 668ndash682 2008
[7] X Z Lai J H She S X Yang andMWu ldquoComprehensive uni-fied control strategy for underactuated two-link manipulatorsrdquoIEEE Transactions on Systems Man and Cybernetics B vol 39no 2 pp 389ndash398 2009
[8] J P Ordaz-Oliver O J Santos-Sanchez and V Lopez-MoralesldquoToward a generalized sub-optimal control method of underac-tuated systemsrdquo Optimal Control Applications amp Methods vol33 no 3 pp 338ndash351 2012
16 Abstract and Applied Analysis
[9] R Seifried ldquoTwo approaches for feedforward control andoptimal design of underactuatedmultibody systemsrdquoMultibodySystem Dynamics vol 27 no 1 pp 75ndash93 2012
[10] M Buss O von Stryk R Bulirsch and G Schmidt ldquoTowardshybrid optimal controlrdquo atmdashAutomatisierungstechnik vol 48no 9 pp 448ndash459 2000
[11] M Buss M Glocker M Hardt O von Stryk R Bulirsch andG Schmidt ldquoNonlinear hybrid dynamical systems modelingoptimal control and applicationsrdquo in Modelling Analysis andDesign of Hybrid Systems S Engell G Frehse and E SchniederEds vol 279 of Lecture Notes in Control and InformationScience pp 331ndash335 Springer 2002
[12] G Oriolo and Y Nakamura ldquoControl of mechanical systemswith second-order nonholonomic constraints underactuatedmanipulatorsrdquo in Proceedings of the 30th IEEE Conference onDecision and Control pp 2398ndash2403 December 1991
[13] T J Tarn M Zhang and A Serrani ldquoNew integrability condi-tions for differential constraintsrdquo Systems and Control Lettersvol 49 no 5 pp 335ndash345 2003
[14] H J Sussmann ldquoA general theorem on local controllabilityrdquoSIAM Journal on Control and Optimization vol 25 no 1 pp158ndash194 1987
[15] F Bullo A D Lewis and K M Lynch ldquoControllable kinematicreductions for mechanical systems concepts computationaltools and examplesrdquo in Proceedings of International Symposiumon Mathematical Theory of Networks and Systems 2002
[16] F Bullo and A D Lewis ldquoLow-order controllability and kine-matic reductions for affine connection control systemsrdquo SIAMJournal on Control andOptimization vol 44 no 3 pp 885ndash9082006
[17] A D Lewis and R MMurray ldquoConfiguration controllability ofsimple mechanical control systemsrdquo SIAM Journal on Controland Optimization vol 35 no 3 pp 766ndash790 1997
[18] F Bullo and K M Lynch ldquoKinematic controllability for decou-pled trajectory planning in underactuatedmechanical systemsrdquoIEEE Transactions on Robotics and Automation vol 17 no 4 pp402ndash412 2001
[19] M R Hestenes Calculus of Variations and Optimcl ControlTheory John Wiley amp Sons 1966
[20] J Gregory ldquoA new systematic method for efficiently solvingholonomic (and nonholonomic) constraint problemsrdquo Analysisand Applications vol 8 no 1 pp 85ndash98 2010
[21] J Gregory and R S Wang ldquoDiscrete variable methods forthe m-dependent variable nonlinear extremal problem in thecalculus of variationsrdquo SIAM Journal onNumerical Analysis vol27 no 2 pp 470ndash487 1990
[22] Wolfram Research 2012[23] J J More and D J Thuente ldquoLine search algorithms with guar-
anteed sufficient decreaserdquo ACM Transactions on MathematicalSoftware vol 20 no 3 pp 286ndash307 1994
where the first term of this equation 119861(120579) 120579 represents theinertial forces due to acceleration at the joints and the secondterm 119862(120579 120579) 120579 represents the Coriolis and centrifugal forcesThe third term 119865 120579 is a simplified model of the frictionin which only the viscous friction is considered The term119890(120579) represents the potential forces such as elasticity andgravity Matrix119866(120579) on the right-hand side maps the externalforcestorques 119908 to forcestorques at the joints Finally 119906represents the forcestorques at the joints that are the controlvariables of the system
We suppose that the links are rigid as well as the trans-mission elements and that the robot moves in a horizontalplane in such a way the gravity does not affect the dynamicsof the manipulator Finally we do not take into account theeffects of the friction and we suppose that no external forcesare acting on themechanical systemUnder these hypothesesthe dynamic model of the robotic system reduces to
A horizontal planar 119877119877 manipulator is composed of twohomogeneous links and two revolute joints moving in ahorizontal plane 119909 119910 as shown in Figure 1 where 119897
119894is the
length of link 119894 119903119894is the distance between joint 119894 and the
mass center of link 119894 119898119894is the mass of link 119894 and 119868
119911119894is the
barycentric inertia with respect to a vertical axis 119911 of link 119894
for 119894 = 1 2 In this case the two matrices 119861(120579) and 119862(120579 120579)have the form
119861 (120579) = [120572 + 2120573 cos 120579
2120575 + 120573 cos 120579
2
120575 + 120573 cos 1205792
120575]
119862 (120579 120579) = [minus120573 sin 120579
21205792minus120573 sin 120579
2( 1205791+ 1205792)
120573 sin 12057921205791
0]
(3)
where 120579 = (1205791 1205792)119879 is the vector of configuration variables
being 1205791the angular position of link 1 with respect to the 119909
axis of the reference frame 119909 119910 and 1205792the angular position
of link 2 with respect to link 1 as illustrated in Figure 1 Thevector 120579 = ( 120579
1 1205792)119879 is the vector of angular velocities and
120579 = ( 1205791 1205792)119879 is the vector of accelerations The control inputs
of the system are 119906 = (1199061 1199062)119879 where 119906
1is the torque applied
by the actuator at joint 1 and 1199062is the torque applied by the
actuator at joint 2 The parameters 120572 120573 and 120575 in (3) have thefollowing expressions
A robot manipulator is said to be underactuated whenthe number of actuators is less than the degree of freedom ofthe mechanical systemThe dynamic model (2) that does notconsider the effects of gravity and friction can be rewrittenfor a 119877119877 robot manipulator underactuated by one control inthe form [1]
[119861119886119886 (120579) 119861
119906119886 (120579)
119861119906119886 (120579) 119861
119906119906 (120579)](
120579119886
120579119906
) + [119862119886(120579 120579)
119862119906(120579 120579)
] = (119906119886
0) (5)
in which the state variables 120579119886and 120579
119906correspond to the
actuated and unactuated joints and 119906119886is the available control
input The last equation of (5) describes the dynamics of theunactuated part of the mechanical system and has the form
119861119906119886 (120579)
120579119886+ 119861119906119906 (120579)
120579119906+ 119862119906(120579 120579) = 0 (6)
which is a second-order differential constraint without inputvariables
Underactuated manipulators may be equipped withbrakes at the passive joints Hybrid optimal control strategiescan be designed in this case [10 11] The presence of brakeswill not be considered in this paper
3 Control Properties of Underactuated RRRobot Manipulators
In this section the main control properties of planar under-actuated 119877119877 robot manipulators without the effects of thegravity will be described For a more general description ofthe control properties of underactuated robot manipulatorssee [1]
4 Abstract and Applied Analysis
Optimal control approaches to trajectory planning ass-ume that there exists a control input that steers the systembetween two specify states Thus controllability is the mostimportant aspect to check before studying optimal control ofa dynamic system If in the trajectory planning problem theduration of the motion 119879 is not assigned the existence of afinite-time solution for any state (120579
119865 120579119865) in a neighborhood
of (120579119868 120579119868) is equivalent for the robotic system to the property
of local controllability at (120579119868 120579119868) If local controllability holds
at any state then the system is controllable and the trajectoryplanning problem is solvable for any pair of initial and finalstates
For underactuated119877119877 robotmanipulators controllabilityis related to integrability of the second-order nonholonomicconstraints The second-order differential constraint (6) mayeither be partially integrable to a first-order differentialequation or completely integrable to a holonomic equationNecessary and sufficient integrability conditions are givenin [12 13] If (6) is not partially integrable it is possible tosteer the system between equilibrium points This occursfor planar underactuated 119877119877 robot manipulators withoutgravity which therefore are controllable If (6) is completelyintegrable to a holonomic constraint the motion of themechanical system is restricted to a 1-dimensional sub-manifold of the configuration space which depends on theinitial configuration This occurs for the dynamic equationsof planar underactuated 119877119877 robot manipulators withoutthe effects of the gravity [12] For this robot model thetrajectory planning problem has solution only for particularinitial and final states Thus when (6) is not partially orcompletely integrable the mechanical system is controllableHowever several aspects of controllability can be studiedwhich characterize this model of underactuatedmanipulator
A dynamical system is linearly controllable at an equi-librium point if the linear approximation of the systemaround this point is controllable Planar underactuated 119877119877robot manipulators in the absence of gravity are not linearlycontrollable On the contrary both planar underactuated 119877119877and 119877119877 robot manipulators are linearly controllable in thepresence of gravity
A mechanical system is said to be small-time locallycontrollable (STLC) at 119909
119868= (120579119868 120579119868) if for any neighborhood
V119909of 119909119868and any time 119879 gt 0 the set RV119909 119879
(119909119868) of states
that are reachable from 119909119868within time 119879 along trajectories
contained in V includes a neighborhood of 119909119868 Note that
small-time local controllability is a stronger property thancontrollability [14] Non-STLC but controllable system mustin general perform finite maneuvers in order to performarbitrarily small changes of configuration It has been provenin [15 16] that planar underactuated 119877119877 robot manipulatorsin the absence of gravity are not STLC Both planar under-actuated 119877119877 and 119877119877 robot manipulators in the presence ofgravity are also not STLC
Second-order mechanical systems cannot be STLC atstates with nonzero velocity Therefore the weaker conceptof small-time local configuration controllability has beenintroduced [17] A system is said to be small-time local
configuration controllable (STLCC) at a configuration 120579119868if
for any neighborhood V120579of 120579119868in the configuration space
and any time 119879 gt 0 the set RV120579119879(120579119868) of configurations that
are reachable (with some final velocity 120579) within 119879 startingfrom (120579
119868 0) and along a path in configuration space contained
in V120579 includes a neighborhood of 120579
119868 By definition STLC
systems are also STLCC Sufficient conditions for STLCC
are given in [17] It has been proven in [15 16] that planarunderactuated 119877119877 robot manipulators in the absence ofgravity are not STLCC Both planar underactuated 119877119877 and119877119877 robot manipulators in the presence of gravity are also notSTLCC
A final question is to investigate if the trajectory planningproblem for119877119877planar underactuated robotmanipulators canbe solved with algorithmic methods A mechanical systemis kinematically controllable (KC) if every configuration isreachable by means of a sequence of kinematic motions thatis feasible paths in the configuration space which may befollowed with any arbitrary timing law [15 16 18] Note thatKC mechanical systems are also STLCC and that kinematiccontrollability does not imply small-time local controllabilityIf amechanism isKC the trajectory planning problemmaybesolved with algorithmic methods Planar underactuated 119877119877robotmanipulators in the absence of gravity are notKC Bothplanar underactuated 119877119877 and 119877119877 robot manipulators in thepresence of gravity are not KC [15]
4 The Optimal Control Problem
Given the dynamic equation of an underactuated planar119877119877 robot manipulator an initial state (120579
119868 120579119868) and a final
state (120579119865 120579119865) the optimal control problem consists in finding
the available control input 1199061(119905) or 119906
2(119905) and the resulting
trajectory with 119905 isin [119905119868 119905119865] that steers the system between
initial and final states satisfying the dynamic equation (5) andminimizing the objective functional
119869 = int
119905119865
119905119868
1199062
119894119889119905 (7)
where 119894 = 1 2 depending on which joint is actuated and119905119868and 119905119865are the initial and final time values respectively
This cost functional represents a measure of the energyconsumed during the motion since torque produced withan electromechanical actuator is approximately proportionalto the current flow and the rate of energy consumption isapproximately equal to the square of this current
If 120579119868= 120579119865= 0 the problem is called rest-to-rest trajectory
planning problem If the final or the initial states or part ofthem is not assigned the problems are called initial valueproblem and final value problem respectively The final time119905119865may be fixed or notThis problem is a particular case of an optimal control
problem which can be stated in a more general form asfollows Minimize the integral
where 119909(119905) = (1199091(119905) 1199092(119905) 119909
119899(119905))119879 is an 119899-vector called
the state vector 119906(119905) = (1199061(119905) 1199062(119905) 119906
119898(119905))119879 is an 119898-
vector called the control vector the real-valued function119869(119909 119906) is the objective functional (9) is called the trajectoryequation and the conditions (14) are called the boundaryconditions The set 119880 sub R119898 is called the set of controls with119906(119905) isin 119880 for every 119905 isin [119905
119868 119905119865] We assume that 119891 119892 ℎ 119897 119901
and 119902 are sufficiently smooth for our purposeThis will implysolutions such that 119909(119905) is piecewise smooth whereas 119906(119905) ispiecewise continuous [19]
5 Variational Reformulation of the OptimalControl Problem
A variational approach has been used to solve the more gen-eral optimal control problem stated in the previous section
The classical calculus of variations problem is tominimizean integral of the form
where the independent 119909 variable is assumed to be in theinterval [119886 119887] and the dependent variable 119910 = 119910(119909) = (119910
1(119909)
1199102(119909) 119910
119899(119909))119879 is assumed to be an 119899-vector continuous on
[119886 119887] with derivative 1199101015840 = 1199101015840(119909) = (11991010158401(119909) 1199101015840
2(119909) 119910
1015840
119899(119909))119879
It is also assumed that 119910 is piecewise smooth that is thereexists a finite set of points 119886
1 1198862 119886
119896so that 119886 le 119886
1lt 1198862lt
sdot sdot sdot lt 119886119896le 119887 119910(119909) is continuously differentiable on (119886
119897 119886119897+1)
and that the respective left- and right-handed limits of 1199101015840(119909)exist If 119910(119909) is piecewise smooth and satisfies the boundaryconditions 119910(119886) = 119860 119910(119887) = 119861 then 119910(119909) is said to be anadmissible arc In words this problem consists in findingamong all arcs connecting end points (119886 119860) and (119887 119861) theone minimizing the integral (16)
The main optimality conditions are obtained by defininga variation 119911(119909) a set of functions
where 120575 gt 0 is a fixed real number and the variation 119911(119909) isa piecewise smooth function with 119911(119886) = 119911(119887) = 0 Usinga Taylor series expansion it is easy to see that a necessarycondition that 0 is a relative minimum to 119865 is
where 119891119910 1198911199101015840 denote the partial derivatives of 119891 evaluated
along (119909 119910(119909) 1199101015840(119909)) and the terms 119911 and 1199111015840 are evaluated at119909
Integrating (20) by parts for all admissible variations 119911(119909)another necessary condition for 119910 = 119910(119909) to give a relativeminimum of the variational problem (16)-(17) is obtainedwhich is the following second-order differential equation
known as Euler-Lagrange conditionThis equationmust holdalong (119909 119910(119909) 1199101015840(119909)) except at a finite number of points [3Section 21]
The extremals of (16)-(17) can be obtained by solving theEuler-Lagrange equation but it only holds at points where theextremal 119910lowast(119909) is smooth At points where 119910lowast1015840(119909) has jumpscalled corners the Weierstrass-Erdmann corner conditionsmust be fulfilled [3 Section 23] Since the location of thecorners their number and the amplitudes of the jumps in1199101015840lowast(119909) 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 ourmethodis that the integral form of this condition
holds for all 119909 isin [119886 119887] and some 119888 and therefore theWeierstrass-Erdmann corner conditions are not neededThus an alternative way of computing the extremals can bebased on this necessary condition in integral form
Note that necessary condition requires that boundaryvalues fulfill Euler-Lagrange equation Thus if some of thefour values 119886 119910(119886) 119887 and 119910(119887) are not explicitly givenalternate boundary conditions have to be provided This iswhat transversality conditions do Assume that 119886 119910(119886) and 119887are given but119910(119887) is free In this case the additional necessarytransversality condition
1198911199101015840 (119887 119910
lowast(119887) 119910
1015840lowast(119887)) = 0 (23)
must holdThe variational approach does not consider constraints
However the optimal control problem has at least a first-order differential constraint (9) representing the dynamicequation of the system Moreover since the dynamic equa-tion of a planar 119877119877 robot manipulator is a second-order
6 Abstract and Applied Analysis
differential equation additional differential constraints willarise while rewriting it as a first-order differential equationTherefore the optimal control problemmust be reformulatedas an unconstrained calculus of variations problem in orderto deal with differential and algebraic constraints as describedin the following section
Following [3 Chapter 5] we reformulate as an uncon-strained calculus of variations problem the optimal controlproblem consisting inminimizing (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 variablefrom 119905 to 119909 and the dependent variable from 119909 to 119910 to beconsistent with the notation of calculus of variations Ourreformulation is based on special derivative multipliers anda change of variables in which
1199101(119909) = 119910(119909) is the renamed state vector
1199101015840
2(119909) = 119906(119909) is the renamed state vector
1199101015840
3(119909) is the multiplier associated with (9)
1199101015840
4(119909) is themultiplier associated with constraint (10)
1199101015840
5(119909) is the multiplier associated with constraint (11)
1199101015840
6(119909) is the excess variable of constraint (11)
Since 1199102(119909) 119910
6(119909) are not unique without an extra condi-
tion we initialize these variables by defining 119910119894(119909119868) = 0 119894 =
1 6 Thus our problem becomes
min 119868 (Y) = int119909119865
119909119868
119865 (119909YY1015840) 119889119909 (24)
where
Y = (1199101 1199102 1199103 1199104 1199105 1199106)119879
119865 = 119891 (119909 1199101 1199101015840
2) + 1199101015840119879
3(1199101015840
1minus 119892 (119909 119910
1 1199101015840
2))
+ 1199101015840119879
4ℎ (119909 119910
1 1199101015840
2) + 1199101015840119879
5(119897 (119909 119910
1 1199101015840
2) + 11991010158402
6)
(25)
Since the values of 119910119894(119909119865) 119894 = 2 6 are unknown
transversality conditions are needed having the form
In the above lines 119910(119909) is an 119899-vector and120595 120593 120601 are assumedto be differentiable in their arguments or with the neededsmoothness We also assume that 120595
11991010158401199101015840 gt 0 The boundary
conditions of the problems are any combination of fixedboundary conditions for the components of 119910 with thepossibility of leaving some of them unspecified
If we reformulate problem (27)-(28) using the techniquedescribed in Section 5 we get the following HamiltonianΨ(119909 119884 119884
1015840) = 120595(119909 119910
1 1199101015840
1) + 1199101015840
2120593(119909 119910
1) with 119910
1(119909) = 119910(119909)
where 11991010158402is the multiplier We have in this case
Ψ11988410158401198841015840 = [
1205951199101015840
11199101015840
1
0
0 0] (31)
which is singular The singularity of Ψ11988410158401198841015840 is a difficulty we
must avoid Furthermore even when it is not difficult tochange from the120593 constraint to the120601 constraint by increasingthe dimension of the independent variables it is not easy todeal with the new associated boundary conditionsThis is thereason that problem (27)-(28) is so difficult to solve
It has been shown in [20] that problem (27)-(28) can bereformulated as an equivalent problem of the form (29)-(30)In particular 119910(119909) is a solution to (27)-(28) if and only if 119910(119909)is a solution to
The numerical method used is based on the discretizationof the unconstrained variational calculus problem stated inthe previous section In particular the main underlying ideais obtaining a discretized solution 119910
ℎ(119909) solving (20) for all
piecewise linear spline function variations 119911(119909) instead of
Abstract and Applied Analysis 7
dealing with the Euler-Lagrange equation (21) Thus thismethod uses no numerical corner conditions and avoidspatching solutions to (21) between corners
Let 119873 be a large positive integer ℎ = (119887 minus 119886)119873 and let120587 = (119886 = 119886
0lt 1198861lt sdot sdot sdot lt 119886
119873= 119887) be a partition of the
interval [119886 119887] where 119886119896= 119886 + 119896ℎ for 119896 = 0 1 119873 Define
the one-dimensional spline hat functions
119908119896 (119909) =
119909 minus 119886119896minus1
ℎif 119886119896minus1
lt 119909 lt 119886119896
119886119896+1
minus 119909
ℎif 119886119896lt 119909 lt 119886
119896+1
0 otherwise
(35)
where 119896 = 1 2 119873 minus 1 Define also the 119898-dimensionalpiecewise linear component functions
119910ℎ (119909) =
119873
sum
119896=0
119882119896 (119909) 119862119896 119911
ℎ (119909) =
119873
sum
119896=0
119882119896 (119909)119863119896 (36)
where 119882119896(119909) = 119908
119896(119909)119868119898times119898
119910ℎ(119909) is the sought numerical
solution and 119911ℎ(119909) is a numerical variation In particular the
constant vectors 119862119896are to be determined by the algorithm
developed by us and the constant vectors 119863119896are arbitrary
Thus the discretized form of (20) is obtained in eachsubinterval [119886
119896minus1 119886119896+1] For the sake of clarity of exposition
we assume that119898 = 1 Note that 1198681015840(119910 119911) in (20) is linear in 119911so that a three-term relationship may be obtained at 119909 = 119886
119896
by choosing 119911(119909) = 119908119896(119909) for 119896 = 1 2 119873 minus 1 Thus
In these equations 119886lowast119896= (119886119896+ 119886119896+1)2 and 119910
119896= 119910ℎ(119886119896) is
the computed value of 119910ℎ(119909) at 119886
119896 In the general case when
119898 gt 1 the same result is obtained but 1198911199101015840 and 119891
119910are
column 119898-vectors of functions with 119894th component 1198911199101198941015840 and
119891119910119894 respectively Similarly (119910
119896+119910119896minus1)2 is the119898-vector which
is the average of the119898-vectors 119910ℎ(119886119896) and 119910
ℎ(119886119896minus1)
By the same arguments that led to (38)
1198911199101015840 (119886lowast
119873minus1119910119873+ 119910119873minus1
2119910119873minus 119910119873minus1
ℎ)
+ℎ
2119891119910(119886lowast
119896minus1119910119873+ 119910119873minus1
2119910119873minus 119910119873minus1
ℎ) = 0
(39)
which is the numerical equivalent of the transversality condi-tion (23) For further details see [3 Chapter 6]
It has been shown in [21] that with this method the globalerror has a priori global reduction ratio of119874(ℎ2) In practiceif the step size ℎ is halved the error decreases by 4
7 Implementation and Results
Several numerical experiments have been carried out forboth 119877119877 and 119877119877 planar horizontal underactuated robotmanipulators
71 Planar Horizontal Underactuated 119877119877 Robot ManipulatorIn this section the optimal control problem of a planar hor-izontal underactuated 119877119877 is studied In this robot model thesecond joint is not actuated thus 119906 = (119906
1 0)119879 In this case
it is neither possible to integrate partially nor completely thenonholonomic constraint because the manipulator inertiamatrix contains terms in 120579
2(see [12]) Hence the system is
controllable The numerical results of the application of ourmethod for optimal control to a boundary value problem andto an initial value problem for this system will be described
For a planar horizontal underactuated119877119877 (2) can be splitinto
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792
minus 120573 sin 1205792(2 12057911205792+ 1205792
2) = 1199061
(120575 + 120573 cos 1205792) 1205791+ 120575 1205792+ 120573 sin 120579
21205792
1= 0
(40)
To express optimal control problems that involve this second-order differential constraints in the form of a basic optimal
8 Abstract and Applied Analysis
control problem we have first to convert it into first-orderdifferential constraints introducing the following change ofvariables
1199091= 1205791 119909
3= 1205791
1199092= 1205792 119909
4= 1205792
(41)
with the following additional relations
1199091015840
1= 1199093
1199091015840
2= 1199094
(42)
Thus the second-order differential equations (40) are con-verted into the first-order differential equations
(120572 + 2120573 cos1199092) 1199091015840
3+ (120575 + 120573 cos119909
2) 1199091015840
4
minus 120573 sin1199092(211990931199094+ 1199092
4) = 1199061
(43)
(120575 + 120573 cos1199092) 1199091015840
3+ 1205751199091015840
4+ 120573 sin119909
21199092
3= 0 (44)
Relations (42) (43) and (44) are now the differential con-straints of the optimal control problem and the objectivefunctional to minimize is
119869 = int
119905119865
119905119868
1199062
1119889119905 (45)
Then we introduce the following new variables
X = [
[
1198831
sdot sdot sdot
1198839
]
]
(46)
such that
119883119894= 119909119894 119894 = 1 4 (47)
1198831015840
5= 1199061 119883
5(119905119868) = 0 (48)
where 11988310158406with 119883
6(119905119868) = 0 is the multiplier associated with
whereas in the initial value problem 119883119894(119905119865) will be free for
some 119894The initial values of control variables and multipliers
have been set to zero whereas their final values have notbeen assigned in both optimal control problems Thereforetransversality conditions are needed in both cases for thevariables119883
119894(119905119865) 119894 = 5 10 and they will be of the form
Figure 2 Sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 corresponding to the optimal solutionof problem 1 a boundary value problem for the planar 119877119877 robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad]
1205791(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads] obtained with a
discretization of [119905119868 119905119865] into 64 subintervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control and
state variables are represented in Figure 3
The initial values of control variable and of the multipliershave been set to zero whereas their final values are left freeFigure 2 shows the sequence of configurations of the robot attimes 119905 = 11989632 119896 = 0 1 32 Since the configurations ofthe sequence overlap it has been split into smaller sequencesfor a better visualization of the manipulator motion Figure 3depicts the corresponding control and state variables of theoptimal solution of this boundary value problem obtainedwith a discretization of the time interval [119905
119868 119905119865] into 64
subintervals The value of the objective functional for thissolution is 345185 [J]
712 Problem 2 Initial Value Problem An initial valueproblem has also been solved with the following initial andfinal conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) =
120587
2[rad]
1205792(119905119868) = minus
120587
2[rad] 120579
2(119905119865) = minus
120587
2[rad]
1205791(119905119868) = 0 [rads] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(55)
The initial values of control variable and of the multipliershave been set to zero whereas their final values are left freeThe only difference between these conditions and those of theboundary value problem described in Section 711 is that now1205791(119905119865) = free
Figures 4 and 5 depict the sequence of configurations the119877119877 robot manipulator and the corresponding control andstate variables of the optimal solution of this initial valueproblem respectively obtained with a discretization of thetime interval [119905
119868 119905119865] into 64 subintervals The value of the
objective functional for this solution is 56472 [J]This value islower than the value of the objective functional of the solutionof the boundary value problem described in Section 711because now is 120579
1(119905119865) = free and the control system does
not have to spend energy to stop it
72 Planar Horizontal Underactuated 119877119877 Robot ManipulatorIn this section the optimal control problem of a planarhorizontal underactuated 119877119877 robot manipulator is studiedIn this robot model the first joint is not actuated thus 119906 =
(0 1199062)119879 and (2) can be split into
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792
minus 120573 sin 1205792(2 12057911205792+ 1205792
2) = 0
(56)
(120575 + 120573 cos 1205792) 1205791+ 120575 1205792+ 120573 sin 120579
21205792
1= 1199062 (57)
As explained in [12] since gravity terms are all zero and1205791does not intervene in the system inertia matrix (56) can
be partially integrated to
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792+ 1198881= 0 (58)
Actually constraint (56) is completely integrable giving rise toan holonomic constraintThe resulting holonomic constrainttakes different forms depending on the value of 119888
1which
depends on the initial conditions Therefore two cases havebeen considered
(i) when the initial velocities 1205791(119905119868) and 120579
2(119905119868) are both
zero(ii) when the initial velocity 120579
1(119905119868) is nonzero
721 Problem 3 Initial Value Problem with Zero Initial Veloc-ities An initial value problem has been solved with the fol-lowing initial and final conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rad] 120579
2(119905119865) = 120587 [rad]
1205791(119905119868) = 0 [rads] 120579
1(119905119865) = 0 [rads]
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(59)
10 Abstract and Applied Analysis
10 20 30 40 50 60
05
1
15
2
25
(a) 1205791
10 20 30 40 50 60minus05
minus1
minus15
minus2
minus25
minus3
(b) 1205792
10 20 30 40 50 60
minus10
minus5
5
10
(c) 1205791
10 20 30 40 50 60
minus10
minus5
5
10
15
(d) 1205792
10 20 30 40 50 60
minus300
minus200
minus100
100
200
300
(e) 1199061
Figure 3 Control and state variables of the optimal solution of problem 1 a boundary value problem for the planar 119877119877 robot manipulatorwith boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 2
Figure 4 Sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 corresponding to the optimal solutionof problem 2 an initial value problem for an 119877119877 robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) =
0 [rads] 1205792(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained with a discretization of
[119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control and state variables are
represented in Figure 5
Abstract and Applied Analysis 11
10 20 30 40 50 60
05
1
15
2
25
(a) 1205791
10 20 30 40 50 60
minus25
minus2
(b) 1205792
10 20 30 40 50 60
minus4
minus2
2
4
6
8
10
(c) 1205791
minus4
minus6
minus210 20 30 40 50 60
2
4
(d) 1205792
10 20 30 40 50 60minus50
50
100
150
(e) 1199061
Figure 5 Control and state variables of the optimal solution of problem 2 an initial value problem for a119877119877 robotmanipulator with boundaryconditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = free
and 1205792(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of configurations of the
robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 4
The initial values of the control variable and of themultipliershave been set to zero whereas their final value is left free
Since there is a holonomic constraint that relates thevalues of the angles 120579
1and 120579
2 without integrating (58) we
are not able to find the value of 1205791(119905119865) consistent with 120579
1(119905119868)
Therefore no final conditions have been imposed on 1205791
From the initial conditions of the problem we obtain 1198881=
0 Equation (58) with 1198881= 0 corresponds to the homogeneous
differential constraint
119889120577 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792= 0 (60)
The differential 119889120577 is not exact However it becomes an exactdifferential if multiplied by the factor 1(120572 + 2120573 cos 120579
2) This
operation does not alter the differential equation (60) Inthis case there does exist a function 120577 whose differentialcoincides with the expression 119889120577(120572 + 2120573 cos 120579
2) Due to
the existence of this function the integral of 119889120577 between
two points depends only on these points and not on theintegration path Equation (60) rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2)
120572 + 2120573 cos 1205792
1198891205792
(61)
can be integrated by separating variables The correspondingholonomic constraint has the following expression
1205791= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1205792
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1205792
2+ 1198882
(62)
To express this optimal control problem in the form ofa basic optimal control problem we first have to convert (57)
12 Abstract and Applied Analysis
into a first-order differential model introducing the followingchange of variables
1199091= 1205791 119909
3= 1205791
1199092= 1205792 119909
4= 1205792
(63)
with the following additional relations
1199091015840
1= 1199093
1199091015840
2= 1199094
(64)
Thus the optimal control problem is to minimize
int
1
0
1199062
2119889119905 (65)
subject to the constraints1199091
= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1199092
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1199092
2+ 1198882
(66)
(120575 + 120573 cos 1205792) 1199091015840
3+ 1205751199091015840
4+ 120573 sin119909
21199092
3= 1199062 (67)
and the additional constraints (64) To reformulate thisoptimal control problem as an unconstrained calculus ofvariations problem let X be
X = [
[
1198831
sdot sdot sdot
1198839
]
]
(68)
such that119883119894= 119909119894 119894 = 1 4
1198831015840
5= 1199061 119883
5(119905119868) = 0
(69)
where 11988310158406with 119883
6(119905119868) = 0 is the multiplier associated with
the holonomic constraint (66) 11988310158407with 119883
7(119905119868) = 0 is the
multipliers associated with the differential constraint (67)and 1198831015840
8with 119883
8(119905119868) = 0 and 1198831015840
9with 119883
9(119905119868) = 0 are the
multipliers associatedwith the additional equality constraints(64)
Thus the holonomic constraint of the problem can berewritten as follows
120593 (119905X)
= 1198831minus((120572 minus 2120575)
times tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1198832
2)]]
]
)
times (radic41205732 minus 1205722)
minus1
+1198832
2= 0
(70)
Now the technique described in Section 51 to deal withholonomic constraints can be applied to 120593(119905X) and thisholonomic constraint is replaced by
120593119905+ 1205931198831198831015840= 0
120593 (0 119883 (0)) = 0
(71)
From the initial conditions of the problem the latter equationreduces to the equality 0 = 0 whereas the former takes thefollowing form
(120572 + 2120573 cos (1198832))1198833+ (120575 + 120573 cos (119883
2))1198834= 0 (72)
The corresponding Hamiltonian is
1198661= 11988310158402
5+ 1198831015840
6((120572 + 2120573 cos (119883
2))1198833
+ (120575 + 120573 cos (1198832))1198834)
+1198831015840
7(120573 sin (119883
2)1198832
3+ (120575 + 120573 cos (119883
2))1198831015840
3
+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(73)
It is not difficult to check that matrix 1198661X1015840X1015840
is singularThis is due to the fact that to handle our optimal controlproblemwhich involves second-order differential constraintswe converted them into first-order differential constraintsTherefore we apply again the technique of Section 51obtaining the identity 0 = 0 and the following constraint
minus 120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4= 0
(74)
The corresponding Hamiltonian is
1198662= 11988310158402
5+ 1198831015840
6
times (minus120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4)
+ 1198831015840
7(120573 sin (119883
2)1198832
3+(120575+120573 cos (119883
2))1198831015840
3+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(75)
It is not difficult to check that matrix 1198662X1015840X1015840
in this case is notsingular since its determinant is
Substituting the values of120572120573 and 120575 this expression becomesdet(119866
2X1015840X1015840) = 1128(43 minus 2 cos(2119883
2))2 which is always
positive for any real value1198832 Figure 6 shows the sequence of
configurations of the robot at times 11989632with 119896 = 0 1 32and Figure 7 depicts control and state variables of the optimal
Abstract and Applied Analysis 13
Figure 6 Sequence of configurations of the robot manipulator attimes 11989632 with 119896 = 0 1 32 corresponding to the optimal sol-ution of problem 3 an initial value problem for an underactuated119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) =
0 [rad] 1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads]
1205791(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2
(119905119865) = 0 [rads] obtained with a discretization of [119905
119868 119905119865] into 64
subintervals The initial and final times are 119905119868= 0 and 119905
119865=
1 [s] respectively The corresponding control and state variables arerepresented in Figure 7
solution obtained with a discretization of the interval [119905119868 119905119865]
into 64 subintervalsIn particular we get 120579
1(119905119865) = minus110248 [rad] To check the
consistency of this result with the holonomic constraint (62)since 120579
1(119905119868) = 1205792(119905119868) = 0 [rads] we get from (58) that 119888
1= 0
and using the initial condition 1205791(119905119868) = 1205792(119905119868) = 0 [rad] we
get from (62) that 1198882= 0 Having established the value of the
constant 1198882 we obtain from the same equation for 120579
2(119905119865) =
120587 [rad] that 1205791(119905119865) = minus110248 [rad] which coincides with
the value of 1205791(119905119865) obtained numerically
722 Problem 4 Initial Value Problem with Nonzero InitialVelocity 120579
1 Another initial value problem has been solved
with the following conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rad] 120579
2(119905119865) = 120587 [rad]
1205791(119905119868) = 5 [rads] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(77)
The initial values of the multipliers have been set to zerowhereas their final value is left free Notice that no finalconditions have been imposed on 120579
1and 120579
1 The same
considerations done in previous section hold in this caseas well The technique described in Section 51 must beapplied twice leading to the differential constraint (74) andto the Hamiltonian (75) Figure 8 shows the sequence ofconfigurations of the robot at times 11989632with 119896 = 0 1 32and Figure 9 depicts the control and state variables ofthe optimal solution obtained with a discretization of theinterval [119905
119868 119905119865] into 64 subintervals In particular we get that
1205791(119905119865) = 617172 [rad] and 120579
1(119905119865) = 900163 [rads] To
check the consistency of the obtained value of 1205791(119905119865) with
the holonomic constraint consider (58) We can calculate
the constant 1198881using the initial conditions of the problem
obtaining
1198881= minus 120579
1(119905119868) (120572 + 2120573 cos 120579
2(119905119868))
minus 1205792(119905119868) (120575 + 120573 cos 120579
2(119905119868)) = minus225
(78)
Since 1198881= 0 (58) corresponds in this case to the differential
constraint
119889120578 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792+ 1198881119889119905 = 0
(79)
Equation (79) can be rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2 (119905))
120572 + 2120573 cos 1205792 (119905)
1198891205792minus
1198881
120572 + 2120573 cos 1205792 (119905)
119889119905 (80)
To check the obtained value of 1205791(119905119865) 1198891205791is numerically
integrated between 1205791(119905119868) and 120579
1(119905119865) using the interpolated
numerical optimal solution obtained for 1205792(119905) We get that
1205791(119905119865) = 618705 This value is close to 617172
To check the consistency of the obtained value of 1205791(119905119865)
with the constraint (58) using the computed value 1198881= minus225
and the final conditions 1205792(119905119865) = 0 120579
2(119905119865) = 120587 of the problem
we obtain
1205791(119905119865) =
minus (120575 + 120573 cos 1205792(119905119865))
120572 + 2120573 cos 1205792(119905119865)
1205792(119905119865)
minus1198881
120572 + 2120573 cos 1205792(119905119865)= 9
(81)
This value is very close to the value of 1205791(119905119865) obtained
numerically
73 Computational Issues If the optimal control problem has119898 variables and the time interval [119905
119868 119905119865] has been discretized
into 119873 subintervals the resulting set of difference equations(38) has119898times(119873minus1) equations and119898times(119873minus1) variables plusthe equations and variables due to transversality conditionsFeasible solutions have been used as initial guesses of thealgorithm
The solution of the nonlinear system of difference equa-tions (38) has been obtained using a damped Newtonalgorithm within a line search methodology implementedin Mathematica 7 under Mac OS X operating system (see[22 23] for more details)
8 Conclusion
In this paper the trajectory planning problem for planarunderactuated robot manipulators with two revolute jointswithout gravity has been studied This problem is solved asan optimal control problem based on a numerical resolutionof an unconstrained variational calculus reformulation of theoptimal control problem in which the dynamic equation ofthe mechanical system is regarded as a constraint It hasbeen shown that this reformulation method based on special
14 Abstract and Applied Analysis
10 20 30 40 50 60
minus1
minus08
minus06
minus04
minus02
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
minus15
minus1
minus05
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
(d) 1205792
10 20 30 40 50 60
minus15
minus10
5
10
15
minus5
(e) 1199062
Figure 7 Control and state variables of the optimal solution of problem 3 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 119906
2(119905119868) = 0Nm 120579
1(119905119865) = free
1205792(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads]The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectivelyThe corresponding
sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 6
Figure 8 Sequence of configurations of the robot manipulator at times 11989632 with 119896 = 0 1 32 corresponding to the optimal solutionof problem 4 an initial value problem for an underactuated 119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad]
1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained
with a discretization of [119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control
and state variables are represented in Figure 9
Abstract and Applied Analysis 15
10 20 30 40 50 60
1
2
3
4
5
6
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
4
5
6
7
8
9
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
6
(d) 1205792
10 20 30 40 50 60minus5
5
10
15
20
(e) 1199062
Figure 9 Control and state variables of the optimal solution of problem 4 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 8
derivative multipliers is able to tackle both integrable andnonintegrable differential constraints of the dynamic modelsof underactuated planar horizontal robot manipulators withtwo revolute joints This method can be seamlessly appliedin the presence of additional constraints on the mechanicalsystem
References
[1] A De Luca S Iannitti R Mattone and G Oriolo ldquoUnderactu-ated manipulators control properties and techniquesrdquoMachineIntelligence and Robotic Control vol 4 no 3 pp 113ndash125 2002
[2] G A Bliss Lectures on the Calculus of Variations University ofChicago Press Chicago Ill USA 1946
[3] J Gregory and C Lin Constrained Optimization in the Calculusof Variations and Optimal Control theory Chapman amp Hall1996
[4] W-S Koon and J E Marsden ldquoOptimal control for holonomicand nonholonomic mechanical systems with symmetry andLagrangian reductionrdquo SIAM Journal on Control and Optimiza-tion vol 35 no 3 pp 901ndash929 1997
[5] A M Bloch Nonholonomic Mechanics and Control SpringerNew York NY USA 2003
[6] I I Hussein and A M Bloch ldquoOptimal control of underactu-ated nonholonomic mechanical systemsrdquo IEEE Transactions onAutomatic Control vol 53 no 3 pp 668ndash682 2008
[7] X Z Lai J H She S X Yang andMWu ldquoComprehensive uni-fied control strategy for underactuated two-link manipulatorsrdquoIEEE Transactions on Systems Man and Cybernetics B vol 39no 2 pp 389ndash398 2009
[8] J P Ordaz-Oliver O J Santos-Sanchez and V Lopez-MoralesldquoToward a generalized sub-optimal control method of underac-tuated systemsrdquo Optimal Control Applications amp Methods vol33 no 3 pp 338ndash351 2012
16 Abstract and Applied Analysis
[9] R Seifried ldquoTwo approaches for feedforward control andoptimal design of underactuatedmultibody systemsrdquoMultibodySystem Dynamics vol 27 no 1 pp 75ndash93 2012
[10] M Buss O von Stryk R Bulirsch and G Schmidt ldquoTowardshybrid optimal controlrdquo atmdashAutomatisierungstechnik vol 48no 9 pp 448ndash459 2000
[11] M Buss M Glocker M Hardt O von Stryk R Bulirsch andG Schmidt ldquoNonlinear hybrid dynamical systems modelingoptimal control and applicationsrdquo in Modelling Analysis andDesign of Hybrid Systems S Engell G Frehse and E SchniederEds vol 279 of Lecture Notes in Control and InformationScience pp 331ndash335 Springer 2002
[12] G Oriolo and Y Nakamura ldquoControl of mechanical systemswith second-order nonholonomic constraints underactuatedmanipulatorsrdquo in Proceedings of the 30th IEEE Conference onDecision and Control pp 2398ndash2403 December 1991
[13] T J Tarn M Zhang and A Serrani ldquoNew integrability condi-tions for differential constraintsrdquo Systems and Control Lettersvol 49 no 5 pp 335ndash345 2003
[14] H J Sussmann ldquoA general theorem on local controllabilityrdquoSIAM Journal on Control and Optimization vol 25 no 1 pp158ndash194 1987
[15] F Bullo A D Lewis and K M Lynch ldquoControllable kinematicreductions for mechanical systems concepts computationaltools and examplesrdquo in Proceedings of International Symposiumon Mathematical Theory of Networks and Systems 2002
[16] F Bullo and A D Lewis ldquoLow-order controllability and kine-matic reductions for affine connection control systemsrdquo SIAMJournal on Control andOptimization vol 44 no 3 pp 885ndash9082006
[17] A D Lewis and R MMurray ldquoConfiguration controllability ofsimple mechanical control systemsrdquo SIAM Journal on Controland Optimization vol 35 no 3 pp 766ndash790 1997
[18] F Bullo and K M Lynch ldquoKinematic controllability for decou-pled trajectory planning in underactuatedmechanical systemsrdquoIEEE Transactions on Robotics and Automation vol 17 no 4 pp402ndash412 2001
[19] M R Hestenes Calculus of Variations and Optimcl ControlTheory John Wiley amp Sons 1966
[20] J Gregory ldquoA new systematic method for efficiently solvingholonomic (and nonholonomic) constraint problemsrdquo Analysisand Applications vol 8 no 1 pp 85ndash98 2010
[21] J Gregory and R S Wang ldquoDiscrete variable methods forthe m-dependent variable nonlinear extremal problem in thecalculus of variationsrdquo SIAM Journal onNumerical Analysis vol27 no 2 pp 470ndash487 1990
[22] Wolfram Research 2012[23] J J More and D J Thuente ldquoLine search algorithms with guar-
anteed sufficient decreaserdquo ACM Transactions on MathematicalSoftware vol 20 no 3 pp 286ndash307 1994
Optimal control approaches to trajectory planning ass-ume that there exists a control input that steers the systembetween two specify states Thus controllability is the mostimportant aspect to check before studying optimal control ofa dynamic system If in the trajectory planning problem theduration of the motion 119879 is not assigned the existence of afinite-time solution for any state (120579
119865 120579119865) in a neighborhood
of (120579119868 120579119868) is equivalent for the robotic system to the property
of local controllability at (120579119868 120579119868) If local controllability holds
at any state then the system is controllable and the trajectoryplanning problem is solvable for any pair of initial and finalstates
For underactuated119877119877 robotmanipulators controllabilityis related to integrability of the second-order nonholonomicconstraints The second-order differential constraint (6) mayeither be partially integrable to a first-order differentialequation or completely integrable to a holonomic equationNecessary and sufficient integrability conditions are givenin [12 13] If (6) is not partially integrable it is possible tosteer the system between equilibrium points This occursfor planar underactuated 119877119877 robot manipulators withoutgravity which therefore are controllable If (6) is completelyintegrable to a holonomic constraint the motion of themechanical system is restricted to a 1-dimensional sub-manifold of the configuration space which depends on theinitial configuration This occurs for the dynamic equationsof planar underactuated 119877119877 robot manipulators withoutthe effects of the gravity [12] For this robot model thetrajectory planning problem has solution only for particularinitial and final states Thus when (6) is not partially orcompletely integrable the mechanical system is controllableHowever several aspects of controllability can be studiedwhich characterize this model of underactuatedmanipulator
A dynamical system is linearly controllable at an equi-librium point if the linear approximation of the systemaround this point is controllable Planar underactuated 119877119877robot manipulators in the absence of gravity are not linearlycontrollable On the contrary both planar underactuated 119877119877and 119877119877 robot manipulators are linearly controllable in thepresence of gravity
A mechanical system is said to be small-time locallycontrollable (STLC) at 119909
119868= (120579119868 120579119868) if for any neighborhood
V119909of 119909119868and any time 119879 gt 0 the set RV119909 119879
(119909119868) of states
that are reachable from 119909119868within time 119879 along trajectories
contained in V includes a neighborhood of 119909119868 Note that
small-time local controllability is a stronger property thancontrollability [14] Non-STLC but controllable system mustin general perform finite maneuvers in order to performarbitrarily small changes of configuration It has been provenin [15 16] that planar underactuated 119877119877 robot manipulatorsin the absence of gravity are not STLC Both planar under-actuated 119877119877 and 119877119877 robot manipulators in the presence ofgravity are also not STLC
Second-order mechanical systems cannot be STLC atstates with nonzero velocity Therefore the weaker conceptof small-time local configuration controllability has beenintroduced [17] A system is said to be small-time local
configuration controllable (STLCC) at a configuration 120579119868if
for any neighborhood V120579of 120579119868in the configuration space
and any time 119879 gt 0 the set RV120579119879(120579119868) of configurations that
are reachable (with some final velocity 120579) within 119879 startingfrom (120579
119868 0) and along a path in configuration space contained
in V120579 includes a neighborhood of 120579
119868 By definition STLC
systems are also STLCC Sufficient conditions for STLCC
are given in [17] It has been proven in [15 16] that planarunderactuated 119877119877 robot manipulators in the absence ofgravity are not STLCC Both planar underactuated 119877119877 and119877119877 robot manipulators in the presence of gravity are also notSTLCC
A final question is to investigate if the trajectory planningproblem for119877119877planar underactuated robotmanipulators canbe solved with algorithmic methods A mechanical systemis kinematically controllable (KC) if every configuration isreachable by means of a sequence of kinematic motions thatis feasible paths in the configuration space which may befollowed with any arbitrary timing law [15 16 18] Note thatKC mechanical systems are also STLCC and that kinematiccontrollability does not imply small-time local controllabilityIf amechanism isKC the trajectory planning problemmaybesolved with algorithmic methods Planar underactuated 119877119877robotmanipulators in the absence of gravity are notKC Bothplanar underactuated 119877119877 and 119877119877 robot manipulators in thepresence of gravity are not KC [15]
4 The Optimal Control Problem
Given the dynamic equation of an underactuated planar119877119877 robot manipulator an initial state (120579
119868 120579119868) and a final
state (120579119865 120579119865) the optimal control problem consists in finding
the available control input 1199061(119905) or 119906
2(119905) and the resulting
trajectory with 119905 isin [119905119868 119905119865] that steers the system between
initial and final states satisfying the dynamic equation (5) andminimizing the objective functional
119869 = int
119905119865
119905119868
1199062
119894119889119905 (7)
where 119894 = 1 2 depending on which joint is actuated and119905119868and 119905119865are the initial and final time values respectively
This cost functional represents a measure of the energyconsumed during the motion since torque produced withan electromechanical actuator is approximately proportionalto the current flow and the rate of energy consumption isapproximately equal to the square of this current
If 120579119868= 120579119865= 0 the problem is called rest-to-rest trajectory
planning problem If the final or the initial states or part ofthem is not assigned the problems are called initial valueproblem and final value problem respectively The final time119905119865may be fixed or notThis problem is a particular case of an optimal control
problem which can be stated in a more general form asfollows Minimize the integral
where 119909(119905) = (1199091(119905) 1199092(119905) 119909
119899(119905))119879 is an 119899-vector called
the state vector 119906(119905) = (1199061(119905) 1199062(119905) 119906
119898(119905))119879 is an 119898-
vector called the control vector the real-valued function119869(119909 119906) is the objective functional (9) is called the trajectoryequation and the conditions (14) are called the boundaryconditions The set 119880 sub R119898 is called the set of controls with119906(119905) isin 119880 for every 119905 isin [119905
119868 119905119865] We assume that 119891 119892 ℎ 119897 119901
and 119902 are sufficiently smooth for our purposeThis will implysolutions such that 119909(119905) is piecewise smooth whereas 119906(119905) ispiecewise continuous [19]
5 Variational Reformulation of the OptimalControl Problem
A variational approach has been used to solve the more gen-eral optimal control problem stated in the previous section
The classical calculus of variations problem is tominimizean integral of the form
where the independent 119909 variable is assumed to be in theinterval [119886 119887] and the dependent variable 119910 = 119910(119909) = (119910
1(119909)
1199102(119909) 119910
119899(119909))119879 is assumed to be an 119899-vector continuous on
[119886 119887] with derivative 1199101015840 = 1199101015840(119909) = (11991010158401(119909) 1199101015840
2(119909) 119910
1015840
119899(119909))119879
It is also assumed that 119910 is piecewise smooth that is thereexists a finite set of points 119886
1 1198862 119886
119896so that 119886 le 119886
1lt 1198862lt
sdot sdot sdot lt 119886119896le 119887 119910(119909) is continuously differentiable on (119886
119897 119886119897+1)
and that the respective left- and right-handed limits of 1199101015840(119909)exist If 119910(119909) is piecewise smooth and satisfies the boundaryconditions 119910(119886) = 119860 119910(119887) = 119861 then 119910(119909) is said to be anadmissible arc In words this problem consists in findingamong all arcs connecting end points (119886 119860) and (119887 119861) theone minimizing the integral (16)
The main optimality conditions are obtained by defininga variation 119911(119909) a set of functions
where 120575 gt 0 is a fixed real number and the variation 119911(119909) isa piecewise smooth function with 119911(119886) = 119911(119887) = 0 Usinga Taylor series expansion it is easy to see that a necessarycondition that 0 is a relative minimum to 119865 is
where 119891119910 1198911199101015840 denote the partial derivatives of 119891 evaluated
along (119909 119910(119909) 1199101015840(119909)) and the terms 119911 and 1199111015840 are evaluated at119909
Integrating (20) by parts for all admissible variations 119911(119909)another necessary condition for 119910 = 119910(119909) to give a relativeminimum of the variational problem (16)-(17) is obtainedwhich is the following second-order differential equation
known as Euler-Lagrange conditionThis equationmust holdalong (119909 119910(119909) 1199101015840(119909)) except at a finite number of points [3Section 21]
The extremals of (16)-(17) can be obtained by solving theEuler-Lagrange equation but it only holds at points where theextremal 119910lowast(119909) is smooth At points where 119910lowast1015840(119909) has jumpscalled corners the Weierstrass-Erdmann corner conditionsmust be fulfilled [3 Section 23] Since the location of thecorners their number and the amplitudes of the jumps in1199101015840lowast(119909) 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 ourmethodis that the integral form of this condition
holds for all 119909 isin [119886 119887] and some 119888 and therefore theWeierstrass-Erdmann corner conditions are not neededThus an alternative way of computing the extremals can bebased on this necessary condition in integral form
Note that necessary condition requires that boundaryvalues fulfill Euler-Lagrange equation Thus if some of thefour values 119886 119910(119886) 119887 and 119910(119887) are not explicitly givenalternate boundary conditions have to be provided This iswhat transversality conditions do Assume that 119886 119910(119886) and 119887are given but119910(119887) is free In this case the additional necessarytransversality condition
1198911199101015840 (119887 119910
lowast(119887) 119910
1015840lowast(119887)) = 0 (23)
must holdThe variational approach does not consider constraints
However the optimal control problem has at least a first-order differential constraint (9) representing the dynamicequation of the system Moreover since the dynamic equa-tion of a planar 119877119877 robot manipulator is a second-order
6 Abstract and Applied Analysis
differential equation additional differential constraints willarise while rewriting it as a first-order differential equationTherefore the optimal control problemmust be reformulatedas an unconstrained calculus of variations problem in orderto deal with differential and algebraic constraints as describedin the following section
Following [3 Chapter 5] we reformulate as an uncon-strained calculus of variations problem the optimal controlproblem consisting inminimizing (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 variablefrom 119905 to 119909 and the dependent variable from 119909 to 119910 to beconsistent with the notation of calculus of variations Ourreformulation is based on special derivative multipliers anda change of variables in which
1199101(119909) = 119910(119909) is the renamed state vector
1199101015840
2(119909) = 119906(119909) is the renamed state vector
1199101015840
3(119909) is the multiplier associated with (9)
1199101015840
4(119909) is themultiplier associated with constraint (10)
1199101015840
5(119909) is the multiplier associated with constraint (11)
1199101015840
6(119909) is the excess variable of constraint (11)
Since 1199102(119909) 119910
6(119909) are not unique without an extra condi-
tion we initialize these variables by defining 119910119894(119909119868) = 0 119894 =
1 6 Thus our problem becomes
min 119868 (Y) = int119909119865
119909119868
119865 (119909YY1015840) 119889119909 (24)
where
Y = (1199101 1199102 1199103 1199104 1199105 1199106)119879
119865 = 119891 (119909 1199101 1199101015840
2) + 1199101015840119879
3(1199101015840
1minus 119892 (119909 119910
1 1199101015840
2))
+ 1199101015840119879
4ℎ (119909 119910
1 1199101015840
2) + 1199101015840119879
5(119897 (119909 119910
1 1199101015840
2) + 11991010158402
6)
(25)
Since the values of 119910119894(119909119865) 119894 = 2 6 are unknown
transversality conditions are needed having the form
In the above lines 119910(119909) is an 119899-vector and120595 120593 120601 are assumedto be differentiable in their arguments or with the neededsmoothness We also assume that 120595
11991010158401199101015840 gt 0 The boundary
conditions of the problems are any combination of fixedboundary conditions for the components of 119910 with thepossibility of leaving some of them unspecified
If we reformulate problem (27)-(28) using the techniquedescribed in Section 5 we get the following HamiltonianΨ(119909 119884 119884
1015840) = 120595(119909 119910
1 1199101015840
1) + 1199101015840
2120593(119909 119910
1) with 119910
1(119909) = 119910(119909)
where 11991010158402is the multiplier We have in this case
Ψ11988410158401198841015840 = [
1205951199101015840
11199101015840
1
0
0 0] (31)
which is singular The singularity of Ψ11988410158401198841015840 is a difficulty we
must avoid Furthermore even when it is not difficult tochange from the120593 constraint to the120601 constraint by increasingthe dimension of the independent variables it is not easy todeal with the new associated boundary conditionsThis is thereason that problem (27)-(28) is so difficult to solve
It has been shown in [20] that problem (27)-(28) can bereformulated as an equivalent problem of the form (29)-(30)In particular 119910(119909) is a solution to (27)-(28) if and only if 119910(119909)is a solution to
The numerical method used is based on the discretizationof the unconstrained variational calculus problem stated inthe previous section In particular the main underlying ideais obtaining a discretized solution 119910
ℎ(119909) solving (20) for all
piecewise linear spline function variations 119911(119909) instead of
Abstract and Applied Analysis 7
dealing with the Euler-Lagrange equation (21) Thus thismethod uses no numerical corner conditions and avoidspatching solutions to (21) between corners
Let 119873 be a large positive integer ℎ = (119887 minus 119886)119873 and let120587 = (119886 = 119886
0lt 1198861lt sdot sdot sdot lt 119886
119873= 119887) be a partition of the
interval [119886 119887] where 119886119896= 119886 + 119896ℎ for 119896 = 0 1 119873 Define
the one-dimensional spline hat functions
119908119896 (119909) =
119909 minus 119886119896minus1
ℎif 119886119896minus1
lt 119909 lt 119886119896
119886119896+1
minus 119909
ℎif 119886119896lt 119909 lt 119886
119896+1
0 otherwise
(35)
where 119896 = 1 2 119873 minus 1 Define also the 119898-dimensionalpiecewise linear component functions
119910ℎ (119909) =
119873
sum
119896=0
119882119896 (119909) 119862119896 119911
ℎ (119909) =
119873
sum
119896=0
119882119896 (119909)119863119896 (36)
where 119882119896(119909) = 119908
119896(119909)119868119898times119898
119910ℎ(119909) is the sought numerical
solution and 119911ℎ(119909) is a numerical variation In particular the
constant vectors 119862119896are to be determined by the algorithm
developed by us and the constant vectors 119863119896are arbitrary
Thus the discretized form of (20) is obtained in eachsubinterval [119886
119896minus1 119886119896+1] For the sake of clarity of exposition
we assume that119898 = 1 Note that 1198681015840(119910 119911) in (20) is linear in 119911so that a three-term relationship may be obtained at 119909 = 119886
119896
by choosing 119911(119909) = 119908119896(119909) for 119896 = 1 2 119873 minus 1 Thus
In these equations 119886lowast119896= (119886119896+ 119886119896+1)2 and 119910
119896= 119910ℎ(119886119896) is
the computed value of 119910ℎ(119909) at 119886
119896 In the general case when
119898 gt 1 the same result is obtained but 1198911199101015840 and 119891
119910are
column 119898-vectors of functions with 119894th component 1198911199101198941015840 and
119891119910119894 respectively Similarly (119910
119896+119910119896minus1)2 is the119898-vector which
is the average of the119898-vectors 119910ℎ(119886119896) and 119910
ℎ(119886119896minus1)
By the same arguments that led to (38)
1198911199101015840 (119886lowast
119873minus1119910119873+ 119910119873minus1
2119910119873minus 119910119873minus1
ℎ)
+ℎ
2119891119910(119886lowast
119896minus1119910119873+ 119910119873minus1
2119910119873minus 119910119873minus1
ℎ) = 0
(39)
which is the numerical equivalent of the transversality condi-tion (23) For further details see [3 Chapter 6]
It has been shown in [21] that with this method the globalerror has a priori global reduction ratio of119874(ℎ2) In practiceif the step size ℎ is halved the error decreases by 4
7 Implementation and Results
Several numerical experiments have been carried out forboth 119877119877 and 119877119877 planar horizontal underactuated robotmanipulators
71 Planar Horizontal Underactuated 119877119877 Robot ManipulatorIn this section the optimal control problem of a planar hor-izontal underactuated 119877119877 is studied In this robot model thesecond joint is not actuated thus 119906 = (119906
1 0)119879 In this case
it is neither possible to integrate partially nor completely thenonholonomic constraint because the manipulator inertiamatrix contains terms in 120579
2(see [12]) Hence the system is
controllable The numerical results of the application of ourmethod for optimal control to a boundary value problem andto an initial value problem for this system will be described
For a planar horizontal underactuated119877119877 (2) can be splitinto
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792
minus 120573 sin 1205792(2 12057911205792+ 1205792
2) = 1199061
(120575 + 120573 cos 1205792) 1205791+ 120575 1205792+ 120573 sin 120579
21205792
1= 0
(40)
To express optimal control problems that involve this second-order differential constraints in the form of a basic optimal
8 Abstract and Applied Analysis
control problem we have first to convert it into first-orderdifferential constraints introducing the following change ofvariables
1199091= 1205791 119909
3= 1205791
1199092= 1205792 119909
4= 1205792
(41)
with the following additional relations
1199091015840
1= 1199093
1199091015840
2= 1199094
(42)
Thus the second-order differential equations (40) are con-verted into the first-order differential equations
(120572 + 2120573 cos1199092) 1199091015840
3+ (120575 + 120573 cos119909
2) 1199091015840
4
minus 120573 sin1199092(211990931199094+ 1199092
4) = 1199061
(43)
(120575 + 120573 cos1199092) 1199091015840
3+ 1205751199091015840
4+ 120573 sin119909
21199092
3= 0 (44)
Relations (42) (43) and (44) are now the differential con-straints of the optimal control problem and the objectivefunctional to minimize is
119869 = int
119905119865
119905119868
1199062
1119889119905 (45)
Then we introduce the following new variables
X = [
[
1198831
sdot sdot sdot
1198839
]
]
(46)
such that
119883119894= 119909119894 119894 = 1 4 (47)
1198831015840
5= 1199061 119883
5(119905119868) = 0 (48)
where 11988310158406with 119883
6(119905119868) = 0 is the multiplier associated with
whereas in the initial value problem 119883119894(119905119865) will be free for
some 119894The initial values of control variables and multipliers
have been set to zero whereas their final values have notbeen assigned in both optimal control problems Thereforetransversality conditions are needed in both cases for thevariables119883
119894(119905119865) 119894 = 5 10 and they will be of the form
Figure 2 Sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 corresponding to the optimal solutionof problem 1 a boundary value problem for the planar 119877119877 robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad]
1205791(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads] obtained with a
discretization of [119905119868 119905119865] into 64 subintervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control and
state variables are represented in Figure 3
The initial values of control variable and of the multipliershave been set to zero whereas their final values are left freeFigure 2 shows the sequence of configurations of the robot attimes 119905 = 11989632 119896 = 0 1 32 Since the configurations ofthe sequence overlap it has been split into smaller sequencesfor a better visualization of the manipulator motion Figure 3depicts the corresponding control and state variables of theoptimal solution of this boundary value problem obtainedwith a discretization of the time interval [119905
119868 119905119865] into 64
subintervals The value of the objective functional for thissolution is 345185 [J]
712 Problem 2 Initial Value Problem An initial valueproblem has also been solved with the following initial andfinal conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) =
120587
2[rad]
1205792(119905119868) = minus
120587
2[rad] 120579
2(119905119865) = minus
120587
2[rad]
1205791(119905119868) = 0 [rads] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(55)
The initial values of control variable and of the multipliershave been set to zero whereas their final values are left freeThe only difference between these conditions and those of theboundary value problem described in Section 711 is that now1205791(119905119865) = free
Figures 4 and 5 depict the sequence of configurations the119877119877 robot manipulator and the corresponding control andstate variables of the optimal solution of this initial valueproblem respectively obtained with a discretization of thetime interval [119905
119868 119905119865] into 64 subintervals The value of the
objective functional for this solution is 56472 [J]This value islower than the value of the objective functional of the solutionof the boundary value problem described in Section 711because now is 120579
1(119905119865) = free and the control system does
not have to spend energy to stop it
72 Planar Horizontal Underactuated 119877119877 Robot ManipulatorIn this section the optimal control problem of a planarhorizontal underactuated 119877119877 robot manipulator is studiedIn this robot model the first joint is not actuated thus 119906 =
(0 1199062)119879 and (2) can be split into
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792
minus 120573 sin 1205792(2 12057911205792+ 1205792
2) = 0
(56)
(120575 + 120573 cos 1205792) 1205791+ 120575 1205792+ 120573 sin 120579
21205792
1= 1199062 (57)
As explained in [12] since gravity terms are all zero and1205791does not intervene in the system inertia matrix (56) can
be partially integrated to
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792+ 1198881= 0 (58)
Actually constraint (56) is completely integrable giving rise toan holonomic constraintThe resulting holonomic constrainttakes different forms depending on the value of 119888
1which
depends on the initial conditions Therefore two cases havebeen considered
(i) when the initial velocities 1205791(119905119868) and 120579
2(119905119868) are both
zero(ii) when the initial velocity 120579
1(119905119868) is nonzero
721 Problem 3 Initial Value Problem with Zero Initial Veloc-ities An initial value problem has been solved with the fol-lowing initial and final conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rad] 120579
2(119905119865) = 120587 [rad]
1205791(119905119868) = 0 [rads] 120579
1(119905119865) = 0 [rads]
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(59)
10 Abstract and Applied Analysis
10 20 30 40 50 60
05
1
15
2
25
(a) 1205791
10 20 30 40 50 60minus05
minus1
minus15
minus2
minus25
minus3
(b) 1205792
10 20 30 40 50 60
minus10
minus5
5
10
(c) 1205791
10 20 30 40 50 60
minus10
minus5
5
10
15
(d) 1205792
10 20 30 40 50 60
minus300
minus200
minus100
100
200
300
(e) 1199061
Figure 3 Control and state variables of the optimal solution of problem 1 a boundary value problem for the planar 119877119877 robot manipulatorwith boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 2
Figure 4 Sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 corresponding to the optimal solutionof problem 2 an initial value problem for an 119877119877 robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) =
0 [rads] 1205792(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained with a discretization of
[119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control and state variables are
represented in Figure 5
Abstract and Applied Analysis 11
10 20 30 40 50 60
05
1
15
2
25
(a) 1205791
10 20 30 40 50 60
minus25
minus2
(b) 1205792
10 20 30 40 50 60
minus4
minus2
2
4
6
8
10
(c) 1205791
minus4
minus6
minus210 20 30 40 50 60
2
4
(d) 1205792
10 20 30 40 50 60minus50
50
100
150
(e) 1199061
Figure 5 Control and state variables of the optimal solution of problem 2 an initial value problem for a119877119877 robotmanipulator with boundaryconditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = free
and 1205792(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of configurations of the
robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 4
The initial values of the control variable and of themultipliershave been set to zero whereas their final value is left free
Since there is a holonomic constraint that relates thevalues of the angles 120579
1and 120579
2 without integrating (58) we
are not able to find the value of 1205791(119905119865) consistent with 120579
1(119905119868)
Therefore no final conditions have been imposed on 1205791
From the initial conditions of the problem we obtain 1198881=
0 Equation (58) with 1198881= 0 corresponds to the homogeneous
differential constraint
119889120577 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792= 0 (60)
The differential 119889120577 is not exact However it becomes an exactdifferential if multiplied by the factor 1(120572 + 2120573 cos 120579
2) This
operation does not alter the differential equation (60) Inthis case there does exist a function 120577 whose differentialcoincides with the expression 119889120577(120572 + 2120573 cos 120579
2) Due to
the existence of this function the integral of 119889120577 between
two points depends only on these points and not on theintegration path Equation (60) rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2)
120572 + 2120573 cos 1205792
1198891205792
(61)
can be integrated by separating variables The correspondingholonomic constraint has the following expression
1205791= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1205792
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1205792
2+ 1198882
(62)
To express this optimal control problem in the form ofa basic optimal control problem we first have to convert (57)
12 Abstract and Applied Analysis
into a first-order differential model introducing the followingchange of variables
1199091= 1205791 119909
3= 1205791
1199092= 1205792 119909
4= 1205792
(63)
with the following additional relations
1199091015840
1= 1199093
1199091015840
2= 1199094
(64)
Thus the optimal control problem is to minimize
int
1
0
1199062
2119889119905 (65)
subject to the constraints1199091
= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1199092
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1199092
2+ 1198882
(66)
(120575 + 120573 cos 1205792) 1199091015840
3+ 1205751199091015840
4+ 120573 sin119909
21199092
3= 1199062 (67)
and the additional constraints (64) To reformulate thisoptimal control problem as an unconstrained calculus ofvariations problem let X be
X = [
[
1198831
sdot sdot sdot
1198839
]
]
(68)
such that119883119894= 119909119894 119894 = 1 4
1198831015840
5= 1199061 119883
5(119905119868) = 0
(69)
where 11988310158406with 119883
6(119905119868) = 0 is the multiplier associated with
the holonomic constraint (66) 11988310158407with 119883
7(119905119868) = 0 is the
multipliers associated with the differential constraint (67)and 1198831015840
8with 119883
8(119905119868) = 0 and 1198831015840
9with 119883
9(119905119868) = 0 are the
multipliers associatedwith the additional equality constraints(64)
Thus the holonomic constraint of the problem can berewritten as follows
120593 (119905X)
= 1198831minus((120572 minus 2120575)
times tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1198832
2)]]
]
)
times (radic41205732 minus 1205722)
minus1
+1198832
2= 0
(70)
Now the technique described in Section 51 to deal withholonomic constraints can be applied to 120593(119905X) and thisholonomic constraint is replaced by
120593119905+ 1205931198831198831015840= 0
120593 (0 119883 (0)) = 0
(71)
From the initial conditions of the problem the latter equationreduces to the equality 0 = 0 whereas the former takes thefollowing form
(120572 + 2120573 cos (1198832))1198833+ (120575 + 120573 cos (119883
2))1198834= 0 (72)
The corresponding Hamiltonian is
1198661= 11988310158402
5+ 1198831015840
6((120572 + 2120573 cos (119883
2))1198833
+ (120575 + 120573 cos (1198832))1198834)
+1198831015840
7(120573 sin (119883
2)1198832
3+ (120575 + 120573 cos (119883
2))1198831015840
3
+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(73)
It is not difficult to check that matrix 1198661X1015840X1015840
is singularThis is due to the fact that to handle our optimal controlproblemwhich involves second-order differential constraintswe converted them into first-order differential constraintsTherefore we apply again the technique of Section 51obtaining the identity 0 = 0 and the following constraint
minus 120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4= 0
(74)
The corresponding Hamiltonian is
1198662= 11988310158402
5+ 1198831015840
6
times (minus120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4)
+ 1198831015840
7(120573 sin (119883
2)1198832
3+(120575+120573 cos (119883
2))1198831015840
3+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(75)
It is not difficult to check that matrix 1198662X1015840X1015840
in this case is notsingular since its determinant is
Substituting the values of120572120573 and 120575 this expression becomesdet(119866
2X1015840X1015840) = 1128(43 minus 2 cos(2119883
2))2 which is always
positive for any real value1198832 Figure 6 shows the sequence of
configurations of the robot at times 11989632with 119896 = 0 1 32and Figure 7 depicts control and state variables of the optimal
Abstract and Applied Analysis 13
Figure 6 Sequence of configurations of the robot manipulator attimes 11989632 with 119896 = 0 1 32 corresponding to the optimal sol-ution of problem 3 an initial value problem for an underactuated119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) =
0 [rad] 1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads]
1205791(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2
(119905119865) = 0 [rads] obtained with a discretization of [119905
119868 119905119865] into 64
subintervals The initial and final times are 119905119868= 0 and 119905
119865=
1 [s] respectively The corresponding control and state variables arerepresented in Figure 7
solution obtained with a discretization of the interval [119905119868 119905119865]
into 64 subintervalsIn particular we get 120579
1(119905119865) = minus110248 [rad] To check the
consistency of this result with the holonomic constraint (62)since 120579
1(119905119868) = 1205792(119905119868) = 0 [rads] we get from (58) that 119888
1= 0
and using the initial condition 1205791(119905119868) = 1205792(119905119868) = 0 [rad] we
get from (62) that 1198882= 0 Having established the value of the
constant 1198882 we obtain from the same equation for 120579
2(119905119865) =
120587 [rad] that 1205791(119905119865) = minus110248 [rad] which coincides with
the value of 1205791(119905119865) obtained numerically
722 Problem 4 Initial Value Problem with Nonzero InitialVelocity 120579
1 Another initial value problem has been solved
with the following conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rad] 120579
2(119905119865) = 120587 [rad]
1205791(119905119868) = 5 [rads] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(77)
The initial values of the multipliers have been set to zerowhereas their final value is left free Notice that no finalconditions have been imposed on 120579
1and 120579
1 The same
considerations done in previous section hold in this caseas well The technique described in Section 51 must beapplied twice leading to the differential constraint (74) andto the Hamiltonian (75) Figure 8 shows the sequence ofconfigurations of the robot at times 11989632with 119896 = 0 1 32and Figure 9 depicts the control and state variables ofthe optimal solution obtained with a discretization of theinterval [119905
119868 119905119865] into 64 subintervals In particular we get that
1205791(119905119865) = 617172 [rad] and 120579
1(119905119865) = 900163 [rads] To
check the consistency of the obtained value of 1205791(119905119865) with
the holonomic constraint consider (58) We can calculate
the constant 1198881using the initial conditions of the problem
obtaining
1198881= minus 120579
1(119905119868) (120572 + 2120573 cos 120579
2(119905119868))
minus 1205792(119905119868) (120575 + 120573 cos 120579
2(119905119868)) = minus225
(78)
Since 1198881= 0 (58) corresponds in this case to the differential
constraint
119889120578 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792+ 1198881119889119905 = 0
(79)
Equation (79) can be rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2 (119905))
120572 + 2120573 cos 1205792 (119905)
1198891205792minus
1198881
120572 + 2120573 cos 1205792 (119905)
119889119905 (80)
To check the obtained value of 1205791(119905119865) 1198891205791is numerically
integrated between 1205791(119905119868) and 120579
1(119905119865) using the interpolated
numerical optimal solution obtained for 1205792(119905) We get that
1205791(119905119865) = 618705 This value is close to 617172
To check the consistency of the obtained value of 1205791(119905119865)
with the constraint (58) using the computed value 1198881= minus225
and the final conditions 1205792(119905119865) = 0 120579
2(119905119865) = 120587 of the problem
we obtain
1205791(119905119865) =
minus (120575 + 120573 cos 1205792(119905119865))
120572 + 2120573 cos 1205792(119905119865)
1205792(119905119865)
minus1198881
120572 + 2120573 cos 1205792(119905119865)= 9
(81)
This value is very close to the value of 1205791(119905119865) obtained
numerically
73 Computational Issues If the optimal control problem has119898 variables and the time interval [119905
119868 119905119865] has been discretized
into 119873 subintervals the resulting set of difference equations(38) has119898times(119873minus1) equations and119898times(119873minus1) variables plusthe equations and variables due to transversality conditionsFeasible solutions have been used as initial guesses of thealgorithm
The solution of the nonlinear system of difference equa-tions (38) has been obtained using a damped Newtonalgorithm within a line search methodology implementedin Mathematica 7 under Mac OS X operating system (see[22 23] for more details)
8 Conclusion
In this paper the trajectory planning problem for planarunderactuated robot manipulators with two revolute jointswithout gravity has been studied This problem is solved asan optimal control problem based on a numerical resolutionof an unconstrained variational calculus reformulation of theoptimal control problem in which the dynamic equation ofthe mechanical system is regarded as a constraint It hasbeen shown that this reformulation method based on special
14 Abstract and Applied Analysis
10 20 30 40 50 60
minus1
minus08
minus06
minus04
minus02
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
minus15
minus1
minus05
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
(d) 1205792
10 20 30 40 50 60
minus15
minus10
5
10
15
minus5
(e) 1199062
Figure 7 Control and state variables of the optimal solution of problem 3 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 119906
2(119905119868) = 0Nm 120579
1(119905119865) = free
1205792(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads]The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectivelyThe corresponding
sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 6
Figure 8 Sequence of configurations of the robot manipulator at times 11989632 with 119896 = 0 1 32 corresponding to the optimal solutionof problem 4 an initial value problem for an underactuated 119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad]
1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained
with a discretization of [119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control
and state variables are represented in Figure 9
Abstract and Applied Analysis 15
10 20 30 40 50 60
1
2
3
4
5
6
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
4
5
6
7
8
9
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
6
(d) 1205792
10 20 30 40 50 60minus5
5
10
15
20
(e) 1199062
Figure 9 Control and state variables of the optimal solution of problem 4 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 8
derivative multipliers is able to tackle both integrable andnonintegrable differential constraints of the dynamic modelsof underactuated planar horizontal robot manipulators withtwo revolute joints This method can be seamlessly appliedin the presence of additional constraints on the mechanicalsystem
References
[1] A De Luca S Iannitti R Mattone and G Oriolo ldquoUnderactu-ated manipulators control properties and techniquesrdquoMachineIntelligence and Robotic Control vol 4 no 3 pp 113ndash125 2002
[2] G A Bliss Lectures on the Calculus of Variations University ofChicago Press Chicago Ill USA 1946
[3] J Gregory and C Lin Constrained Optimization in the Calculusof Variations and Optimal Control theory Chapman amp Hall1996
[4] W-S Koon and J E Marsden ldquoOptimal control for holonomicand nonholonomic mechanical systems with symmetry andLagrangian reductionrdquo SIAM Journal on Control and Optimiza-tion vol 35 no 3 pp 901ndash929 1997
[5] A M Bloch Nonholonomic Mechanics and Control SpringerNew York NY USA 2003
[6] I I Hussein and A M Bloch ldquoOptimal control of underactu-ated nonholonomic mechanical systemsrdquo IEEE Transactions onAutomatic Control vol 53 no 3 pp 668ndash682 2008
[7] X Z Lai J H She S X Yang andMWu ldquoComprehensive uni-fied control strategy for underactuated two-link manipulatorsrdquoIEEE Transactions on Systems Man and Cybernetics B vol 39no 2 pp 389ndash398 2009
[8] J P Ordaz-Oliver O J Santos-Sanchez and V Lopez-MoralesldquoToward a generalized sub-optimal control method of underac-tuated systemsrdquo Optimal Control Applications amp Methods vol33 no 3 pp 338ndash351 2012
16 Abstract and Applied Analysis
[9] R Seifried ldquoTwo approaches for feedforward control andoptimal design of underactuatedmultibody systemsrdquoMultibodySystem Dynamics vol 27 no 1 pp 75ndash93 2012
[10] M Buss O von Stryk R Bulirsch and G Schmidt ldquoTowardshybrid optimal controlrdquo atmdashAutomatisierungstechnik vol 48no 9 pp 448ndash459 2000
[11] M Buss M Glocker M Hardt O von Stryk R Bulirsch andG Schmidt ldquoNonlinear hybrid dynamical systems modelingoptimal control and applicationsrdquo in Modelling Analysis andDesign of Hybrid Systems S Engell G Frehse and E SchniederEds vol 279 of Lecture Notes in Control and InformationScience pp 331ndash335 Springer 2002
[12] G Oriolo and Y Nakamura ldquoControl of mechanical systemswith second-order nonholonomic constraints underactuatedmanipulatorsrdquo in Proceedings of the 30th IEEE Conference onDecision and Control pp 2398ndash2403 December 1991
[13] T J Tarn M Zhang and A Serrani ldquoNew integrability condi-tions for differential constraintsrdquo Systems and Control Lettersvol 49 no 5 pp 335ndash345 2003
[14] H J Sussmann ldquoA general theorem on local controllabilityrdquoSIAM Journal on Control and Optimization vol 25 no 1 pp158ndash194 1987
[15] F Bullo A D Lewis and K M Lynch ldquoControllable kinematicreductions for mechanical systems concepts computationaltools and examplesrdquo in Proceedings of International Symposiumon Mathematical Theory of Networks and Systems 2002
[16] F Bullo and A D Lewis ldquoLow-order controllability and kine-matic reductions for affine connection control systemsrdquo SIAMJournal on Control andOptimization vol 44 no 3 pp 885ndash9082006
[17] A D Lewis and R MMurray ldquoConfiguration controllability ofsimple mechanical control systemsrdquo SIAM Journal on Controland Optimization vol 35 no 3 pp 766ndash790 1997
[18] F Bullo and K M Lynch ldquoKinematic controllability for decou-pled trajectory planning in underactuatedmechanical systemsrdquoIEEE Transactions on Robotics and Automation vol 17 no 4 pp402ndash412 2001
[19] M R Hestenes Calculus of Variations and Optimcl ControlTheory John Wiley amp Sons 1966
[20] J Gregory ldquoA new systematic method for efficiently solvingholonomic (and nonholonomic) constraint problemsrdquo Analysisand Applications vol 8 no 1 pp 85ndash98 2010
[21] J Gregory and R S Wang ldquoDiscrete variable methods forthe m-dependent variable nonlinear extremal problem in thecalculus of variationsrdquo SIAM Journal onNumerical Analysis vol27 no 2 pp 470ndash487 1990
[22] Wolfram Research 2012[23] J J More and D J Thuente ldquoLine search algorithms with guar-
anteed sufficient decreaserdquo ACM Transactions on MathematicalSoftware vol 20 no 3 pp 286ndash307 1994
where 119909(119905) = (1199091(119905) 1199092(119905) 119909
119899(119905))119879 is an 119899-vector called
the state vector 119906(119905) = (1199061(119905) 1199062(119905) 119906
119898(119905))119879 is an 119898-
vector called the control vector the real-valued function119869(119909 119906) is the objective functional (9) is called the trajectoryequation and the conditions (14) are called the boundaryconditions The set 119880 sub R119898 is called the set of controls with119906(119905) isin 119880 for every 119905 isin [119905
119868 119905119865] We assume that 119891 119892 ℎ 119897 119901
and 119902 are sufficiently smooth for our purposeThis will implysolutions such that 119909(119905) is piecewise smooth whereas 119906(119905) ispiecewise continuous [19]
5 Variational Reformulation of the OptimalControl Problem
A variational approach has been used to solve the more gen-eral optimal control problem stated in the previous section
The classical calculus of variations problem is tominimizean integral of the form
where the independent 119909 variable is assumed to be in theinterval [119886 119887] and the dependent variable 119910 = 119910(119909) = (119910
1(119909)
1199102(119909) 119910
119899(119909))119879 is assumed to be an 119899-vector continuous on
[119886 119887] with derivative 1199101015840 = 1199101015840(119909) = (11991010158401(119909) 1199101015840
2(119909) 119910
1015840
119899(119909))119879
It is also assumed that 119910 is piecewise smooth that is thereexists a finite set of points 119886
1 1198862 119886
119896so that 119886 le 119886
1lt 1198862lt
sdot sdot sdot lt 119886119896le 119887 119910(119909) is continuously differentiable on (119886
119897 119886119897+1)
and that the respective left- and right-handed limits of 1199101015840(119909)exist If 119910(119909) is piecewise smooth and satisfies the boundaryconditions 119910(119886) = 119860 119910(119887) = 119861 then 119910(119909) is said to be anadmissible arc In words this problem consists in findingamong all arcs connecting end points (119886 119860) and (119887 119861) theone minimizing the integral (16)
The main optimality conditions are obtained by defininga variation 119911(119909) a set of functions
where 120575 gt 0 is a fixed real number and the variation 119911(119909) isa piecewise smooth function with 119911(119886) = 119911(119887) = 0 Usinga Taylor series expansion it is easy to see that a necessarycondition that 0 is a relative minimum to 119865 is
where 119891119910 1198911199101015840 denote the partial derivatives of 119891 evaluated
along (119909 119910(119909) 1199101015840(119909)) and the terms 119911 and 1199111015840 are evaluated at119909
Integrating (20) by parts for all admissible variations 119911(119909)another necessary condition for 119910 = 119910(119909) to give a relativeminimum of the variational problem (16)-(17) is obtainedwhich is the following second-order differential equation
known as Euler-Lagrange conditionThis equationmust holdalong (119909 119910(119909) 1199101015840(119909)) except at a finite number of points [3Section 21]
The extremals of (16)-(17) can be obtained by solving theEuler-Lagrange equation but it only holds at points where theextremal 119910lowast(119909) is smooth At points where 119910lowast1015840(119909) has jumpscalled corners the Weierstrass-Erdmann corner conditionsmust be fulfilled [3 Section 23] Since the location of thecorners their number and the amplitudes of the jumps in1199101015840lowast(119909) 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 ourmethodis that the integral form of this condition
holds for all 119909 isin [119886 119887] and some 119888 and therefore theWeierstrass-Erdmann corner conditions are not neededThus an alternative way of computing the extremals can bebased on this necessary condition in integral form
Note that necessary condition requires that boundaryvalues fulfill Euler-Lagrange equation Thus if some of thefour values 119886 119910(119886) 119887 and 119910(119887) are not explicitly givenalternate boundary conditions have to be provided This iswhat transversality conditions do Assume that 119886 119910(119886) and 119887are given but119910(119887) is free In this case the additional necessarytransversality condition
1198911199101015840 (119887 119910
lowast(119887) 119910
1015840lowast(119887)) = 0 (23)
must holdThe variational approach does not consider constraints
However the optimal control problem has at least a first-order differential constraint (9) representing the dynamicequation of the system Moreover since the dynamic equa-tion of a planar 119877119877 robot manipulator is a second-order
6 Abstract and Applied Analysis
differential equation additional differential constraints willarise while rewriting it as a first-order differential equationTherefore the optimal control problemmust be reformulatedas an unconstrained calculus of variations problem in orderto deal with differential and algebraic constraints as describedin the following section
Following [3 Chapter 5] we reformulate as an uncon-strained calculus of variations problem the optimal controlproblem consisting inminimizing (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 variablefrom 119905 to 119909 and the dependent variable from 119909 to 119910 to beconsistent with the notation of calculus of variations Ourreformulation is based on special derivative multipliers anda change of variables in which
1199101(119909) = 119910(119909) is the renamed state vector
1199101015840
2(119909) = 119906(119909) is the renamed state vector
1199101015840
3(119909) is the multiplier associated with (9)
1199101015840
4(119909) is themultiplier associated with constraint (10)
1199101015840
5(119909) is the multiplier associated with constraint (11)
1199101015840
6(119909) is the excess variable of constraint (11)
Since 1199102(119909) 119910
6(119909) are not unique without an extra condi-
tion we initialize these variables by defining 119910119894(119909119868) = 0 119894 =
1 6 Thus our problem becomes
min 119868 (Y) = int119909119865
119909119868
119865 (119909YY1015840) 119889119909 (24)
where
Y = (1199101 1199102 1199103 1199104 1199105 1199106)119879
119865 = 119891 (119909 1199101 1199101015840
2) + 1199101015840119879
3(1199101015840
1minus 119892 (119909 119910
1 1199101015840
2))
+ 1199101015840119879
4ℎ (119909 119910
1 1199101015840
2) + 1199101015840119879
5(119897 (119909 119910
1 1199101015840
2) + 11991010158402
6)
(25)
Since the values of 119910119894(119909119865) 119894 = 2 6 are unknown
transversality conditions are needed having the form
In the above lines 119910(119909) is an 119899-vector and120595 120593 120601 are assumedto be differentiable in their arguments or with the neededsmoothness We also assume that 120595
11991010158401199101015840 gt 0 The boundary
conditions of the problems are any combination of fixedboundary conditions for the components of 119910 with thepossibility of leaving some of them unspecified
If we reformulate problem (27)-(28) using the techniquedescribed in Section 5 we get the following HamiltonianΨ(119909 119884 119884
1015840) = 120595(119909 119910
1 1199101015840
1) + 1199101015840
2120593(119909 119910
1) with 119910
1(119909) = 119910(119909)
where 11991010158402is the multiplier We have in this case
Ψ11988410158401198841015840 = [
1205951199101015840
11199101015840
1
0
0 0] (31)
which is singular The singularity of Ψ11988410158401198841015840 is a difficulty we
must avoid Furthermore even when it is not difficult tochange from the120593 constraint to the120601 constraint by increasingthe dimension of the independent variables it is not easy todeal with the new associated boundary conditionsThis is thereason that problem (27)-(28) is so difficult to solve
It has been shown in [20] that problem (27)-(28) can bereformulated as an equivalent problem of the form (29)-(30)In particular 119910(119909) is a solution to (27)-(28) if and only if 119910(119909)is a solution to
The numerical method used is based on the discretizationof the unconstrained variational calculus problem stated inthe previous section In particular the main underlying ideais obtaining a discretized solution 119910
ℎ(119909) solving (20) for all
piecewise linear spline function variations 119911(119909) instead of
Abstract and Applied Analysis 7
dealing with the Euler-Lagrange equation (21) Thus thismethod uses no numerical corner conditions and avoidspatching solutions to (21) between corners
Let 119873 be a large positive integer ℎ = (119887 minus 119886)119873 and let120587 = (119886 = 119886
0lt 1198861lt sdot sdot sdot lt 119886
119873= 119887) be a partition of the
interval [119886 119887] where 119886119896= 119886 + 119896ℎ for 119896 = 0 1 119873 Define
the one-dimensional spline hat functions
119908119896 (119909) =
119909 minus 119886119896minus1
ℎif 119886119896minus1
lt 119909 lt 119886119896
119886119896+1
minus 119909
ℎif 119886119896lt 119909 lt 119886
119896+1
0 otherwise
(35)
where 119896 = 1 2 119873 minus 1 Define also the 119898-dimensionalpiecewise linear component functions
119910ℎ (119909) =
119873
sum
119896=0
119882119896 (119909) 119862119896 119911
ℎ (119909) =
119873
sum
119896=0
119882119896 (119909)119863119896 (36)
where 119882119896(119909) = 119908
119896(119909)119868119898times119898
119910ℎ(119909) is the sought numerical
solution and 119911ℎ(119909) is a numerical variation In particular the
constant vectors 119862119896are to be determined by the algorithm
developed by us and the constant vectors 119863119896are arbitrary
Thus the discretized form of (20) is obtained in eachsubinterval [119886
119896minus1 119886119896+1] For the sake of clarity of exposition
we assume that119898 = 1 Note that 1198681015840(119910 119911) in (20) is linear in 119911so that a three-term relationship may be obtained at 119909 = 119886
119896
by choosing 119911(119909) = 119908119896(119909) for 119896 = 1 2 119873 minus 1 Thus
In these equations 119886lowast119896= (119886119896+ 119886119896+1)2 and 119910
119896= 119910ℎ(119886119896) is
the computed value of 119910ℎ(119909) at 119886
119896 In the general case when
119898 gt 1 the same result is obtained but 1198911199101015840 and 119891
119910are
column 119898-vectors of functions with 119894th component 1198911199101198941015840 and
119891119910119894 respectively Similarly (119910
119896+119910119896minus1)2 is the119898-vector which
is the average of the119898-vectors 119910ℎ(119886119896) and 119910
ℎ(119886119896minus1)
By the same arguments that led to (38)
1198911199101015840 (119886lowast
119873minus1119910119873+ 119910119873minus1
2119910119873minus 119910119873minus1
ℎ)
+ℎ
2119891119910(119886lowast
119896minus1119910119873+ 119910119873minus1
2119910119873minus 119910119873minus1
ℎ) = 0
(39)
which is the numerical equivalent of the transversality condi-tion (23) For further details see [3 Chapter 6]
It has been shown in [21] that with this method the globalerror has a priori global reduction ratio of119874(ℎ2) In practiceif the step size ℎ is halved the error decreases by 4
7 Implementation and Results
Several numerical experiments have been carried out forboth 119877119877 and 119877119877 planar horizontal underactuated robotmanipulators
71 Planar Horizontal Underactuated 119877119877 Robot ManipulatorIn this section the optimal control problem of a planar hor-izontal underactuated 119877119877 is studied In this robot model thesecond joint is not actuated thus 119906 = (119906
1 0)119879 In this case
it is neither possible to integrate partially nor completely thenonholonomic constraint because the manipulator inertiamatrix contains terms in 120579
2(see [12]) Hence the system is
controllable The numerical results of the application of ourmethod for optimal control to a boundary value problem andto an initial value problem for this system will be described
For a planar horizontal underactuated119877119877 (2) can be splitinto
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792
minus 120573 sin 1205792(2 12057911205792+ 1205792
2) = 1199061
(120575 + 120573 cos 1205792) 1205791+ 120575 1205792+ 120573 sin 120579
21205792
1= 0
(40)
To express optimal control problems that involve this second-order differential constraints in the form of a basic optimal
8 Abstract and Applied Analysis
control problem we have first to convert it into first-orderdifferential constraints introducing the following change ofvariables
1199091= 1205791 119909
3= 1205791
1199092= 1205792 119909
4= 1205792
(41)
with the following additional relations
1199091015840
1= 1199093
1199091015840
2= 1199094
(42)
Thus the second-order differential equations (40) are con-verted into the first-order differential equations
(120572 + 2120573 cos1199092) 1199091015840
3+ (120575 + 120573 cos119909
2) 1199091015840
4
minus 120573 sin1199092(211990931199094+ 1199092
4) = 1199061
(43)
(120575 + 120573 cos1199092) 1199091015840
3+ 1205751199091015840
4+ 120573 sin119909
21199092
3= 0 (44)
Relations (42) (43) and (44) are now the differential con-straints of the optimal control problem and the objectivefunctional to minimize is
119869 = int
119905119865
119905119868
1199062
1119889119905 (45)
Then we introduce the following new variables
X = [
[
1198831
sdot sdot sdot
1198839
]
]
(46)
such that
119883119894= 119909119894 119894 = 1 4 (47)
1198831015840
5= 1199061 119883
5(119905119868) = 0 (48)
where 11988310158406with 119883
6(119905119868) = 0 is the multiplier associated with
whereas in the initial value problem 119883119894(119905119865) will be free for
some 119894The initial values of control variables and multipliers
have been set to zero whereas their final values have notbeen assigned in both optimal control problems Thereforetransversality conditions are needed in both cases for thevariables119883
119894(119905119865) 119894 = 5 10 and they will be of the form
Figure 2 Sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 corresponding to the optimal solutionof problem 1 a boundary value problem for the planar 119877119877 robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad]
1205791(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads] obtained with a
discretization of [119905119868 119905119865] into 64 subintervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control and
state variables are represented in Figure 3
The initial values of control variable and of the multipliershave been set to zero whereas their final values are left freeFigure 2 shows the sequence of configurations of the robot attimes 119905 = 11989632 119896 = 0 1 32 Since the configurations ofthe sequence overlap it has been split into smaller sequencesfor a better visualization of the manipulator motion Figure 3depicts the corresponding control and state variables of theoptimal solution of this boundary value problem obtainedwith a discretization of the time interval [119905
119868 119905119865] into 64
subintervals The value of the objective functional for thissolution is 345185 [J]
712 Problem 2 Initial Value Problem An initial valueproblem has also been solved with the following initial andfinal conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) =
120587
2[rad]
1205792(119905119868) = minus
120587
2[rad] 120579
2(119905119865) = minus
120587
2[rad]
1205791(119905119868) = 0 [rads] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(55)
The initial values of control variable and of the multipliershave been set to zero whereas their final values are left freeThe only difference between these conditions and those of theboundary value problem described in Section 711 is that now1205791(119905119865) = free
Figures 4 and 5 depict the sequence of configurations the119877119877 robot manipulator and the corresponding control andstate variables of the optimal solution of this initial valueproblem respectively obtained with a discretization of thetime interval [119905
119868 119905119865] into 64 subintervals The value of the
objective functional for this solution is 56472 [J]This value islower than the value of the objective functional of the solutionof the boundary value problem described in Section 711because now is 120579
1(119905119865) = free and the control system does
not have to spend energy to stop it
72 Planar Horizontal Underactuated 119877119877 Robot ManipulatorIn this section the optimal control problem of a planarhorizontal underactuated 119877119877 robot manipulator is studiedIn this robot model the first joint is not actuated thus 119906 =
(0 1199062)119879 and (2) can be split into
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792
minus 120573 sin 1205792(2 12057911205792+ 1205792
2) = 0
(56)
(120575 + 120573 cos 1205792) 1205791+ 120575 1205792+ 120573 sin 120579
21205792
1= 1199062 (57)
As explained in [12] since gravity terms are all zero and1205791does not intervene in the system inertia matrix (56) can
be partially integrated to
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792+ 1198881= 0 (58)
Actually constraint (56) is completely integrable giving rise toan holonomic constraintThe resulting holonomic constrainttakes different forms depending on the value of 119888
1which
depends on the initial conditions Therefore two cases havebeen considered
(i) when the initial velocities 1205791(119905119868) and 120579
2(119905119868) are both
zero(ii) when the initial velocity 120579
1(119905119868) is nonzero
721 Problem 3 Initial Value Problem with Zero Initial Veloc-ities An initial value problem has been solved with the fol-lowing initial and final conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rad] 120579
2(119905119865) = 120587 [rad]
1205791(119905119868) = 0 [rads] 120579
1(119905119865) = 0 [rads]
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(59)
10 Abstract and Applied Analysis
10 20 30 40 50 60
05
1
15
2
25
(a) 1205791
10 20 30 40 50 60minus05
minus1
minus15
minus2
minus25
minus3
(b) 1205792
10 20 30 40 50 60
minus10
minus5
5
10
(c) 1205791
10 20 30 40 50 60
minus10
minus5
5
10
15
(d) 1205792
10 20 30 40 50 60
minus300
minus200
minus100
100
200
300
(e) 1199061
Figure 3 Control and state variables of the optimal solution of problem 1 a boundary value problem for the planar 119877119877 robot manipulatorwith boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 2
Figure 4 Sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 corresponding to the optimal solutionof problem 2 an initial value problem for an 119877119877 robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) =
0 [rads] 1205792(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained with a discretization of
[119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control and state variables are
represented in Figure 5
Abstract and Applied Analysis 11
10 20 30 40 50 60
05
1
15
2
25
(a) 1205791
10 20 30 40 50 60
minus25
minus2
(b) 1205792
10 20 30 40 50 60
minus4
minus2
2
4
6
8
10
(c) 1205791
minus4
minus6
minus210 20 30 40 50 60
2
4
(d) 1205792
10 20 30 40 50 60minus50
50
100
150
(e) 1199061
Figure 5 Control and state variables of the optimal solution of problem 2 an initial value problem for a119877119877 robotmanipulator with boundaryconditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = free
and 1205792(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of configurations of the
robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 4
The initial values of the control variable and of themultipliershave been set to zero whereas their final value is left free
Since there is a holonomic constraint that relates thevalues of the angles 120579
1and 120579
2 without integrating (58) we
are not able to find the value of 1205791(119905119865) consistent with 120579
1(119905119868)
Therefore no final conditions have been imposed on 1205791
From the initial conditions of the problem we obtain 1198881=
0 Equation (58) with 1198881= 0 corresponds to the homogeneous
differential constraint
119889120577 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792= 0 (60)
The differential 119889120577 is not exact However it becomes an exactdifferential if multiplied by the factor 1(120572 + 2120573 cos 120579
2) This
operation does not alter the differential equation (60) Inthis case there does exist a function 120577 whose differentialcoincides with the expression 119889120577(120572 + 2120573 cos 120579
2) Due to
the existence of this function the integral of 119889120577 between
two points depends only on these points and not on theintegration path Equation (60) rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2)
120572 + 2120573 cos 1205792
1198891205792
(61)
can be integrated by separating variables The correspondingholonomic constraint has the following expression
1205791= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1205792
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1205792
2+ 1198882
(62)
To express this optimal control problem in the form ofa basic optimal control problem we first have to convert (57)
12 Abstract and Applied Analysis
into a first-order differential model introducing the followingchange of variables
1199091= 1205791 119909
3= 1205791
1199092= 1205792 119909
4= 1205792
(63)
with the following additional relations
1199091015840
1= 1199093
1199091015840
2= 1199094
(64)
Thus the optimal control problem is to minimize
int
1
0
1199062
2119889119905 (65)
subject to the constraints1199091
= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1199092
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1199092
2+ 1198882
(66)
(120575 + 120573 cos 1205792) 1199091015840
3+ 1205751199091015840
4+ 120573 sin119909
21199092
3= 1199062 (67)
and the additional constraints (64) To reformulate thisoptimal control problem as an unconstrained calculus ofvariations problem let X be
X = [
[
1198831
sdot sdot sdot
1198839
]
]
(68)
such that119883119894= 119909119894 119894 = 1 4
1198831015840
5= 1199061 119883
5(119905119868) = 0
(69)
where 11988310158406with 119883
6(119905119868) = 0 is the multiplier associated with
the holonomic constraint (66) 11988310158407with 119883
7(119905119868) = 0 is the
multipliers associated with the differential constraint (67)and 1198831015840
8with 119883
8(119905119868) = 0 and 1198831015840
9with 119883
9(119905119868) = 0 are the
multipliers associatedwith the additional equality constraints(64)
Thus the holonomic constraint of the problem can berewritten as follows
120593 (119905X)
= 1198831minus((120572 minus 2120575)
times tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1198832
2)]]
]
)
times (radic41205732 minus 1205722)
minus1
+1198832
2= 0
(70)
Now the technique described in Section 51 to deal withholonomic constraints can be applied to 120593(119905X) and thisholonomic constraint is replaced by
120593119905+ 1205931198831198831015840= 0
120593 (0 119883 (0)) = 0
(71)
From the initial conditions of the problem the latter equationreduces to the equality 0 = 0 whereas the former takes thefollowing form
(120572 + 2120573 cos (1198832))1198833+ (120575 + 120573 cos (119883
2))1198834= 0 (72)
The corresponding Hamiltonian is
1198661= 11988310158402
5+ 1198831015840
6((120572 + 2120573 cos (119883
2))1198833
+ (120575 + 120573 cos (1198832))1198834)
+1198831015840
7(120573 sin (119883
2)1198832
3+ (120575 + 120573 cos (119883
2))1198831015840
3
+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(73)
It is not difficult to check that matrix 1198661X1015840X1015840
is singularThis is due to the fact that to handle our optimal controlproblemwhich involves second-order differential constraintswe converted them into first-order differential constraintsTherefore we apply again the technique of Section 51obtaining the identity 0 = 0 and the following constraint
minus 120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4= 0
(74)
The corresponding Hamiltonian is
1198662= 11988310158402
5+ 1198831015840
6
times (minus120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4)
+ 1198831015840
7(120573 sin (119883
2)1198832
3+(120575+120573 cos (119883
2))1198831015840
3+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(75)
It is not difficult to check that matrix 1198662X1015840X1015840
in this case is notsingular since its determinant is
Substituting the values of120572120573 and 120575 this expression becomesdet(119866
2X1015840X1015840) = 1128(43 minus 2 cos(2119883
2))2 which is always
positive for any real value1198832 Figure 6 shows the sequence of
configurations of the robot at times 11989632with 119896 = 0 1 32and Figure 7 depicts control and state variables of the optimal
Abstract and Applied Analysis 13
Figure 6 Sequence of configurations of the robot manipulator attimes 11989632 with 119896 = 0 1 32 corresponding to the optimal sol-ution of problem 3 an initial value problem for an underactuated119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) =
0 [rad] 1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads]
1205791(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2
(119905119865) = 0 [rads] obtained with a discretization of [119905
119868 119905119865] into 64
subintervals The initial and final times are 119905119868= 0 and 119905
119865=
1 [s] respectively The corresponding control and state variables arerepresented in Figure 7
solution obtained with a discretization of the interval [119905119868 119905119865]
into 64 subintervalsIn particular we get 120579
1(119905119865) = minus110248 [rad] To check the
consistency of this result with the holonomic constraint (62)since 120579
1(119905119868) = 1205792(119905119868) = 0 [rads] we get from (58) that 119888
1= 0
and using the initial condition 1205791(119905119868) = 1205792(119905119868) = 0 [rad] we
get from (62) that 1198882= 0 Having established the value of the
constant 1198882 we obtain from the same equation for 120579
2(119905119865) =
120587 [rad] that 1205791(119905119865) = minus110248 [rad] which coincides with
the value of 1205791(119905119865) obtained numerically
722 Problem 4 Initial Value Problem with Nonzero InitialVelocity 120579
1 Another initial value problem has been solved
with the following conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rad] 120579
2(119905119865) = 120587 [rad]
1205791(119905119868) = 5 [rads] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(77)
The initial values of the multipliers have been set to zerowhereas their final value is left free Notice that no finalconditions have been imposed on 120579
1and 120579
1 The same
considerations done in previous section hold in this caseas well The technique described in Section 51 must beapplied twice leading to the differential constraint (74) andto the Hamiltonian (75) Figure 8 shows the sequence ofconfigurations of the robot at times 11989632with 119896 = 0 1 32and Figure 9 depicts the control and state variables ofthe optimal solution obtained with a discretization of theinterval [119905
119868 119905119865] into 64 subintervals In particular we get that
1205791(119905119865) = 617172 [rad] and 120579
1(119905119865) = 900163 [rads] To
check the consistency of the obtained value of 1205791(119905119865) with
the holonomic constraint consider (58) We can calculate
the constant 1198881using the initial conditions of the problem
obtaining
1198881= minus 120579
1(119905119868) (120572 + 2120573 cos 120579
2(119905119868))
minus 1205792(119905119868) (120575 + 120573 cos 120579
2(119905119868)) = minus225
(78)
Since 1198881= 0 (58) corresponds in this case to the differential
constraint
119889120578 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792+ 1198881119889119905 = 0
(79)
Equation (79) can be rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2 (119905))
120572 + 2120573 cos 1205792 (119905)
1198891205792minus
1198881
120572 + 2120573 cos 1205792 (119905)
119889119905 (80)
To check the obtained value of 1205791(119905119865) 1198891205791is numerically
integrated between 1205791(119905119868) and 120579
1(119905119865) using the interpolated
numerical optimal solution obtained for 1205792(119905) We get that
1205791(119905119865) = 618705 This value is close to 617172
To check the consistency of the obtained value of 1205791(119905119865)
with the constraint (58) using the computed value 1198881= minus225
and the final conditions 1205792(119905119865) = 0 120579
2(119905119865) = 120587 of the problem
we obtain
1205791(119905119865) =
minus (120575 + 120573 cos 1205792(119905119865))
120572 + 2120573 cos 1205792(119905119865)
1205792(119905119865)
minus1198881
120572 + 2120573 cos 1205792(119905119865)= 9
(81)
This value is very close to the value of 1205791(119905119865) obtained
numerically
73 Computational Issues If the optimal control problem has119898 variables and the time interval [119905
119868 119905119865] has been discretized
into 119873 subintervals the resulting set of difference equations(38) has119898times(119873minus1) equations and119898times(119873minus1) variables plusthe equations and variables due to transversality conditionsFeasible solutions have been used as initial guesses of thealgorithm
The solution of the nonlinear system of difference equa-tions (38) has been obtained using a damped Newtonalgorithm within a line search methodology implementedin Mathematica 7 under Mac OS X operating system (see[22 23] for more details)
8 Conclusion
In this paper the trajectory planning problem for planarunderactuated robot manipulators with two revolute jointswithout gravity has been studied This problem is solved asan optimal control problem based on a numerical resolutionof an unconstrained variational calculus reformulation of theoptimal control problem in which the dynamic equation ofthe mechanical system is regarded as a constraint It hasbeen shown that this reformulation method based on special
14 Abstract and Applied Analysis
10 20 30 40 50 60
minus1
minus08
minus06
minus04
minus02
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
minus15
minus1
minus05
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
(d) 1205792
10 20 30 40 50 60
minus15
minus10
5
10
15
minus5
(e) 1199062
Figure 7 Control and state variables of the optimal solution of problem 3 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 119906
2(119905119868) = 0Nm 120579
1(119905119865) = free
1205792(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads]The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectivelyThe corresponding
sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 6
Figure 8 Sequence of configurations of the robot manipulator at times 11989632 with 119896 = 0 1 32 corresponding to the optimal solutionof problem 4 an initial value problem for an underactuated 119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad]
1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained
with a discretization of [119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control
and state variables are represented in Figure 9
Abstract and Applied Analysis 15
10 20 30 40 50 60
1
2
3
4
5
6
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
4
5
6
7
8
9
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
6
(d) 1205792
10 20 30 40 50 60minus5
5
10
15
20
(e) 1199062
Figure 9 Control and state variables of the optimal solution of problem 4 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 8
derivative multipliers is able to tackle both integrable andnonintegrable differential constraints of the dynamic modelsof underactuated planar horizontal robot manipulators withtwo revolute joints This method can be seamlessly appliedin the presence of additional constraints on the mechanicalsystem
References
[1] A De Luca S Iannitti R Mattone and G Oriolo ldquoUnderactu-ated manipulators control properties and techniquesrdquoMachineIntelligence and Robotic Control vol 4 no 3 pp 113ndash125 2002
[2] G A Bliss Lectures on the Calculus of Variations University ofChicago Press Chicago Ill USA 1946
[3] J Gregory and C Lin Constrained Optimization in the Calculusof Variations and Optimal Control theory Chapman amp Hall1996
[4] W-S Koon and J E Marsden ldquoOptimal control for holonomicand nonholonomic mechanical systems with symmetry andLagrangian reductionrdquo SIAM Journal on Control and Optimiza-tion vol 35 no 3 pp 901ndash929 1997
[5] A M Bloch Nonholonomic Mechanics and Control SpringerNew York NY USA 2003
[6] I I Hussein and A M Bloch ldquoOptimal control of underactu-ated nonholonomic mechanical systemsrdquo IEEE Transactions onAutomatic Control vol 53 no 3 pp 668ndash682 2008
[7] X Z Lai J H She S X Yang andMWu ldquoComprehensive uni-fied control strategy for underactuated two-link manipulatorsrdquoIEEE Transactions on Systems Man and Cybernetics B vol 39no 2 pp 389ndash398 2009
[8] J P Ordaz-Oliver O J Santos-Sanchez and V Lopez-MoralesldquoToward a generalized sub-optimal control method of underac-tuated systemsrdquo Optimal Control Applications amp Methods vol33 no 3 pp 338ndash351 2012
16 Abstract and Applied Analysis
[9] R Seifried ldquoTwo approaches for feedforward control andoptimal design of underactuatedmultibody systemsrdquoMultibodySystem Dynamics vol 27 no 1 pp 75ndash93 2012
[10] M Buss O von Stryk R Bulirsch and G Schmidt ldquoTowardshybrid optimal controlrdquo atmdashAutomatisierungstechnik vol 48no 9 pp 448ndash459 2000
[11] M Buss M Glocker M Hardt O von Stryk R Bulirsch andG Schmidt ldquoNonlinear hybrid dynamical systems modelingoptimal control and applicationsrdquo in Modelling Analysis andDesign of Hybrid Systems S Engell G Frehse and E SchniederEds vol 279 of Lecture Notes in Control and InformationScience pp 331ndash335 Springer 2002
[12] G Oriolo and Y Nakamura ldquoControl of mechanical systemswith second-order nonholonomic constraints underactuatedmanipulatorsrdquo in Proceedings of the 30th IEEE Conference onDecision and Control pp 2398ndash2403 December 1991
[13] T J Tarn M Zhang and A Serrani ldquoNew integrability condi-tions for differential constraintsrdquo Systems and Control Lettersvol 49 no 5 pp 335ndash345 2003
[14] H J Sussmann ldquoA general theorem on local controllabilityrdquoSIAM Journal on Control and Optimization vol 25 no 1 pp158ndash194 1987
[15] F Bullo A D Lewis and K M Lynch ldquoControllable kinematicreductions for mechanical systems concepts computationaltools and examplesrdquo in Proceedings of International Symposiumon Mathematical Theory of Networks and Systems 2002
[16] F Bullo and A D Lewis ldquoLow-order controllability and kine-matic reductions for affine connection control systemsrdquo SIAMJournal on Control andOptimization vol 44 no 3 pp 885ndash9082006
[17] A D Lewis and R MMurray ldquoConfiguration controllability ofsimple mechanical control systemsrdquo SIAM Journal on Controland Optimization vol 35 no 3 pp 766ndash790 1997
[18] F Bullo and K M Lynch ldquoKinematic controllability for decou-pled trajectory planning in underactuatedmechanical systemsrdquoIEEE Transactions on Robotics and Automation vol 17 no 4 pp402ndash412 2001
[19] M R Hestenes Calculus of Variations and Optimcl ControlTheory John Wiley amp Sons 1966
[20] J Gregory ldquoA new systematic method for efficiently solvingholonomic (and nonholonomic) constraint problemsrdquo Analysisand Applications vol 8 no 1 pp 85ndash98 2010
[21] J Gregory and R S Wang ldquoDiscrete variable methods forthe m-dependent variable nonlinear extremal problem in thecalculus of variationsrdquo SIAM Journal onNumerical Analysis vol27 no 2 pp 470ndash487 1990
[22] Wolfram Research 2012[23] J J More and D J Thuente ldquoLine search algorithms with guar-
anteed sufficient decreaserdquo ACM Transactions on MathematicalSoftware vol 20 no 3 pp 286ndash307 1994
differential equation additional differential constraints willarise while rewriting it as a first-order differential equationTherefore the optimal control problemmust be reformulatedas an unconstrained calculus of variations problem in orderto deal with differential and algebraic constraints as describedin the following section
Following [3 Chapter 5] we reformulate as an uncon-strained calculus of variations problem the optimal controlproblem consisting inminimizing (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 variablefrom 119905 to 119909 and the dependent variable from 119909 to 119910 to beconsistent with the notation of calculus of variations Ourreformulation is based on special derivative multipliers anda change of variables in which
1199101(119909) = 119910(119909) is the renamed state vector
1199101015840
2(119909) = 119906(119909) is the renamed state vector
1199101015840
3(119909) is the multiplier associated with (9)
1199101015840
4(119909) is themultiplier associated with constraint (10)
1199101015840
5(119909) is the multiplier associated with constraint (11)
1199101015840
6(119909) is the excess variable of constraint (11)
Since 1199102(119909) 119910
6(119909) are not unique without an extra condi-
tion we initialize these variables by defining 119910119894(119909119868) = 0 119894 =
1 6 Thus our problem becomes
min 119868 (Y) = int119909119865
119909119868
119865 (119909YY1015840) 119889119909 (24)
where
Y = (1199101 1199102 1199103 1199104 1199105 1199106)119879
119865 = 119891 (119909 1199101 1199101015840
2) + 1199101015840119879
3(1199101015840
1minus 119892 (119909 119910
1 1199101015840
2))
+ 1199101015840119879
4ℎ (119909 119910
1 1199101015840
2) + 1199101015840119879
5(119897 (119909 119910
1 1199101015840
2) + 11991010158402
6)
(25)
Since the values of 119910119894(119909119865) 119894 = 2 6 are unknown
transversality conditions are needed having the form
In the above lines 119910(119909) is an 119899-vector and120595 120593 120601 are assumedto be differentiable in their arguments or with the neededsmoothness We also assume that 120595
11991010158401199101015840 gt 0 The boundary
conditions of the problems are any combination of fixedboundary conditions for the components of 119910 with thepossibility of leaving some of them unspecified
If we reformulate problem (27)-(28) using the techniquedescribed in Section 5 we get the following HamiltonianΨ(119909 119884 119884
1015840) = 120595(119909 119910
1 1199101015840
1) + 1199101015840
2120593(119909 119910
1) with 119910
1(119909) = 119910(119909)
where 11991010158402is the multiplier We have in this case
Ψ11988410158401198841015840 = [
1205951199101015840
11199101015840
1
0
0 0] (31)
which is singular The singularity of Ψ11988410158401198841015840 is a difficulty we
must avoid Furthermore even when it is not difficult tochange from the120593 constraint to the120601 constraint by increasingthe dimension of the independent variables it is not easy todeal with the new associated boundary conditionsThis is thereason that problem (27)-(28) is so difficult to solve
It has been shown in [20] that problem (27)-(28) can bereformulated as an equivalent problem of the form (29)-(30)In particular 119910(119909) is a solution to (27)-(28) if and only if 119910(119909)is a solution to
The numerical method used is based on the discretizationof the unconstrained variational calculus problem stated inthe previous section In particular the main underlying ideais obtaining a discretized solution 119910
ℎ(119909) solving (20) for all
piecewise linear spline function variations 119911(119909) instead of
Abstract and Applied Analysis 7
dealing with the Euler-Lagrange equation (21) Thus thismethod uses no numerical corner conditions and avoidspatching solutions to (21) between corners
Let 119873 be a large positive integer ℎ = (119887 minus 119886)119873 and let120587 = (119886 = 119886
0lt 1198861lt sdot sdot sdot lt 119886
119873= 119887) be a partition of the
interval [119886 119887] where 119886119896= 119886 + 119896ℎ for 119896 = 0 1 119873 Define
the one-dimensional spline hat functions
119908119896 (119909) =
119909 minus 119886119896minus1
ℎif 119886119896minus1
lt 119909 lt 119886119896
119886119896+1
minus 119909
ℎif 119886119896lt 119909 lt 119886
119896+1
0 otherwise
(35)
where 119896 = 1 2 119873 minus 1 Define also the 119898-dimensionalpiecewise linear component functions
119910ℎ (119909) =
119873
sum
119896=0
119882119896 (119909) 119862119896 119911
ℎ (119909) =
119873
sum
119896=0
119882119896 (119909)119863119896 (36)
where 119882119896(119909) = 119908
119896(119909)119868119898times119898
119910ℎ(119909) is the sought numerical
solution and 119911ℎ(119909) is a numerical variation In particular the
constant vectors 119862119896are to be determined by the algorithm
developed by us and the constant vectors 119863119896are arbitrary
Thus the discretized form of (20) is obtained in eachsubinterval [119886
119896minus1 119886119896+1] For the sake of clarity of exposition
we assume that119898 = 1 Note that 1198681015840(119910 119911) in (20) is linear in 119911so that a three-term relationship may be obtained at 119909 = 119886
119896
by choosing 119911(119909) = 119908119896(119909) for 119896 = 1 2 119873 minus 1 Thus
In these equations 119886lowast119896= (119886119896+ 119886119896+1)2 and 119910
119896= 119910ℎ(119886119896) is
the computed value of 119910ℎ(119909) at 119886
119896 In the general case when
119898 gt 1 the same result is obtained but 1198911199101015840 and 119891
119910are
column 119898-vectors of functions with 119894th component 1198911199101198941015840 and
119891119910119894 respectively Similarly (119910
119896+119910119896minus1)2 is the119898-vector which
is the average of the119898-vectors 119910ℎ(119886119896) and 119910
ℎ(119886119896minus1)
By the same arguments that led to (38)
1198911199101015840 (119886lowast
119873minus1119910119873+ 119910119873minus1
2119910119873minus 119910119873minus1
ℎ)
+ℎ
2119891119910(119886lowast
119896minus1119910119873+ 119910119873minus1
2119910119873minus 119910119873minus1
ℎ) = 0
(39)
which is the numerical equivalent of the transversality condi-tion (23) For further details see [3 Chapter 6]
It has been shown in [21] that with this method the globalerror has a priori global reduction ratio of119874(ℎ2) In practiceif the step size ℎ is halved the error decreases by 4
7 Implementation and Results
Several numerical experiments have been carried out forboth 119877119877 and 119877119877 planar horizontal underactuated robotmanipulators
71 Planar Horizontal Underactuated 119877119877 Robot ManipulatorIn this section the optimal control problem of a planar hor-izontal underactuated 119877119877 is studied In this robot model thesecond joint is not actuated thus 119906 = (119906
1 0)119879 In this case
it is neither possible to integrate partially nor completely thenonholonomic constraint because the manipulator inertiamatrix contains terms in 120579
2(see [12]) Hence the system is
controllable The numerical results of the application of ourmethod for optimal control to a boundary value problem andto an initial value problem for this system will be described
For a planar horizontal underactuated119877119877 (2) can be splitinto
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792
minus 120573 sin 1205792(2 12057911205792+ 1205792
2) = 1199061
(120575 + 120573 cos 1205792) 1205791+ 120575 1205792+ 120573 sin 120579
21205792
1= 0
(40)
To express optimal control problems that involve this second-order differential constraints in the form of a basic optimal
8 Abstract and Applied Analysis
control problem we have first to convert it into first-orderdifferential constraints introducing the following change ofvariables
1199091= 1205791 119909
3= 1205791
1199092= 1205792 119909
4= 1205792
(41)
with the following additional relations
1199091015840
1= 1199093
1199091015840
2= 1199094
(42)
Thus the second-order differential equations (40) are con-verted into the first-order differential equations
(120572 + 2120573 cos1199092) 1199091015840
3+ (120575 + 120573 cos119909
2) 1199091015840
4
minus 120573 sin1199092(211990931199094+ 1199092
4) = 1199061
(43)
(120575 + 120573 cos1199092) 1199091015840
3+ 1205751199091015840
4+ 120573 sin119909
21199092
3= 0 (44)
Relations (42) (43) and (44) are now the differential con-straints of the optimal control problem and the objectivefunctional to minimize is
119869 = int
119905119865
119905119868
1199062
1119889119905 (45)
Then we introduce the following new variables
X = [
[
1198831
sdot sdot sdot
1198839
]
]
(46)
such that
119883119894= 119909119894 119894 = 1 4 (47)
1198831015840
5= 1199061 119883
5(119905119868) = 0 (48)
where 11988310158406with 119883
6(119905119868) = 0 is the multiplier associated with
whereas in the initial value problem 119883119894(119905119865) will be free for
some 119894The initial values of control variables and multipliers
have been set to zero whereas their final values have notbeen assigned in both optimal control problems Thereforetransversality conditions are needed in both cases for thevariables119883
119894(119905119865) 119894 = 5 10 and they will be of the form
Figure 2 Sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 corresponding to the optimal solutionof problem 1 a boundary value problem for the planar 119877119877 robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad]
1205791(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads] obtained with a
discretization of [119905119868 119905119865] into 64 subintervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control and
state variables are represented in Figure 3
The initial values of control variable and of the multipliershave been set to zero whereas their final values are left freeFigure 2 shows the sequence of configurations of the robot attimes 119905 = 11989632 119896 = 0 1 32 Since the configurations ofthe sequence overlap it has been split into smaller sequencesfor a better visualization of the manipulator motion Figure 3depicts the corresponding control and state variables of theoptimal solution of this boundary value problem obtainedwith a discretization of the time interval [119905
119868 119905119865] into 64
subintervals The value of the objective functional for thissolution is 345185 [J]
712 Problem 2 Initial Value Problem An initial valueproblem has also been solved with the following initial andfinal conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) =
120587
2[rad]
1205792(119905119868) = minus
120587
2[rad] 120579
2(119905119865) = minus
120587
2[rad]
1205791(119905119868) = 0 [rads] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(55)
The initial values of control variable and of the multipliershave been set to zero whereas their final values are left freeThe only difference between these conditions and those of theboundary value problem described in Section 711 is that now1205791(119905119865) = free
Figures 4 and 5 depict the sequence of configurations the119877119877 robot manipulator and the corresponding control andstate variables of the optimal solution of this initial valueproblem respectively obtained with a discretization of thetime interval [119905
119868 119905119865] into 64 subintervals The value of the
objective functional for this solution is 56472 [J]This value islower than the value of the objective functional of the solutionof the boundary value problem described in Section 711because now is 120579
1(119905119865) = free and the control system does
not have to spend energy to stop it
72 Planar Horizontal Underactuated 119877119877 Robot ManipulatorIn this section the optimal control problem of a planarhorizontal underactuated 119877119877 robot manipulator is studiedIn this robot model the first joint is not actuated thus 119906 =
(0 1199062)119879 and (2) can be split into
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792
minus 120573 sin 1205792(2 12057911205792+ 1205792
2) = 0
(56)
(120575 + 120573 cos 1205792) 1205791+ 120575 1205792+ 120573 sin 120579
21205792
1= 1199062 (57)
As explained in [12] since gravity terms are all zero and1205791does not intervene in the system inertia matrix (56) can
be partially integrated to
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792+ 1198881= 0 (58)
Actually constraint (56) is completely integrable giving rise toan holonomic constraintThe resulting holonomic constrainttakes different forms depending on the value of 119888
1which
depends on the initial conditions Therefore two cases havebeen considered
(i) when the initial velocities 1205791(119905119868) and 120579
2(119905119868) are both
zero(ii) when the initial velocity 120579
1(119905119868) is nonzero
721 Problem 3 Initial Value Problem with Zero Initial Veloc-ities An initial value problem has been solved with the fol-lowing initial and final conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rad] 120579
2(119905119865) = 120587 [rad]
1205791(119905119868) = 0 [rads] 120579
1(119905119865) = 0 [rads]
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(59)
10 Abstract and Applied Analysis
10 20 30 40 50 60
05
1
15
2
25
(a) 1205791
10 20 30 40 50 60minus05
minus1
minus15
minus2
minus25
minus3
(b) 1205792
10 20 30 40 50 60
minus10
minus5
5
10
(c) 1205791
10 20 30 40 50 60
minus10
minus5
5
10
15
(d) 1205792
10 20 30 40 50 60
minus300
minus200
minus100
100
200
300
(e) 1199061
Figure 3 Control and state variables of the optimal solution of problem 1 a boundary value problem for the planar 119877119877 robot manipulatorwith boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 2
Figure 4 Sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 corresponding to the optimal solutionof problem 2 an initial value problem for an 119877119877 robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) =
0 [rads] 1205792(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained with a discretization of
[119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control and state variables are
represented in Figure 5
Abstract and Applied Analysis 11
10 20 30 40 50 60
05
1
15
2
25
(a) 1205791
10 20 30 40 50 60
minus25
minus2
(b) 1205792
10 20 30 40 50 60
minus4
minus2
2
4
6
8
10
(c) 1205791
minus4
minus6
minus210 20 30 40 50 60
2
4
(d) 1205792
10 20 30 40 50 60minus50
50
100
150
(e) 1199061
Figure 5 Control and state variables of the optimal solution of problem 2 an initial value problem for a119877119877 robotmanipulator with boundaryconditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = free
and 1205792(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of configurations of the
robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 4
The initial values of the control variable and of themultipliershave been set to zero whereas their final value is left free
Since there is a holonomic constraint that relates thevalues of the angles 120579
1and 120579
2 without integrating (58) we
are not able to find the value of 1205791(119905119865) consistent with 120579
1(119905119868)
Therefore no final conditions have been imposed on 1205791
From the initial conditions of the problem we obtain 1198881=
0 Equation (58) with 1198881= 0 corresponds to the homogeneous
differential constraint
119889120577 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792= 0 (60)
The differential 119889120577 is not exact However it becomes an exactdifferential if multiplied by the factor 1(120572 + 2120573 cos 120579
2) This
operation does not alter the differential equation (60) Inthis case there does exist a function 120577 whose differentialcoincides with the expression 119889120577(120572 + 2120573 cos 120579
2) Due to
the existence of this function the integral of 119889120577 between
two points depends only on these points and not on theintegration path Equation (60) rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2)
120572 + 2120573 cos 1205792
1198891205792
(61)
can be integrated by separating variables The correspondingholonomic constraint has the following expression
1205791= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1205792
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1205792
2+ 1198882
(62)
To express this optimal control problem in the form ofa basic optimal control problem we first have to convert (57)
12 Abstract and Applied Analysis
into a first-order differential model introducing the followingchange of variables
1199091= 1205791 119909
3= 1205791
1199092= 1205792 119909
4= 1205792
(63)
with the following additional relations
1199091015840
1= 1199093
1199091015840
2= 1199094
(64)
Thus the optimal control problem is to minimize
int
1
0
1199062
2119889119905 (65)
subject to the constraints1199091
= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1199092
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1199092
2+ 1198882
(66)
(120575 + 120573 cos 1205792) 1199091015840
3+ 1205751199091015840
4+ 120573 sin119909
21199092
3= 1199062 (67)
and the additional constraints (64) To reformulate thisoptimal control problem as an unconstrained calculus ofvariations problem let X be
X = [
[
1198831
sdot sdot sdot
1198839
]
]
(68)
such that119883119894= 119909119894 119894 = 1 4
1198831015840
5= 1199061 119883
5(119905119868) = 0
(69)
where 11988310158406with 119883
6(119905119868) = 0 is the multiplier associated with
the holonomic constraint (66) 11988310158407with 119883
7(119905119868) = 0 is the
multipliers associated with the differential constraint (67)and 1198831015840
8with 119883
8(119905119868) = 0 and 1198831015840
9with 119883
9(119905119868) = 0 are the
multipliers associatedwith the additional equality constraints(64)
Thus the holonomic constraint of the problem can berewritten as follows
120593 (119905X)
= 1198831minus((120572 minus 2120575)
times tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1198832
2)]]
]
)
times (radic41205732 minus 1205722)
minus1
+1198832
2= 0
(70)
Now the technique described in Section 51 to deal withholonomic constraints can be applied to 120593(119905X) and thisholonomic constraint is replaced by
120593119905+ 1205931198831198831015840= 0
120593 (0 119883 (0)) = 0
(71)
From the initial conditions of the problem the latter equationreduces to the equality 0 = 0 whereas the former takes thefollowing form
(120572 + 2120573 cos (1198832))1198833+ (120575 + 120573 cos (119883
2))1198834= 0 (72)
The corresponding Hamiltonian is
1198661= 11988310158402
5+ 1198831015840
6((120572 + 2120573 cos (119883
2))1198833
+ (120575 + 120573 cos (1198832))1198834)
+1198831015840
7(120573 sin (119883
2)1198832
3+ (120575 + 120573 cos (119883
2))1198831015840
3
+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(73)
It is not difficult to check that matrix 1198661X1015840X1015840
is singularThis is due to the fact that to handle our optimal controlproblemwhich involves second-order differential constraintswe converted them into first-order differential constraintsTherefore we apply again the technique of Section 51obtaining the identity 0 = 0 and the following constraint
minus 120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4= 0
(74)
The corresponding Hamiltonian is
1198662= 11988310158402
5+ 1198831015840
6
times (minus120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4)
+ 1198831015840
7(120573 sin (119883
2)1198832
3+(120575+120573 cos (119883
2))1198831015840
3+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(75)
It is not difficult to check that matrix 1198662X1015840X1015840
in this case is notsingular since its determinant is
Substituting the values of120572120573 and 120575 this expression becomesdet(119866
2X1015840X1015840) = 1128(43 minus 2 cos(2119883
2))2 which is always
positive for any real value1198832 Figure 6 shows the sequence of
configurations of the robot at times 11989632with 119896 = 0 1 32and Figure 7 depicts control and state variables of the optimal
Abstract and Applied Analysis 13
Figure 6 Sequence of configurations of the robot manipulator attimes 11989632 with 119896 = 0 1 32 corresponding to the optimal sol-ution of problem 3 an initial value problem for an underactuated119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) =
0 [rad] 1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads]
1205791(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2
(119905119865) = 0 [rads] obtained with a discretization of [119905
119868 119905119865] into 64
subintervals The initial and final times are 119905119868= 0 and 119905
119865=
1 [s] respectively The corresponding control and state variables arerepresented in Figure 7
solution obtained with a discretization of the interval [119905119868 119905119865]
into 64 subintervalsIn particular we get 120579
1(119905119865) = minus110248 [rad] To check the
consistency of this result with the holonomic constraint (62)since 120579
1(119905119868) = 1205792(119905119868) = 0 [rads] we get from (58) that 119888
1= 0
and using the initial condition 1205791(119905119868) = 1205792(119905119868) = 0 [rad] we
get from (62) that 1198882= 0 Having established the value of the
constant 1198882 we obtain from the same equation for 120579
2(119905119865) =
120587 [rad] that 1205791(119905119865) = minus110248 [rad] which coincides with
the value of 1205791(119905119865) obtained numerically
722 Problem 4 Initial Value Problem with Nonzero InitialVelocity 120579
1 Another initial value problem has been solved
with the following conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rad] 120579
2(119905119865) = 120587 [rad]
1205791(119905119868) = 5 [rads] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(77)
The initial values of the multipliers have been set to zerowhereas their final value is left free Notice that no finalconditions have been imposed on 120579
1and 120579
1 The same
considerations done in previous section hold in this caseas well The technique described in Section 51 must beapplied twice leading to the differential constraint (74) andto the Hamiltonian (75) Figure 8 shows the sequence ofconfigurations of the robot at times 11989632with 119896 = 0 1 32and Figure 9 depicts the control and state variables ofthe optimal solution obtained with a discretization of theinterval [119905
119868 119905119865] into 64 subintervals In particular we get that
1205791(119905119865) = 617172 [rad] and 120579
1(119905119865) = 900163 [rads] To
check the consistency of the obtained value of 1205791(119905119865) with
the holonomic constraint consider (58) We can calculate
the constant 1198881using the initial conditions of the problem
obtaining
1198881= minus 120579
1(119905119868) (120572 + 2120573 cos 120579
2(119905119868))
minus 1205792(119905119868) (120575 + 120573 cos 120579
2(119905119868)) = minus225
(78)
Since 1198881= 0 (58) corresponds in this case to the differential
constraint
119889120578 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792+ 1198881119889119905 = 0
(79)
Equation (79) can be rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2 (119905))
120572 + 2120573 cos 1205792 (119905)
1198891205792minus
1198881
120572 + 2120573 cos 1205792 (119905)
119889119905 (80)
To check the obtained value of 1205791(119905119865) 1198891205791is numerically
integrated between 1205791(119905119868) and 120579
1(119905119865) using the interpolated
numerical optimal solution obtained for 1205792(119905) We get that
1205791(119905119865) = 618705 This value is close to 617172
To check the consistency of the obtained value of 1205791(119905119865)
with the constraint (58) using the computed value 1198881= minus225
and the final conditions 1205792(119905119865) = 0 120579
2(119905119865) = 120587 of the problem
we obtain
1205791(119905119865) =
minus (120575 + 120573 cos 1205792(119905119865))
120572 + 2120573 cos 1205792(119905119865)
1205792(119905119865)
minus1198881
120572 + 2120573 cos 1205792(119905119865)= 9
(81)
This value is very close to the value of 1205791(119905119865) obtained
numerically
73 Computational Issues If the optimal control problem has119898 variables and the time interval [119905
119868 119905119865] has been discretized
into 119873 subintervals the resulting set of difference equations(38) has119898times(119873minus1) equations and119898times(119873minus1) variables plusthe equations and variables due to transversality conditionsFeasible solutions have been used as initial guesses of thealgorithm
The solution of the nonlinear system of difference equa-tions (38) has been obtained using a damped Newtonalgorithm within a line search methodology implementedin Mathematica 7 under Mac OS X operating system (see[22 23] for more details)
8 Conclusion
In this paper the trajectory planning problem for planarunderactuated robot manipulators with two revolute jointswithout gravity has been studied This problem is solved asan optimal control problem based on a numerical resolutionof an unconstrained variational calculus reformulation of theoptimal control problem in which the dynamic equation ofthe mechanical system is regarded as a constraint It hasbeen shown that this reformulation method based on special
14 Abstract and Applied Analysis
10 20 30 40 50 60
minus1
minus08
minus06
minus04
minus02
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
minus15
minus1
minus05
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
(d) 1205792
10 20 30 40 50 60
minus15
minus10
5
10
15
minus5
(e) 1199062
Figure 7 Control and state variables of the optimal solution of problem 3 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 119906
2(119905119868) = 0Nm 120579
1(119905119865) = free
1205792(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads]The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectivelyThe corresponding
sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 6
Figure 8 Sequence of configurations of the robot manipulator at times 11989632 with 119896 = 0 1 32 corresponding to the optimal solutionof problem 4 an initial value problem for an underactuated 119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad]
1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained
with a discretization of [119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control
and state variables are represented in Figure 9
Abstract and Applied Analysis 15
10 20 30 40 50 60
1
2
3
4
5
6
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
4
5
6
7
8
9
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
6
(d) 1205792
10 20 30 40 50 60minus5
5
10
15
20
(e) 1199062
Figure 9 Control and state variables of the optimal solution of problem 4 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 8
derivative multipliers is able to tackle both integrable andnonintegrable differential constraints of the dynamic modelsof underactuated planar horizontal robot manipulators withtwo revolute joints This method can be seamlessly appliedin the presence of additional constraints on the mechanicalsystem
References
[1] A De Luca S Iannitti R Mattone and G Oriolo ldquoUnderactu-ated manipulators control properties and techniquesrdquoMachineIntelligence and Robotic Control vol 4 no 3 pp 113ndash125 2002
[2] G A Bliss Lectures on the Calculus of Variations University ofChicago Press Chicago Ill USA 1946
[3] J Gregory and C Lin Constrained Optimization in the Calculusof Variations and Optimal Control theory Chapman amp Hall1996
[4] W-S Koon and J E Marsden ldquoOptimal control for holonomicand nonholonomic mechanical systems with symmetry andLagrangian reductionrdquo SIAM Journal on Control and Optimiza-tion vol 35 no 3 pp 901ndash929 1997
[5] A M Bloch Nonholonomic Mechanics and Control SpringerNew York NY USA 2003
[6] I I Hussein and A M Bloch ldquoOptimal control of underactu-ated nonholonomic mechanical systemsrdquo IEEE Transactions onAutomatic Control vol 53 no 3 pp 668ndash682 2008
[7] X Z Lai J H She S X Yang andMWu ldquoComprehensive uni-fied control strategy for underactuated two-link manipulatorsrdquoIEEE Transactions on Systems Man and Cybernetics B vol 39no 2 pp 389ndash398 2009
[8] J P Ordaz-Oliver O J Santos-Sanchez and V Lopez-MoralesldquoToward a generalized sub-optimal control method of underac-tuated systemsrdquo Optimal Control Applications amp Methods vol33 no 3 pp 338ndash351 2012
16 Abstract and Applied Analysis
[9] R Seifried ldquoTwo approaches for feedforward control andoptimal design of underactuatedmultibody systemsrdquoMultibodySystem Dynamics vol 27 no 1 pp 75ndash93 2012
[10] M Buss O von Stryk R Bulirsch and G Schmidt ldquoTowardshybrid optimal controlrdquo atmdashAutomatisierungstechnik vol 48no 9 pp 448ndash459 2000
[11] M Buss M Glocker M Hardt O von Stryk R Bulirsch andG Schmidt ldquoNonlinear hybrid dynamical systems modelingoptimal control and applicationsrdquo in Modelling Analysis andDesign of Hybrid Systems S Engell G Frehse and E SchniederEds vol 279 of Lecture Notes in Control and InformationScience pp 331ndash335 Springer 2002
[12] G Oriolo and Y Nakamura ldquoControl of mechanical systemswith second-order nonholonomic constraints underactuatedmanipulatorsrdquo in Proceedings of the 30th IEEE Conference onDecision and Control pp 2398ndash2403 December 1991
[13] T J Tarn M Zhang and A Serrani ldquoNew integrability condi-tions for differential constraintsrdquo Systems and Control Lettersvol 49 no 5 pp 335ndash345 2003
[14] H J Sussmann ldquoA general theorem on local controllabilityrdquoSIAM Journal on Control and Optimization vol 25 no 1 pp158ndash194 1987
[15] F Bullo A D Lewis and K M Lynch ldquoControllable kinematicreductions for mechanical systems concepts computationaltools and examplesrdquo in Proceedings of International Symposiumon Mathematical Theory of Networks and Systems 2002
[16] F Bullo and A D Lewis ldquoLow-order controllability and kine-matic reductions for affine connection control systemsrdquo SIAMJournal on Control andOptimization vol 44 no 3 pp 885ndash9082006
[17] A D Lewis and R MMurray ldquoConfiguration controllability ofsimple mechanical control systemsrdquo SIAM Journal on Controland Optimization vol 35 no 3 pp 766ndash790 1997
[18] F Bullo and K M Lynch ldquoKinematic controllability for decou-pled trajectory planning in underactuatedmechanical systemsrdquoIEEE Transactions on Robotics and Automation vol 17 no 4 pp402ndash412 2001
[19] M R Hestenes Calculus of Variations and Optimcl ControlTheory John Wiley amp Sons 1966
[20] J Gregory ldquoA new systematic method for efficiently solvingholonomic (and nonholonomic) constraint problemsrdquo Analysisand Applications vol 8 no 1 pp 85ndash98 2010
[21] J Gregory and R S Wang ldquoDiscrete variable methods forthe m-dependent variable nonlinear extremal problem in thecalculus of variationsrdquo SIAM Journal onNumerical Analysis vol27 no 2 pp 470ndash487 1990
[22] Wolfram Research 2012[23] J J More and D J Thuente ldquoLine search algorithms with guar-
anteed sufficient decreaserdquo ACM Transactions on MathematicalSoftware vol 20 no 3 pp 286ndash307 1994
dealing with the Euler-Lagrange equation (21) Thus thismethod uses no numerical corner conditions and avoidspatching solutions to (21) between corners
Let 119873 be a large positive integer ℎ = (119887 minus 119886)119873 and let120587 = (119886 = 119886
0lt 1198861lt sdot sdot sdot lt 119886
119873= 119887) be a partition of the
interval [119886 119887] where 119886119896= 119886 + 119896ℎ for 119896 = 0 1 119873 Define
the one-dimensional spline hat functions
119908119896 (119909) =
119909 minus 119886119896minus1
ℎif 119886119896minus1
lt 119909 lt 119886119896
119886119896+1
minus 119909
ℎif 119886119896lt 119909 lt 119886
119896+1
0 otherwise
(35)
where 119896 = 1 2 119873 minus 1 Define also the 119898-dimensionalpiecewise linear component functions
119910ℎ (119909) =
119873
sum
119896=0
119882119896 (119909) 119862119896 119911
ℎ (119909) =
119873
sum
119896=0
119882119896 (119909)119863119896 (36)
where 119882119896(119909) = 119908
119896(119909)119868119898times119898
119910ℎ(119909) is the sought numerical
solution and 119911ℎ(119909) is a numerical variation In particular the
constant vectors 119862119896are to be determined by the algorithm
developed by us and the constant vectors 119863119896are arbitrary
Thus the discretized form of (20) is obtained in eachsubinterval [119886
119896minus1 119886119896+1] For the sake of clarity of exposition
we assume that119898 = 1 Note that 1198681015840(119910 119911) in (20) is linear in 119911so that a three-term relationship may be obtained at 119909 = 119886
119896
by choosing 119911(119909) = 119908119896(119909) for 119896 = 1 2 119873 minus 1 Thus
In these equations 119886lowast119896= (119886119896+ 119886119896+1)2 and 119910
119896= 119910ℎ(119886119896) is
the computed value of 119910ℎ(119909) at 119886
119896 In the general case when
119898 gt 1 the same result is obtained but 1198911199101015840 and 119891
119910are
column 119898-vectors of functions with 119894th component 1198911199101198941015840 and
119891119910119894 respectively Similarly (119910
119896+119910119896minus1)2 is the119898-vector which
is the average of the119898-vectors 119910ℎ(119886119896) and 119910
ℎ(119886119896minus1)
By the same arguments that led to (38)
1198911199101015840 (119886lowast
119873minus1119910119873+ 119910119873minus1
2119910119873minus 119910119873minus1
ℎ)
+ℎ
2119891119910(119886lowast
119896minus1119910119873+ 119910119873minus1
2119910119873minus 119910119873minus1
ℎ) = 0
(39)
which is the numerical equivalent of the transversality condi-tion (23) For further details see [3 Chapter 6]
It has been shown in [21] that with this method the globalerror has a priori global reduction ratio of119874(ℎ2) In practiceif the step size ℎ is halved the error decreases by 4
7 Implementation and Results
Several numerical experiments have been carried out forboth 119877119877 and 119877119877 planar horizontal underactuated robotmanipulators
71 Planar Horizontal Underactuated 119877119877 Robot ManipulatorIn this section the optimal control problem of a planar hor-izontal underactuated 119877119877 is studied In this robot model thesecond joint is not actuated thus 119906 = (119906
1 0)119879 In this case
it is neither possible to integrate partially nor completely thenonholonomic constraint because the manipulator inertiamatrix contains terms in 120579
2(see [12]) Hence the system is
controllable The numerical results of the application of ourmethod for optimal control to a boundary value problem andto an initial value problem for this system will be described
For a planar horizontal underactuated119877119877 (2) can be splitinto
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792
minus 120573 sin 1205792(2 12057911205792+ 1205792
2) = 1199061
(120575 + 120573 cos 1205792) 1205791+ 120575 1205792+ 120573 sin 120579
21205792
1= 0
(40)
To express optimal control problems that involve this second-order differential constraints in the form of a basic optimal
8 Abstract and Applied Analysis
control problem we have first to convert it into first-orderdifferential constraints introducing the following change ofvariables
1199091= 1205791 119909
3= 1205791
1199092= 1205792 119909
4= 1205792
(41)
with the following additional relations
1199091015840
1= 1199093
1199091015840
2= 1199094
(42)
Thus the second-order differential equations (40) are con-verted into the first-order differential equations
(120572 + 2120573 cos1199092) 1199091015840
3+ (120575 + 120573 cos119909
2) 1199091015840
4
minus 120573 sin1199092(211990931199094+ 1199092
4) = 1199061
(43)
(120575 + 120573 cos1199092) 1199091015840
3+ 1205751199091015840
4+ 120573 sin119909
21199092
3= 0 (44)
Relations (42) (43) and (44) are now the differential con-straints of the optimal control problem and the objectivefunctional to minimize is
119869 = int
119905119865
119905119868
1199062
1119889119905 (45)
Then we introduce the following new variables
X = [
[
1198831
sdot sdot sdot
1198839
]
]
(46)
such that
119883119894= 119909119894 119894 = 1 4 (47)
1198831015840
5= 1199061 119883
5(119905119868) = 0 (48)
where 11988310158406with 119883
6(119905119868) = 0 is the multiplier associated with
whereas in the initial value problem 119883119894(119905119865) will be free for
some 119894The initial values of control variables and multipliers
have been set to zero whereas their final values have notbeen assigned in both optimal control problems Thereforetransversality conditions are needed in both cases for thevariables119883
119894(119905119865) 119894 = 5 10 and they will be of the form
Figure 2 Sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 corresponding to the optimal solutionof problem 1 a boundary value problem for the planar 119877119877 robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad]
1205791(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads] obtained with a
discretization of [119905119868 119905119865] into 64 subintervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control and
state variables are represented in Figure 3
The initial values of control variable and of the multipliershave been set to zero whereas their final values are left freeFigure 2 shows the sequence of configurations of the robot attimes 119905 = 11989632 119896 = 0 1 32 Since the configurations ofthe sequence overlap it has been split into smaller sequencesfor a better visualization of the manipulator motion Figure 3depicts the corresponding control and state variables of theoptimal solution of this boundary value problem obtainedwith a discretization of the time interval [119905
119868 119905119865] into 64
subintervals The value of the objective functional for thissolution is 345185 [J]
712 Problem 2 Initial Value Problem An initial valueproblem has also been solved with the following initial andfinal conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) =
120587
2[rad]
1205792(119905119868) = minus
120587
2[rad] 120579
2(119905119865) = minus
120587
2[rad]
1205791(119905119868) = 0 [rads] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(55)
The initial values of control variable and of the multipliershave been set to zero whereas their final values are left freeThe only difference between these conditions and those of theboundary value problem described in Section 711 is that now1205791(119905119865) = free
Figures 4 and 5 depict the sequence of configurations the119877119877 robot manipulator and the corresponding control andstate variables of the optimal solution of this initial valueproblem respectively obtained with a discretization of thetime interval [119905
119868 119905119865] into 64 subintervals The value of the
objective functional for this solution is 56472 [J]This value islower than the value of the objective functional of the solutionof the boundary value problem described in Section 711because now is 120579
1(119905119865) = free and the control system does
not have to spend energy to stop it
72 Planar Horizontal Underactuated 119877119877 Robot ManipulatorIn this section the optimal control problem of a planarhorizontal underactuated 119877119877 robot manipulator is studiedIn this robot model the first joint is not actuated thus 119906 =
(0 1199062)119879 and (2) can be split into
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792
minus 120573 sin 1205792(2 12057911205792+ 1205792
2) = 0
(56)
(120575 + 120573 cos 1205792) 1205791+ 120575 1205792+ 120573 sin 120579
21205792
1= 1199062 (57)
As explained in [12] since gravity terms are all zero and1205791does not intervene in the system inertia matrix (56) can
be partially integrated to
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792+ 1198881= 0 (58)
Actually constraint (56) is completely integrable giving rise toan holonomic constraintThe resulting holonomic constrainttakes different forms depending on the value of 119888
1which
depends on the initial conditions Therefore two cases havebeen considered
(i) when the initial velocities 1205791(119905119868) and 120579
2(119905119868) are both
zero(ii) when the initial velocity 120579
1(119905119868) is nonzero
721 Problem 3 Initial Value Problem with Zero Initial Veloc-ities An initial value problem has been solved with the fol-lowing initial and final conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rad] 120579
2(119905119865) = 120587 [rad]
1205791(119905119868) = 0 [rads] 120579
1(119905119865) = 0 [rads]
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(59)
10 Abstract and Applied Analysis
10 20 30 40 50 60
05
1
15
2
25
(a) 1205791
10 20 30 40 50 60minus05
minus1
minus15
minus2
minus25
minus3
(b) 1205792
10 20 30 40 50 60
minus10
minus5
5
10
(c) 1205791
10 20 30 40 50 60
minus10
minus5
5
10
15
(d) 1205792
10 20 30 40 50 60
minus300
minus200
minus100
100
200
300
(e) 1199061
Figure 3 Control and state variables of the optimal solution of problem 1 a boundary value problem for the planar 119877119877 robot manipulatorwith boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 2
Figure 4 Sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 corresponding to the optimal solutionof problem 2 an initial value problem for an 119877119877 robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) =
0 [rads] 1205792(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained with a discretization of
[119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control and state variables are
represented in Figure 5
Abstract and Applied Analysis 11
10 20 30 40 50 60
05
1
15
2
25
(a) 1205791
10 20 30 40 50 60
minus25
minus2
(b) 1205792
10 20 30 40 50 60
minus4
minus2
2
4
6
8
10
(c) 1205791
minus4
minus6
minus210 20 30 40 50 60
2
4
(d) 1205792
10 20 30 40 50 60minus50
50
100
150
(e) 1199061
Figure 5 Control and state variables of the optimal solution of problem 2 an initial value problem for a119877119877 robotmanipulator with boundaryconditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = free
and 1205792(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of configurations of the
robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 4
The initial values of the control variable and of themultipliershave been set to zero whereas their final value is left free
Since there is a holonomic constraint that relates thevalues of the angles 120579
1and 120579
2 without integrating (58) we
are not able to find the value of 1205791(119905119865) consistent with 120579
1(119905119868)
Therefore no final conditions have been imposed on 1205791
From the initial conditions of the problem we obtain 1198881=
0 Equation (58) with 1198881= 0 corresponds to the homogeneous
differential constraint
119889120577 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792= 0 (60)
The differential 119889120577 is not exact However it becomes an exactdifferential if multiplied by the factor 1(120572 + 2120573 cos 120579
2) This
operation does not alter the differential equation (60) Inthis case there does exist a function 120577 whose differentialcoincides with the expression 119889120577(120572 + 2120573 cos 120579
2) Due to
the existence of this function the integral of 119889120577 between
two points depends only on these points and not on theintegration path Equation (60) rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2)
120572 + 2120573 cos 1205792
1198891205792
(61)
can be integrated by separating variables The correspondingholonomic constraint has the following expression
1205791= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1205792
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1205792
2+ 1198882
(62)
To express this optimal control problem in the form ofa basic optimal control problem we first have to convert (57)
12 Abstract and Applied Analysis
into a first-order differential model introducing the followingchange of variables
1199091= 1205791 119909
3= 1205791
1199092= 1205792 119909
4= 1205792
(63)
with the following additional relations
1199091015840
1= 1199093
1199091015840
2= 1199094
(64)
Thus the optimal control problem is to minimize
int
1
0
1199062
2119889119905 (65)
subject to the constraints1199091
= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1199092
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1199092
2+ 1198882
(66)
(120575 + 120573 cos 1205792) 1199091015840
3+ 1205751199091015840
4+ 120573 sin119909
21199092
3= 1199062 (67)
and the additional constraints (64) To reformulate thisoptimal control problem as an unconstrained calculus ofvariations problem let X be
X = [
[
1198831
sdot sdot sdot
1198839
]
]
(68)
such that119883119894= 119909119894 119894 = 1 4
1198831015840
5= 1199061 119883
5(119905119868) = 0
(69)
where 11988310158406with 119883
6(119905119868) = 0 is the multiplier associated with
the holonomic constraint (66) 11988310158407with 119883
7(119905119868) = 0 is the
multipliers associated with the differential constraint (67)and 1198831015840
8with 119883
8(119905119868) = 0 and 1198831015840
9with 119883
9(119905119868) = 0 are the
multipliers associatedwith the additional equality constraints(64)
Thus the holonomic constraint of the problem can berewritten as follows
120593 (119905X)
= 1198831minus((120572 minus 2120575)
times tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1198832
2)]]
]
)
times (radic41205732 minus 1205722)
minus1
+1198832
2= 0
(70)
Now the technique described in Section 51 to deal withholonomic constraints can be applied to 120593(119905X) and thisholonomic constraint is replaced by
120593119905+ 1205931198831198831015840= 0
120593 (0 119883 (0)) = 0
(71)
From the initial conditions of the problem the latter equationreduces to the equality 0 = 0 whereas the former takes thefollowing form
(120572 + 2120573 cos (1198832))1198833+ (120575 + 120573 cos (119883
2))1198834= 0 (72)
The corresponding Hamiltonian is
1198661= 11988310158402
5+ 1198831015840
6((120572 + 2120573 cos (119883
2))1198833
+ (120575 + 120573 cos (1198832))1198834)
+1198831015840
7(120573 sin (119883
2)1198832
3+ (120575 + 120573 cos (119883
2))1198831015840
3
+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(73)
It is not difficult to check that matrix 1198661X1015840X1015840
is singularThis is due to the fact that to handle our optimal controlproblemwhich involves second-order differential constraintswe converted them into first-order differential constraintsTherefore we apply again the technique of Section 51obtaining the identity 0 = 0 and the following constraint
minus 120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4= 0
(74)
The corresponding Hamiltonian is
1198662= 11988310158402
5+ 1198831015840
6
times (minus120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4)
+ 1198831015840
7(120573 sin (119883
2)1198832
3+(120575+120573 cos (119883
2))1198831015840
3+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(75)
It is not difficult to check that matrix 1198662X1015840X1015840
in this case is notsingular since its determinant is
Substituting the values of120572120573 and 120575 this expression becomesdet(119866
2X1015840X1015840) = 1128(43 minus 2 cos(2119883
2))2 which is always
positive for any real value1198832 Figure 6 shows the sequence of
configurations of the robot at times 11989632with 119896 = 0 1 32and Figure 7 depicts control and state variables of the optimal
Abstract and Applied Analysis 13
Figure 6 Sequence of configurations of the robot manipulator attimes 11989632 with 119896 = 0 1 32 corresponding to the optimal sol-ution of problem 3 an initial value problem for an underactuated119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) =
0 [rad] 1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads]
1205791(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2
(119905119865) = 0 [rads] obtained with a discretization of [119905
119868 119905119865] into 64
subintervals The initial and final times are 119905119868= 0 and 119905
119865=
1 [s] respectively The corresponding control and state variables arerepresented in Figure 7
solution obtained with a discretization of the interval [119905119868 119905119865]
into 64 subintervalsIn particular we get 120579
1(119905119865) = minus110248 [rad] To check the
consistency of this result with the holonomic constraint (62)since 120579
1(119905119868) = 1205792(119905119868) = 0 [rads] we get from (58) that 119888
1= 0
and using the initial condition 1205791(119905119868) = 1205792(119905119868) = 0 [rad] we
get from (62) that 1198882= 0 Having established the value of the
constant 1198882 we obtain from the same equation for 120579
2(119905119865) =
120587 [rad] that 1205791(119905119865) = minus110248 [rad] which coincides with
the value of 1205791(119905119865) obtained numerically
722 Problem 4 Initial Value Problem with Nonzero InitialVelocity 120579
1 Another initial value problem has been solved
with the following conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rad] 120579
2(119905119865) = 120587 [rad]
1205791(119905119868) = 5 [rads] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(77)
The initial values of the multipliers have been set to zerowhereas their final value is left free Notice that no finalconditions have been imposed on 120579
1and 120579
1 The same
considerations done in previous section hold in this caseas well The technique described in Section 51 must beapplied twice leading to the differential constraint (74) andto the Hamiltonian (75) Figure 8 shows the sequence ofconfigurations of the robot at times 11989632with 119896 = 0 1 32and Figure 9 depicts the control and state variables ofthe optimal solution obtained with a discretization of theinterval [119905
119868 119905119865] into 64 subintervals In particular we get that
1205791(119905119865) = 617172 [rad] and 120579
1(119905119865) = 900163 [rads] To
check the consistency of the obtained value of 1205791(119905119865) with
the holonomic constraint consider (58) We can calculate
the constant 1198881using the initial conditions of the problem
obtaining
1198881= minus 120579
1(119905119868) (120572 + 2120573 cos 120579
2(119905119868))
minus 1205792(119905119868) (120575 + 120573 cos 120579
2(119905119868)) = minus225
(78)
Since 1198881= 0 (58) corresponds in this case to the differential
constraint
119889120578 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792+ 1198881119889119905 = 0
(79)
Equation (79) can be rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2 (119905))
120572 + 2120573 cos 1205792 (119905)
1198891205792minus
1198881
120572 + 2120573 cos 1205792 (119905)
119889119905 (80)
To check the obtained value of 1205791(119905119865) 1198891205791is numerically
integrated between 1205791(119905119868) and 120579
1(119905119865) using the interpolated
numerical optimal solution obtained for 1205792(119905) We get that
1205791(119905119865) = 618705 This value is close to 617172
To check the consistency of the obtained value of 1205791(119905119865)
with the constraint (58) using the computed value 1198881= minus225
and the final conditions 1205792(119905119865) = 0 120579
2(119905119865) = 120587 of the problem
we obtain
1205791(119905119865) =
minus (120575 + 120573 cos 1205792(119905119865))
120572 + 2120573 cos 1205792(119905119865)
1205792(119905119865)
minus1198881
120572 + 2120573 cos 1205792(119905119865)= 9
(81)
This value is very close to the value of 1205791(119905119865) obtained
numerically
73 Computational Issues If the optimal control problem has119898 variables and the time interval [119905
119868 119905119865] has been discretized
into 119873 subintervals the resulting set of difference equations(38) has119898times(119873minus1) equations and119898times(119873minus1) variables plusthe equations and variables due to transversality conditionsFeasible solutions have been used as initial guesses of thealgorithm
The solution of the nonlinear system of difference equa-tions (38) has been obtained using a damped Newtonalgorithm within a line search methodology implementedin Mathematica 7 under Mac OS X operating system (see[22 23] for more details)
8 Conclusion
In this paper the trajectory planning problem for planarunderactuated robot manipulators with two revolute jointswithout gravity has been studied This problem is solved asan optimal control problem based on a numerical resolutionof an unconstrained variational calculus reformulation of theoptimal control problem in which the dynamic equation ofthe mechanical system is regarded as a constraint It hasbeen shown that this reformulation method based on special
14 Abstract and Applied Analysis
10 20 30 40 50 60
minus1
minus08
minus06
minus04
minus02
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
minus15
minus1
minus05
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
(d) 1205792
10 20 30 40 50 60
minus15
minus10
5
10
15
minus5
(e) 1199062
Figure 7 Control and state variables of the optimal solution of problem 3 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 119906
2(119905119868) = 0Nm 120579
1(119905119865) = free
1205792(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads]The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectivelyThe corresponding
sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 6
Figure 8 Sequence of configurations of the robot manipulator at times 11989632 with 119896 = 0 1 32 corresponding to the optimal solutionof problem 4 an initial value problem for an underactuated 119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad]
1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained
with a discretization of [119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control
and state variables are represented in Figure 9
Abstract and Applied Analysis 15
10 20 30 40 50 60
1
2
3
4
5
6
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
4
5
6
7
8
9
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
6
(d) 1205792
10 20 30 40 50 60minus5
5
10
15
20
(e) 1199062
Figure 9 Control and state variables of the optimal solution of problem 4 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 8
derivative multipliers is able to tackle both integrable andnonintegrable differential constraints of the dynamic modelsof underactuated planar horizontal robot manipulators withtwo revolute joints This method can be seamlessly appliedin the presence of additional constraints on the mechanicalsystem
References
[1] A De Luca S Iannitti R Mattone and G Oriolo ldquoUnderactu-ated manipulators control properties and techniquesrdquoMachineIntelligence and Robotic Control vol 4 no 3 pp 113ndash125 2002
[2] G A Bliss Lectures on the Calculus of Variations University ofChicago Press Chicago Ill USA 1946
[3] J Gregory and C Lin Constrained Optimization in the Calculusof Variations and Optimal Control theory Chapman amp Hall1996
[4] W-S Koon and J E Marsden ldquoOptimal control for holonomicand nonholonomic mechanical systems with symmetry andLagrangian reductionrdquo SIAM Journal on Control and Optimiza-tion vol 35 no 3 pp 901ndash929 1997
[5] A M Bloch Nonholonomic Mechanics and Control SpringerNew York NY USA 2003
[6] I I Hussein and A M Bloch ldquoOptimal control of underactu-ated nonholonomic mechanical systemsrdquo IEEE Transactions onAutomatic Control vol 53 no 3 pp 668ndash682 2008
[7] X Z Lai J H She S X Yang andMWu ldquoComprehensive uni-fied control strategy for underactuated two-link manipulatorsrdquoIEEE Transactions on Systems Man and Cybernetics B vol 39no 2 pp 389ndash398 2009
[8] J P Ordaz-Oliver O J Santos-Sanchez and V Lopez-MoralesldquoToward a generalized sub-optimal control method of underac-tuated systemsrdquo Optimal Control Applications amp Methods vol33 no 3 pp 338ndash351 2012
16 Abstract and Applied Analysis
[9] R Seifried ldquoTwo approaches for feedforward control andoptimal design of underactuatedmultibody systemsrdquoMultibodySystem Dynamics vol 27 no 1 pp 75ndash93 2012
[10] M Buss O von Stryk R Bulirsch and G Schmidt ldquoTowardshybrid optimal controlrdquo atmdashAutomatisierungstechnik vol 48no 9 pp 448ndash459 2000
[11] M Buss M Glocker M Hardt O von Stryk R Bulirsch andG Schmidt ldquoNonlinear hybrid dynamical systems modelingoptimal control and applicationsrdquo in Modelling Analysis andDesign of Hybrid Systems S Engell G Frehse and E SchniederEds vol 279 of Lecture Notes in Control and InformationScience pp 331ndash335 Springer 2002
[12] G Oriolo and Y Nakamura ldquoControl of mechanical systemswith second-order nonholonomic constraints underactuatedmanipulatorsrdquo in Proceedings of the 30th IEEE Conference onDecision and Control pp 2398ndash2403 December 1991
[13] T J Tarn M Zhang and A Serrani ldquoNew integrability condi-tions for differential constraintsrdquo Systems and Control Lettersvol 49 no 5 pp 335ndash345 2003
[14] H J Sussmann ldquoA general theorem on local controllabilityrdquoSIAM Journal on Control and Optimization vol 25 no 1 pp158ndash194 1987
[15] F Bullo A D Lewis and K M Lynch ldquoControllable kinematicreductions for mechanical systems concepts computationaltools and examplesrdquo in Proceedings of International Symposiumon Mathematical Theory of Networks and Systems 2002
[16] F Bullo and A D Lewis ldquoLow-order controllability and kine-matic reductions for affine connection control systemsrdquo SIAMJournal on Control andOptimization vol 44 no 3 pp 885ndash9082006
[17] A D Lewis and R MMurray ldquoConfiguration controllability ofsimple mechanical control systemsrdquo SIAM Journal on Controland Optimization vol 35 no 3 pp 766ndash790 1997
[18] F Bullo and K M Lynch ldquoKinematic controllability for decou-pled trajectory planning in underactuatedmechanical systemsrdquoIEEE Transactions on Robotics and Automation vol 17 no 4 pp402ndash412 2001
[19] M R Hestenes Calculus of Variations and Optimcl ControlTheory John Wiley amp Sons 1966
[20] J Gregory ldquoA new systematic method for efficiently solvingholonomic (and nonholonomic) constraint problemsrdquo Analysisand Applications vol 8 no 1 pp 85ndash98 2010
[21] J Gregory and R S Wang ldquoDiscrete variable methods forthe m-dependent variable nonlinear extremal problem in thecalculus of variationsrdquo SIAM Journal onNumerical Analysis vol27 no 2 pp 470ndash487 1990
[22] Wolfram Research 2012[23] J J More and D J Thuente ldquoLine search algorithms with guar-
anteed sufficient decreaserdquo ACM Transactions on MathematicalSoftware vol 20 no 3 pp 286ndash307 1994
whereas in the initial value problem 119883119894(119905119865) will be free for
some 119894The initial values of control variables and multipliers
have been set to zero whereas their final values have notbeen assigned in both optimal control problems Thereforetransversality conditions are needed in both cases for thevariables119883
119894(119905119865) 119894 = 5 10 and they will be of the form
Figure 2 Sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 corresponding to the optimal solutionof problem 1 a boundary value problem for the planar 119877119877 robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad]
1205791(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads] obtained with a
discretization of [119905119868 119905119865] into 64 subintervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control and
state variables are represented in Figure 3
The initial values of control variable and of the multipliershave been set to zero whereas their final values are left freeFigure 2 shows the sequence of configurations of the robot attimes 119905 = 11989632 119896 = 0 1 32 Since the configurations ofthe sequence overlap it has been split into smaller sequencesfor a better visualization of the manipulator motion Figure 3depicts the corresponding control and state variables of theoptimal solution of this boundary value problem obtainedwith a discretization of the time interval [119905
119868 119905119865] into 64
subintervals The value of the objective functional for thissolution is 345185 [J]
712 Problem 2 Initial Value Problem An initial valueproblem has also been solved with the following initial andfinal conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) =
120587
2[rad]
1205792(119905119868) = minus
120587
2[rad] 120579
2(119905119865) = minus
120587
2[rad]
1205791(119905119868) = 0 [rads] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(55)
The initial values of control variable and of the multipliershave been set to zero whereas their final values are left freeThe only difference between these conditions and those of theboundary value problem described in Section 711 is that now1205791(119905119865) = free
Figures 4 and 5 depict the sequence of configurations the119877119877 robot manipulator and the corresponding control andstate variables of the optimal solution of this initial valueproblem respectively obtained with a discretization of thetime interval [119905
119868 119905119865] into 64 subintervals The value of the
objective functional for this solution is 56472 [J]This value islower than the value of the objective functional of the solutionof the boundary value problem described in Section 711because now is 120579
1(119905119865) = free and the control system does
not have to spend energy to stop it
72 Planar Horizontal Underactuated 119877119877 Robot ManipulatorIn this section the optimal control problem of a planarhorizontal underactuated 119877119877 robot manipulator is studiedIn this robot model the first joint is not actuated thus 119906 =
(0 1199062)119879 and (2) can be split into
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792
minus 120573 sin 1205792(2 12057911205792+ 1205792
2) = 0
(56)
(120575 + 120573 cos 1205792) 1205791+ 120575 1205792+ 120573 sin 120579
21205792
1= 1199062 (57)
As explained in [12] since gravity terms are all zero and1205791does not intervene in the system inertia matrix (56) can
be partially integrated to
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792+ 1198881= 0 (58)
Actually constraint (56) is completely integrable giving rise toan holonomic constraintThe resulting holonomic constrainttakes different forms depending on the value of 119888
1which
depends on the initial conditions Therefore two cases havebeen considered
(i) when the initial velocities 1205791(119905119868) and 120579
2(119905119868) are both
zero(ii) when the initial velocity 120579
1(119905119868) is nonzero
721 Problem 3 Initial Value Problem with Zero Initial Veloc-ities An initial value problem has been solved with the fol-lowing initial and final conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rad] 120579
2(119905119865) = 120587 [rad]
1205791(119905119868) = 0 [rads] 120579
1(119905119865) = 0 [rads]
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(59)
10 Abstract and Applied Analysis
10 20 30 40 50 60
05
1
15
2
25
(a) 1205791
10 20 30 40 50 60minus05
minus1
minus15
minus2
minus25
minus3
(b) 1205792
10 20 30 40 50 60
minus10
minus5
5
10
(c) 1205791
10 20 30 40 50 60
minus10
minus5
5
10
15
(d) 1205792
10 20 30 40 50 60
minus300
minus200
minus100
100
200
300
(e) 1199061
Figure 3 Control and state variables of the optimal solution of problem 1 a boundary value problem for the planar 119877119877 robot manipulatorwith boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 2
Figure 4 Sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 corresponding to the optimal solutionof problem 2 an initial value problem for an 119877119877 robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) =
0 [rads] 1205792(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained with a discretization of
[119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control and state variables are
represented in Figure 5
Abstract and Applied Analysis 11
10 20 30 40 50 60
05
1
15
2
25
(a) 1205791
10 20 30 40 50 60
minus25
minus2
(b) 1205792
10 20 30 40 50 60
minus4
minus2
2
4
6
8
10
(c) 1205791
minus4
minus6
minus210 20 30 40 50 60
2
4
(d) 1205792
10 20 30 40 50 60minus50
50
100
150
(e) 1199061
Figure 5 Control and state variables of the optimal solution of problem 2 an initial value problem for a119877119877 robotmanipulator with boundaryconditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = free
and 1205792(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of configurations of the
robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 4
The initial values of the control variable and of themultipliershave been set to zero whereas their final value is left free
Since there is a holonomic constraint that relates thevalues of the angles 120579
1and 120579
2 without integrating (58) we
are not able to find the value of 1205791(119905119865) consistent with 120579
1(119905119868)
Therefore no final conditions have been imposed on 1205791
From the initial conditions of the problem we obtain 1198881=
0 Equation (58) with 1198881= 0 corresponds to the homogeneous
differential constraint
119889120577 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792= 0 (60)
The differential 119889120577 is not exact However it becomes an exactdifferential if multiplied by the factor 1(120572 + 2120573 cos 120579
2) This
operation does not alter the differential equation (60) Inthis case there does exist a function 120577 whose differentialcoincides with the expression 119889120577(120572 + 2120573 cos 120579
2) Due to
the existence of this function the integral of 119889120577 between
two points depends only on these points and not on theintegration path Equation (60) rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2)
120572 + 2120573 cos 1205792
1198891205792
(61)
can be integrated by separating variables The correspondingholonomic constraint has the following expression
1205791= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1205792
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1205792
2+ 1198882
(62)
To express this optimal control problem in the form ofa basic optimal control problem we first have to convert (57)
12 Abstract and Applied Analysis
into a first-order differential model introducing the followingchange of variables
1199091= 1205791 119909
3= 1205791
1199092= 1205792 119909
4= 1205792
(63)
with the following additional relations
1199091015840
1= 1199093
1199091015840
2= 1199094
(64)
Thus the optimal control problem is to minimize
int
1
0
1199062
2119889119905 (65)
subject to the constraints1199091
= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1199092
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1199092
2+ 1198882
(66)
(120575 + 120573 cos 1205792) 1199091015840
3+ 1205751199091015840
4+ 120573 sin119909
21199092
3= 1199062 (67)
and the additional constraints (64) To reformulate thisoptimal control problem as an unconstrained calculus ofvariations problem let X be
X = [
[
1198831
sdot sdot sdot
1198839
]
]
(68)
such that119883119894= 119909119894 119894 = 1 4
1198831015840
5= 1199061 119883
5(119905119868) = 0
(69)
where 11988310158406with 119883
6(119905119868) = 0 is the multiplier associated with
the holonomic constraint (66) 11988310158407with 119883
7(119905119868) = 0 is the
multipliers associated with the differential constraint (67)and 1198831015840
8with 119883
8(119905119868) = 0 and 1198831015840
9with 119883
9(119905119868) = 0 are the
multipliers associatedwith the additional equality constraints(64)
Thus the holonomic constraint of the problem can berewritten as follows
120593 (119905X)
= 1198831minus((120572 minus 2120575)
times tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1198832
2)]]
]
)
times (radic41205732 minus 1205722)
minus1
+1198832
2= 0
(70)
Now the technique described in Section 51 to deal withholonomic constraints can be applied to 120593(119905X) and thisholonomic constraint is replaced by
120593119905+ 1205931198831198831015840= 0
120593 (0 119883 (0)) = 0
(71)
From the initial conditions of the problem the latter equationreduces to the equality 0 = 0 whereas the former takes thefollowing form
(120572 + 2120573 cos (1198832))1198833+ (120575 + 120573 cos (119883
2))1198834= 0 (72)
The corresponding Hamiltonian is
1198661= 11988310158402
5+ 1198831015840
6((120572 + 2120573 cos (119883
2))1198833
+ (120575 + 120573 cos (1198832))1198834)
+1198831015840
7(120573 sin (119883
2)1198832
3+ (120575 + 120573 cos (119883
2))1198831015840
3
+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(73)
It is not difficult to check that matrix 1198661X1015840X1015840
is singularThis is due to the fact that to handle our optimal controlproblemwhich involves second-order differential constraintswe converted them into first-order differential constraintsTherefore we apply again the technique of Section 51obtaining the identity 0 = 0 and the following constraint
minus 120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4= 0
(74)
The corresponding Hamiltonian is
1198662= 11988310158402
5+ 1198831015840
6
times (minus120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4)
+ 1198831015840
7(120573 sin (119883
2)1198832
3+(120575+120573 cos (119883
2))1198831015840
3+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(75)
It is not difficult to check that matrix 1198662X1015840X1015840
in this case is notsingular since its determinant is
Substituting the values of120572120573 and 120575 this expression becomesdet(119866
2X1015840X1015840) = 1128(43 minus 2 cos(2119883
2))2 which is always
positive for any real value1198832 Figure 6 shows the sequence of
configurations of the robot at times 11989632with 119896 = 0 1 32and Figure 7 depicts control and state variables of the optimal
Abstract and Applied Analysis 13
Figure 6 Sequence of configurations of the robot manipulator attimes 11989632 with 119896 = 0 1 32 corresponding to the optimal sol-ution of problem 3 an initial value problem for an underactuated119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) =
0 [rad] 1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads]
1205791(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2
(119905119865) = 0 [rads] obtained with a discretization of [119905
119868 119905119865] into 64
subintervals The initial and final times are 119905119868= 0 and 119905
119865=
1 [s] respectively The corresponding control and state variables arerepresented in Figure 7
solution obtained with a discretization of the interval [119905119868 119905119865]
into 64 subintervalsIn particular we get 120579
1(119905119865) = minus110248 [rad] To check the
consistency of this result with the holonomic constraint (62)since 120579
1(119905119868) = 1205792(119905119868) = 0 [rads] we get from (58) that 119888
1= 0
and using the initial condition 1205791(119905119868) = 1205792(119905119868) = 0 [rad] we
get from (62) that 1198882= 0 Having established the value of the
constant 1198882 we obtain from the same equation for 120579
2(119905119865) =
120587 [rad] that 1205791(119905119865) = minus110248 [rad] which coincides with
the value of 1205791(119905119865) obtained numerically
722 Problem 4 Initial Value Problem with Nonzero InitialVelocity 120579
1 Another initial value problem has been solved
with the following conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rad] 120579
2(119905119865) = 120587 [rad]
1205791(119905119868) = 5 [rads] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(77)
The initial values of the multipliers have been set to zerowhereas their final value is left free Notice that no finalconditions have been imposed on 120579
1and 120579
1 The same
considerations done in previous section hold in this caseas well The technique described in Section 51 must beapplied twice leading to the differential constraint (74) andto the Hamiltonian (75) Figure 8 shows the sequence ofconfigurations of the robot at times 11989632with 119896 = 0 1 32and Figure 9 depicts the control and state variables ofthe optimal solution obtained with a discretization of theinterval [119905
119868 119905119865] into 64 subintervals In particular we get that
1205791(119905119865) = 617172 [rad] and 120579
1(119905119865) = 900163 [rads] To
check the consistency of the obtained value of 1205791(119905119865) with
the holonomic constraint consider (58) We can calculate
the constant 1198881using the initial conditions of the problem
obtaining
1198881= minus 120579
1(119905119868) (120572 + 2120573 cos 120579
2(119905119868))
minus 1205792(119905119868) (120575 + 120573 cos 120579
2(119905119868)) = minus225
(78)
Since 1198881= 0 (58) corresponds in this case to the differential
constraint
119889120578 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792+ 1198881119889119905 = 0
(79)
Equation (79) can be rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2 (119905))
120572 + 2120573 cos 1205792 (119905)
1198891205792minus
1198881
120572 + 2120573 cos 1205792 (119905)
119889119905 (80)
To check the obtained value of 1205791(119905119865) 1198891205791is numerically
integrated between 1205791(119905119868) and 120579
1(119905119865) using the interpolated
numerical optimal solution obtained for 1205792(119905) We get that
1205791(119905119865) = 618705 This value is close to 617172
To check the consistency of the obtained value of 1205791(119905119865)
with the constraint (58) using the computed value 1198881= minus225
and the final conditions 1205792(119905119865) = 0 120579
2(119905119865) = 120587 of the problem
we obtain
1205791(119905119865) =
minus (120575 + 120573 cos 1205792(119905119865))
120572 + 2120573 cos 1205792(119905119865)
1205792(119905119865)
minus1198881
120572 + 2120573 cos 1205792(119905119865)= 9
(81)
This value is very close to the value of 1205791(119905119865) obtained
numerically
73 Computational Issues If the optimal control problem has119898 variables and the time interval [119905
119868 119905119865] has been discretized
into 119873 subintervals the resulting set of difference equations(38) has119898times(119873minus1) equations and119898times(119873minus1) variables plusthe equations and variables due to transversality conditionsFeasible solutions have been used as initial guesses of thealgorithm
The solution of the nonlinear system of difference equa-tions (38) has been obtained using a damped Newtonalgorithm within a line search methodology implementedin Mathematica 7 under Mac OS X operating system (see[22 23] for more details)
8 Conclusion
In this paper the trajectory planning problem for planarunderactuated robot manipulators with two revolute jointswithout gravity has been studied This problem is solved asan optimal control problem based on a numerical resolutionof an unconstrained variational calculus reformulation of theoptimal control problem in which the dynamic equation ofthe mechanical system is regarded as a constraint It hasbeen shown that this reformulation method based on special
14 Abstract and Applied Analysis
10 20 30 40 50 60
minus1
minus08
minus06
minus04
minus02
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
minus15
minus1
minus05
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
(d) 1205792
10 20 30 40 50 60
minus15
minus10
5
10
15
minus5
(e) 1199062
Figure 7 Control and state variables of the optimal solution of problem 3 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 119906
2(119905119868) = 0Nm 120579
1(119905119865) = free
1205792(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads]The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectivelyThe corresponding
sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 6
Figure 8 Sequence of configurations of the robot manipulator at times 11989632 with 119896 = 0 1 32 corresponding to the optimal solutionof problem 4 an initial value problem for an underactuated 119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad]
1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained
with a discretization of [119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control
and state variables are represented in Figure 9
Abstract and Applied Analysis 15
10 20 30 40 50 60
1
2
3
4
5
6
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
4
5
6
7
8
9
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
6
(d) 1205792
10 20 30 40 50 60minus5
5
10
15
20
(e) 1199062
Figure 9 Control and state variables of the optimal solution of problem 4 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 8
derivative multipliers is able to tackle both integrable andnonintegrable differential constraints of the dynamic modelsof underactuated planar horizontal robot manipulators withtwo revolute joints This method can be seamlessly appliedin the presence of additional constraints on the mechanicalsystem
References
[1] A De Luca S Iannitti R Mattone and G Oriolo ldquoUnderactu-ated manipulators control properties and techniquesrdquoMachineIntelligence and Robotic Control vol 4 no 3 pp 113ndash125 2002
[2] G A Bliss Lectures on the Calculus of Variations University ofChicago Press Chicago Ill USA 1946
[3] J Gregory and C Lin Constrained Optimization in the Calculusof Variations and Optimal Control theory Chapman amp Hall1996
[4] W-S Koon and J E Marsden ldquoOptimal control for holonomicand nonholonomic mechanical systems with symmetry andLagrangian reductionrdquo SIAM Journal on Control and Optimiza-tion vol 35 no 3 pp 901ndash929 1997
[5] A M Bloch Nonholonomic Mechanics and Control SpringerNew York NY USA 2003
[6] I I Hussein and A M Bloch ldquoOptimal control of underactu-ated nonholonomic mechanical systemsrdquo IEEE Transactions onAutomatic Control vol 53 no 3 pp 668ndash682 2008
[7] X Z Lai J H She S X Yang andMWu ldquoComprehensive uni-fied control strategy for underactuated two-link manipulatorsrdquoIEEE Transactions on Systems Man and Cybernetics B vol 39no 2 pp 389ndash398 2009
[8] J P Ordaz-Oliver O J Santos-Sanchez and V Lopez-MoralesldquoToward a generalized sub-optimal control method of underac-tuated systemsrdquo Optimal Control Applications amp Methods vol33 no 3 pp 338ndash351 2012
16 Abstract and Applied Analysis
[9] R Seifried ldquoTwo approaches for feedforward control andoptimal design of underactuatedmultibody systemsrdquoMultibodySystem Dynamics vol 27 no 1 pp 75ndash93 2012
[10] M Buss O von Stryk R Bulirsch and G Schmidt ldquoTowardshybrid optimal controlrdquo atmdashAutomatisierungstechnik vol 48no 9 pp 448ndash459 2000
[11] M Buss M Glocker M Hardt O von Stryk R Bulirsch andG Schmidt ldquoNonlinear hybrid dynamical systems modelingoptimal control and applicationsrdquo in Modelling Analysis andDesign of Hybrid Systems S Engell G Frehse and E SchniederEds vol 279 of Lecture Notes in Control and InformationScience pp 331ndash335 Springer 2002
[12] G Oriolo and Y Nakamura ldquoControl of mechanical systemswith second-order nonholonomic constraints underactuatedmanipulatorsrdquo in Proceedings of the 30th IEEE Conference onDecision and Control pp 2398ndash2403 December 1991
[13] T J Tarn M Zhang and A Serrani ldquoNew integrability condi-tions for differential constraintsrdquo Systems and Control Lettersvol 49 no 5 pp 335ndash345 2003
[14] H J Sussmann ldquoA general theorem on local controllabilityrdquoSIAM Journal on Control and Optimization vol 25 no 1 pp158ndash194 1987
[15] F Bullo A D Lewis and K M Lynch ldquoControllable kinematicreductions for mechanical systems concepts computationaltools and examplesrdquo in Proceedings of International Symposiumon Mathematical Theory of Networks and Systems 2002
[16] F Bullo and A D Lewis ldquoLow-order controllability and kine-matic reductions for affine connection control systemsrdquo SIAMJournal on Control andOptimization vol 44 no 3 pp 885ndash9082006
[17] A D Lewis and R MMurray ldquoConfiguration controllability ofsimple mechanical control systemsrdquo SIAM Journal on Controland Optimization vol 35 no 3 pp 766ndash790 1997
[18] F Bullo and K M Lynch ldquoKinematic controllability for decou-pled trajectory planning in underactuatedmechanical systemsrdquoIEEE Transactions on Robotics and Automation vol 17 no 4 pp402ndash412 2001
[19] M R Hestenes Calculus of Variations and Optimcl ControlTheory John Wiley amp Sons 1966
[20] J Gregory ldquoA new systematic method for efficiently solvingholonomic (and nonholonomic) constraint problemsrdquo Analysisand Applications vol 8 no 1 pp 85ndash98 2010
[21] J Gregory and R S Wang ldquoDiscrete variable methods forthe m-dependent variable nonlinear extremal problem in thecalculus of variationsrdquo SIAM Journal onNumerical Analysis vol27 no 2 pp 470ndash487 1990
[22] Wolfram Research 2012[23] J J More and D J Thuente ldquoLine search algorithms with guar-
anteed sufficient decreaserdquo ACM Transactions on MathematicalSoftware vol 20 no 3 pp 286ndash307 1994
Figure 2 Sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 corresponding to the optimal solutionof problem 1 a boundary value problem for the planar 119877119877 robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad]
1205791(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads] obtained with a
discretization of [119905119868 119905119865] into 64 subintervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control and
state variables are represented in Figure 3
The initial values of control variable and of the multipliershave been set to zero whereas their final values are left freeFigure 2 shows the sequence of configurations of the robot attimes 119905 = 11989632 119896 = 0 1 32 Since the configurations ofthe sequence overlap it has been split into smaller sequencesfor a better visualization of the manipulator motion Figure 3depicts the corresponding control and state variables of theoptimal solution of this boundary value problem obtainedwith a discretization of the time interval [119905
119868 119905119865] into 64
subintervals The value of the objective functional for thissolution is 345185 [J]
712 Problem 2 Initial Value Problem An initial valueproblem has also been solved with the following initial andfinal conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) =
120587
2[rad]
1205792(119905119868) = minus
120587
2[rad] 120579
2(119905119865) = minus
120587
2[rad]
1205791(119905119868) = 0 [rads] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(55)
The initial values of control variable and of the multipliershave been set to zero whereas their final values are left freeThe only difference between these conditions and those of theboundary value problem described in Section 711 is that now1205791(119905119865) = free
Figures 4 and 5 depict the sequence of configurations the119877119877 robot manipulator and the corresponding control andstate variables of the optimal solution of this initial valueproblem respectively obtained with a discretization of thetime interval [119905
119868 119905119865] into 64 subintervals The value of the
objective functional for this solution is 56472 [J]This value islower than the value of the objective functional of the solutionof the boundary value problem described in Section 711because now is 120579
1(119905119865) = free and the control system does
not have to spend energy to stop it
72 Planar Horizontal Underactuated 119877119877 Robot ManipulatorIn this section the optimal control problem of a planarhorizontal underactuated 119877119877 robot manipulator is studiedIn this robot model the first joint is not actuated thus 119906 =
(0 1199062)119879 and (2) can be split into
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792
minus 120573 sin 1205792(2 12057911205792+ 1205792
2) = 0
(56)
(120575 + 120573 cos 1205792) 1205791+ 120575 1205792+ 120573 sin 120579
21205792
1= 1199062 (57)
As explained in [12] since gravity terms are all zero and1205791does not intervene in the system inertia matrix (56) can
be partially integrated to
(120572 + 2120573 cos 1205792) 1205791+ (120575 + 120573 cos 120579
2) 1205792+ 1198881= 0 (58)
Actually constraint (56) is completely integrable giving rise toan holonomic constraintThe resulting holonomic constrainttakes different forms depending on the value of 119888
1which
depends on the initial conditions Therefore two cases havebeen considered
(i) when the initial velocities 1205791(119905119868) and 120579
2(119905119868) are both
zero(ii) when the initial velocity 120579
1(119905119868) is nonzero
721 Problem 3 Initial Value Problem with Zero Initial Veloc-ities An initial value problem has been solved with the fol-lowing initial and final conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rad] 120579
2(119905119865) = 120587 [rad]
1205791(119905119868) = 0 [rads] 120579
1(119905119865) = 0 [rads]
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(59)
10 Abstract and Applied Analysis
10 20 30 40 50 60
05
1
15
2
25
(a) 1205791
10 20 30 40 50 60minus05
minus1
minus15
minus2
minus25
minus3
(b) 1205792
10 20 30 40 50 60
minus10
minus5
5
10
(c) 1205791
10 20 30 40 50 60
minus10
minus5
5
10
15
(d) 1205792
10 20 30 40 50 60
minus300
minus200
minus100
100
200
300
(e) 1199061
Figure 3 Control and state variables of the optimal solution of problem 1 a boundary value problem for the planar 119877119877 robot manipulatorwith boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 2
Figure 4 Sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 corresponding to the optimal solutionof problem 2 an initial value problem for an 119877119877 robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) =
0 [rads] 1205792(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained with a discretization of
[119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control and state variables are
represented in Figure 5
Abstract and Applied Analysis 11
10 20 30 40 50 60
05
1
15
2
25
(a) 1205791
10 20 30 40 50 60
minus25
minus2
(b) 1205792
10 20 30 40 50 60
minus4
minus2
2
4
6
8
10
(c) 1205791
minus4
minus6
minus210 20 30 40 50 60
2
4
(d) 1205792
10 20 30 40 50 60minus50
50
100
150
(e) 1199061
Figure 5 Control and state variables of the optimal solution of problem 2 an initial value problem for a119877119877 robotmanipulator with boundaryconditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = free
and 1205792(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of configurations of the
robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 4
The initial values of the control variable and of themultipliershave been set to zero whereas their final value is left free
Since there is a holonomic constraint that relates thevalues of the angles 120579
1and 120579
2 without integrating (58) we
are not able to find the value of 1205791(119905119865) consistent with 120579
1(119905119868)
Therefore no final conditions have been imposed on 1205791
From the initial conditions of the problem we obtain 1198881=
0 Equation (58) with 1198881= 0 corresponds to the homogeneous
differential constraint
119889120577 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792= 0 (60)
The differential 119889120577 is not exact However it becomes an exactdifferential if multiplied by the factor 1(120572 + 2120573 cos 120579
2) This
operation does not alter the differential equation (60) Inthis case there does exist a function 120577 whose differentialcoincides with the expression 119889120577(120572 + 2120573 cos 120579
2) Due to
the existence of this function the integral of 119889120577 between
two points depends only on these points and not on theintegration path Equation (60) rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2)
120572 + 2120573 cos 1205792
1198891205792
(61)
can be integrated by separating variables The correspondingholonomic constraint has the following expression
1205791= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1205792
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1205792
2+ 1198882
(62)
To express this optimal control problem in the form ofa basic optimal control problem we first have to convert (57)
12 Abstract and Applied Analysis
into a first-order differential model introducing the followingchange of variables
1199091= 1205791 119909
3= 1205791
1199092= 1205792 119909
4= 1205792
(63)
with the following additional relations
1199091015840
1= 1199093
1199091015840
2= 1199094
(64)
Thus the optimal control problem is to minimize
int
1
0
1199062
2119889119905 (65)
subject to the constraints1199091
= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1199092
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1199092
2+ 1198882
(66)
(120575 + 120573 cos 1205792) 1199091015840
3+ 1205751199091015840
4+ 120573 sin119909
21199092
3= 1199062 (67)
and the additional constraints (64) To reformulate thisoptimal control problem as an unconstrained calculus ofvariations problem let X be
X = [
[
1198831
sdot sdot sdot
1198839
]
]
(68)
such that119883119894= 119909119894 119894 = 1 4
1198831015840
5= 1199061 119883
5(119905119868) = 0
(69)
where 11988310158406with 119883
6(119905119868) = 0 is the multiplier associated with
the holonomic constraint (66) 11988310158407with 119883
7(119905119868) = 0 is the
multipliers associated with the differential constraint (67)and 1198831015840
8with 119883
8(119905119868) = 0 and 1198831015840
9with 119883
9(119905119868) = 0 are the
multipliers associatedwith the additional equality constraints(64)
Thus the holonomic constraint of the problem can berewritten as follows
120593 (119905X)
= 1198831minus((120572 minus 2120575)
times tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1198832
2)]]
]
)
times (radic41205732 minus 1205722)
minus1
+1198832
2= 0
(70)
Now the technique described in Section 51 to deal withholonomic constraints can be applied to 120593(119905X) and thisholonomic constraint is replaced by
120593119905+ 1205931198831198831015840= 0
120593 (0 119883 (0)) = 0
(71)
From the initial conditions of the problem the latter equationreduces to the equality 0 = 0 whereas the former takes thefollowing form
(120572 + 2120573 cos (1198832))1198833+ (120575 + 120573 cos (119883
2))1198834= 0 (72)
The corresponding Hamiltonian is
1198661= 11988310158402
5+ 1198831015840
6((120572 + 2120573 cos (119883
2))1198833
+ (120575 + 120573 cos (1198832))1198834)
+1198831015840
7(120573 sin (119883
2)1198832
3+ (120575 + 120573 cos (119883
2))1198831015840
3
+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(73)
It is not difficult to check that matrix 1198661X1015840X1015840
is singularThis is due to the fact that to handle our optimal controlproblemwhich involves second-order differential constraintswe converted them into first-order differential constraintsTherefore we apply again the technique of Section 51obtaining the identity 0 = 0 and the following constraint
minus 120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4= 0
(74)
The corresponding Hamiltonian is
1198662= 11988310158402
5+ 1198831015840
6
times (minus120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4)
+ 1198831015840
7(120573 sin (119883
2)1198832
3+(120575+120573 cos (119883
2))1198831015840
3+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(75)
It is not difficult to check that matrix 1198662X1015840X1015840
in this case is notsingular since its determinant is
Substituting the values of120572120573 and 120575 this expression becomesdet(119866
2X1015840X1015840) = 1128(43 minus 2 cos(2119883
2))2 which is always
positive for any real value1198832 Figure 6 shows the sequence of
configurations of the robot at times 11989632with 119896 = 0 1 32and Figure 7 depicts control and state variables of the optimal
Abstract and Applied Analysis 13
Figure 6 Sequence of configurations of the robot manipulator attimes 11989632 with 119896 = 0 1 32 corresponding to the optimal sol-ution of problem 3 an initial value problem for an underactuated119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) =
0 [rad] 1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads]
1205791(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2
(119905119865) = 0 [rads] obtained with a discretization of [119905
119868 119905119865] into 64
subintervals The initial and final times are 119905119868= 0 and 119905
119865=
1 [s] respectively The corresponding control and state variables arerepresented in Figure 7
solution obtained with a discretization of the interval [119905119868 119905119865]
into 64 subintervalsIn particular we get 120579
1(119905119865) = minus110248 [rad] To check the
consistency of this result with the holonomic constraint (62)since 120579
1(119905119868) = 1205792(119905119868) = 0 [rads] we get from (58) that 119888
1= 0
and using the initial condition 1205791(119905119868) = 1205792(119905119868) = 0 [rad] we
get from (62) that 1198882= 0 Having established the value of the
constant 1198882 we obtain from the same equation for 120579
2(119905119865) =
120587 [rad] that 1205791(119905119865) = minus110248 [rad] which coincides with
the value of 1205791(119905119865) obtained numerically
722 Problem 4 Initial Value Problem with Nonzero InitialVelocity 120579
1 Another initial value problem has been solved
with the following conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rad] 120579
2(119905119865) = 120587 [rad]
1205791(119905119868) = 5 [rads] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(77)
The initial values of the multipliers have been set to zerowhereas their final value is left free Notice that no finalconditions have been imposed on 120579
1and 120579
1 The same
considerations done in previous section hold in this caseas well The technique described in Section 51 must beapplied twice leading to the differential constraint (74) andto the Hamiltonian (75) Figure 8 shows the sequence ofconfigurations of the robot at times 11989632with 119896 = 0 1 32and Figure 9 depicts the control and state variables ofthe optimal solution obtained with a discretization of theinterval [119905
119868 119905119865] into 64 subintervals In particular we get that
1205791(119905119865) = 617172 [rad] and 120579
1(119905119865) = 900163 [rads] To
check the consistency of the obtained value of 1205791(119905119865) with
the holonomic constraint consider (58) We can calculate
the constant 1198881using the initial conditions of the problem
obtaining
1198881= minus 120579
1(119905119868) (120572 + 2120573 cos 120579
2(119905119868))
minus 1205792(119905119868) (120575 + 120573 cos 120579
2(119905119868)) = minus225
(78)
Since 1198881= 0 (58) corresponds in this case to the differential
constraint
119889120578 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792+ 1198881119889119905 = 0
(79)
Equation (79) can be rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2 (119905))
120572 + 2120573 cos 1205792 (119905)
1198891205792minus
1198881
120572 + 2120573 cos 1205792 (119905)
119889119905 (80)
To check the obtained value of 1205791(119905119865) 1198891205791is numerically
integrated between 1205791(119905119868) and 120579
1(119905119865) using the interpolated
numerical optimal solution obtained for 1205792(119905) We get that
1205791(119905119865) = 618705 This value is close to 617172
To check the consistency of the obtained value of 1205791(119905119865)
with the constraint (58) using the computed value 1198881= minus225
and the final conditions 1205792(119905119865) = 0 120579
2(119905119865) = 120587 of the problem
we obtain
1205791(119905119865) =
minus (120575 + 120573 cos 1205792(119905119865))
120572 + 2120573 cos 1205792(119905119865)
1205792(119905119865)
minus1198881
120572 + 2120573 cos 1205792(119905119865)= 9
(81)
This value is very close to the value of 1205791(119905119865) obtained
numerically
73 Computational Issues If the optimal control problem has119898 variables and the time interval [119905
119868 119905119865] has been discretized
into 119873 subintervals the resulting set of difference equations(38) has119898times(119873minus1) equations and119898times(119873minus1) variables plusthe equations and variables due to transversality conditionsFeasible solutions have been used as initial guesses of thealgorithm
The solution of the nonlinear system of difference equa-tions (38) has been obtained using a damped Newtonalgorithm within a line search methodology implementedin Mathematica 7 under Mac OS X operating system (see[22 23] for more details)
8 Conclusion
In this paper the trajectory planning problem for planarunderactuated robot manipulators with two revolute jointswithout gravity has been studied This problem is solved asan optimal control problem based on a numerical resolutionof an unconstrained variational calculus reformulation of theoptimal control problem in which the dynamic equation ofthe mechanical system is regarded as a constraint It hasbeen shown that this reformulation method based on special
14 Abstract and Applied Analysis
10 20 30 40 50 60
minus1
minus08
minus06
minus04
minus02
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
minus15
minus1
minus05
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
(d) 1205792
10 20 30 40 50 60
minus15
minus10
5
10
15
minus5
(e) 1199062
Figure 7 Control and state variables of the optimal solution of problem 3 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 119906
2(119905119868) = 0Nm 120579
1(119905119865) = free
1205792(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads]The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectivelyThe corresponding
sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 6
Figure 8 Sequence of configurations of the robot manipulator at times 11989632 with 119896 = 0 1 32 corresponding to the optimal solutionof problem 4 an initial value problem for an underactuated 119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad]
1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained
with a discretization of [119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control
and state variables are represented in Figure 9
Abstract and Applied Analysis 15
10 20 30 40 50 60
1
2
3
4
5
6
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
4
5
6
7
8
9
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
6
(d) 1205792
10 20 30 40 50 60minus5
5
10
15
20
(e) 1199062
Figure 9 Control and state variables of the optimal solution of problem 4 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 8
derivative multipliers is able to tackle both integrable andnonintegrable differential constraints of the dynamic modelsof underactuated planar horizontal robot manipulators withtwo revolute joints This method can be seamlessly appliedin the presence of additional constraints on the mechanicalsystem
References
[1] A De Luca S Iannitti R Mattone and G Oriolo ldquoUnderactu-ated manipulators control properties and techniquesrdquoMachineIntelligence and Robotic Control vol 4 no 3 pp 113ndash125 2002
[2] G A Bliss Lectures on the Calculus of Variations University ofChicago Press Chicago Ill USA 1946
[3] J Gregory and C Lin Constrained Optimization in the Calculusof Variations and Optimal Control theory Chapman amp Hall1996
[4] W-S Koon and J E Marsden ldquoOptimal control for holonomicand nonholonomic mechanical systems with symmetry andLagrangian reductionrdquo SIAM Journal on Control and Optimiza-tion vol 35 no 3 pp 901ndash929 1997
[5] A M Bloch Nonholonomic Mechanics and Control SpringerNew York NY USA 2003
[6] I I Hussein and A M Bloch ldquoOptimal control of underactu-ated nonholonomic mechanical systemsrdquo IEEE Transactions onAutomatic Control vol 53 no 3 pp 668ndash682 2008
[7] X Z Lai J H She S X Yang andMWu ldquoComprehensive uni-fied control strategy for underactuated two-link manipulatorsrdquoIEEE Transactions on Systems Man and Cybernetics B vol 39no 2 pp 389ndash398 2009
[8] J P Ordaz-Oliver O J Santos-Sanchez and V Lopez-MoralesldquoToward a generalized sub-optimal control method of underac-tuated systemsrdquo Optimal Control Applications amp Methods vol33 no 3 pp 338ndash351 2012
16 Abstract and Applied Analysis
[9] R Seifried ldquoTwo approaches for feedforward control andoptimal design of underactuatedmultibody systemsrdquoMultibodySystem Dynamics vol 27 no 1 pp 75ndash93 2012
[10] M Buss O von Stryk R Bulirsch and G Schmidt ldquoTowardshybrid optimal controlrdquo atmdashAutomatisierungstechnik vol 48no 9 pp 448ndash459 2000
[11] M Buss M Glocker M Hardt O von Stryk R Bulirsch andG Schmidt ldquoNonlinear hybrid dynamical systems modelingoptimal control and applicationsrdquo in Modelling Analysis andDesign of Hybrid Systems S Engell G Frehse and E SchniederEds vol 279 of Lecture Notes in Control and InformationScience pp 331ndash335 Springer 2002
[12] G Oriolo and Y Nakamura ldquoControl of mechanical systemswith second-order nonholonomic constraints underactuatedmanipulatorsrdquo in Proceedings of the 30th IEEE Conference onDecision and Control pp 2398ndash2403 December 1991
[13] T J Tarn M Zhang and A Serrani ldquoNew integrability condi-tions for differential constraintsrdquo Systems and Control Lettersvol 49 no 5 pp 335ndash345 2003
[14] H J Sussmann ldquoA general theorem on local controllabilityrdquoSIAM Journal on Control and Optimization vol 25 no 1 pp158ndash194 1987
[15] F Bullo A D Lewis and K M Lynch ldquoControllable kinematicreductions for mechanical systems concepts computationaltools and examplesrdquo in Proceedings of International Symposiumon Mathematical Theory of Networks and Systems 2002
[16] F Bullo and A D Lewis ldquoLow-order controllability and kine-matic reductions for affine connection control systemsrdquo SIAMJournal on Control andOptimization vol 44 no 3 pp 885ndash9082006
[17] A D Lewis and R MMurray ldquoConfiguration controllability ofsimple mechanical control systemsrdquo SIAM Journal on Controland Optimization vol 35 no 3 pp 766ndash790 1997
[18] F Bullo and K M Lynch ldquoKinematic controllability for decou-pled trajectory planning in underactuatedmechanical systemsrdquoIEEE Transactions on Robotics and Automation vol 17 no 4 pp402ndash412 2001
[19] M R Hestenes Calculus of Variations and Optimcl ControlTheory John Wiley amp Sons 1966
[20] J Gregory ldquoA new systematic method for efficiently solvingholonomic (and nonholonomic) constraint problemsrdquo Analysisand Applications vol 8 no 1 pp 85ndash98 2010
[21] J Gregory and R S Wang ldquoDiscrete variable methods forthe m-dependent variable nonlinear extremal problem in thecalculus of variationsrdquo SIAM Journal onNumerical Analysis vol27 no 2 pp 470ndash487 1990
[22] Wolfram Research 2012[23] J J More and D J Thuente ldquoLine search algorithms with guar-
anteed sufficient decreaserdquo ACM Transactions on MathematicalSoftware vol 20 no 3 pp 286ndash307 1994
Figure 3 Control and state variables of the optimal solution of problem 1 a boundary value problem for the planar 119877119877 robot manipulatorwith boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 2
Figure 4 Sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 corresponding to the optimal solutionof problem 2 an initial value problem for an 119877119877 robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) =
0 [rads] 1205792(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained with a discretization of
[119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control and state variables are
represented in Figure 5
Abstract and Applied Analysis 11
10 20 30 40 50 60
05
1
15
2
25
(a) 1205791
10 20 30 40 50 60
minus25
minus2
(b) 1205792
10 20 30 40 50 60
minus4
minus2
2
4
6
8
10
(c) 1205791
minus4
minus6
minus210 20 30 40 50 60
2
4
(d) 1205792
10 20 30 40 50 60minus50
50
100
150
(e) 1199061
Figure 5 Control and state variables of the optimal solution of problem 2 an initial value problem for a119877119877 robotmanipulator with boundaryconditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = free
and 1205792(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of configurations of the
robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 4
The initial values of the control variable and of themultipliershave been set to zero whereas their final value is left free
Since there is a holonomic constraint that relates thevalues of the angles 120579
1and 120579
2 without integrating (58) we
are not able to find the value of 1205791(119905119865) consistent with 120579
1(119905119868)
Therefore no final conditions have been imposed on 1205791
From the initial conditions of the problem we obtain 1198881=
0 Equation (58) with 1198881= 0 corresponds to the homogeneous
differential constraint
119889120577 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792= 0 (60)
The differential 119889120577 is not exact However it becomes an exactdifferential if multiplied by the factor 1(120572 + 2120573 cos 120579
2) This
operation does not alter the differential equation (60) Inthis case there does exist a function 120577 whose differentialcoincides with the expression 119889120577(120572 + 2120573 cos 120579
2) Due to
the existence of this function the integral of 119889120577 between
two points depends only on these points and not on theintegration path Equation (60) rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2)
120572 + 2120573 cos 1205792
1198891205792
(61)
can be integrated by separating variables The correspondingholonomic constraint has the following expression
1205791= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1205792
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1205792
2+ 1198882
(62)
To express this optimal control problem in the form ofa basic optimal control problem we first have to convert (57)
12 Abstract and Applied Analysis
into a first-order differential model introducing the followingchange of variables
1199091= 1205791 119909
3= 1205791
1199092= 1205792 119909
4= 1205792
(63)
with the following additional relations
1199091015840
1= 1199093
1199091015840
2= 1199094
(64)
Thus the optimal control problem is to minimize
int
1
0
1199062
2119889119905 (65)
subject to the constraints1199091
= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1199092
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1199092
2+ 1198882
(66)
(120575 + 120573 cos 1205792) 1199091015840
3+ 1205751199091015840
4+ 120573 sin119909
21199092
3= 1199062 (67)
and the additional constraints (64) To reformulate thisoptimal control problem as an unconstrained calculus ofvariations problem let X be
X = [
[
1198831
sdot sdot sdot
1198839
]
]
(68)
such that119883119894= 119909119894 119894 = 1 4
1198831015840
5= 1199061 119883
5(119905119868) = 0
(69)
where 11988310158406with 119883
6(119905119868) = 0 is the multiplier associated with
the holonomic constraint (66) 11988310158407with 119883
7(119905119868) = 0 is the
multipliers associated with the differential constraint (67)and 1198831015840
8with 119883
8(119905119868) = 0 and 1198831015840
9with 119883
9(119905119868) = 0 are the
multipliers associatedwith the additional equality constraints(64)
Thus the holonomic constraint of the problem can berewritten as follows
120593 (119905X)
= 1198831minus((120572 minus 2120575)
times tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1198832
2)]]
]
)
times (radic41205732 minus 1205722)
minus1
+1198832
2= 0
(70)
Now the technique described in Section 51 to deal withholonomic constraints can be applied to 120593(119905X) and thisholonomic constraint is replaced by
120593119905+ 1205931198831198831015840= 0
120593 (0 119883 (0)) = 0
(71)
From the initial conditions of the problem the latter equationreduces to the equality 0 = 0 whereas the former takes thefollowing form
(120572 + 2120573 cos (1198832))1198833+ (120575 + 120573 cos (119883
2))1198834= 0 (72)
The corresponding Hamiltonian is
1198661= 11988310158402
5+ 1198831015840
6((120572 + 2120573 cos (119883
2))1198833
+ (120575 + 120573 cos (1198832))1198834)
+1198831015840
7(120573 sin (119883
2)1198832
3+ (120575 + 120573 cos (119883
2))1198831015840
3
+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(73)
It is not difficult to check that matrix 1198661X1015840X1015840
is singularThis is due to the fact that to handle our optimal controlproblemwhich involves second-order differential constraintswe converted them into first-order differential constraintsTherefore we apply again the technique of Section 51obtaining the identity 0 = 0 and the following constraint
minus 120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4= 0
(74)
The corresponding Hamiltonian is
1198662= 11988310158402
5+ 1198831015840
6
times (minus120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4)
+ 1198831015840
7(120573 sin (119883
2)1198832
3+(120575+120573 cos (119883
2))1198831015840
3+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(75)
It is not difficult to check that matrix 1198662X1015840X1015840
in this case is notsingular since its determinant is
Substituting the values of120572120573 and 120575 this expression becomesdet(119866
2X1015840X1015840) = 1128(43 minus 2 cos(2119883
2))2 which is always
positive for any real value1198832 Figure 6 shows the sequence of
configurations of the robot at times 11989632with 119896 = 0 1 32and Figure 7 depicts control and state variables of the optimal
Abstract and Applied Analysis 13
Figure 6 Sequence of configurations of the robot manipulator attimes 11989632 with 119896 = 0 1 32 corresponding to the optimal sol-ution of problem 3 an initial value problem for an underactuated119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) =
0 [rad] 1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads]
1205791(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2
(119905119865) = 0 [rads] obtained with a discretization of [119905
119868 119905119865] into 64
subintervals The initial and final times are 119905119868= 0 and 119905
119865=
1 [s] respectively The corresponding control and state variables arerepresented in Figure 7
solution obtained with a discretization of the interval [119905119868 119905119865]
into 64 subintervalsIn particular we get 120579
1(119905119865) = minus110248 [rad] To check the
consistency of this result with the holonomic constraint (62)since 120579
1(119905119868) = 1205792(119905119868) = 0 [rads] we get from (58) that 119888
1= 0
and using the initial condition 1205791(119905119868) = 1205792(119905119868) = 0 [rad] we
get from (62) that 1198882= 0 Having established the value of the
constant 1198882 we obtain from the same equation for 120579
2(119905119865) =
120587 [rad] that 1205791(119905119865) = minus110248 [rad] which coincides with
the value of 1205791(119905119865) obtained numerically
722 Problem 4 Initial Value Problem with Nonzero InitialVelocity 120579
1 Another initial value problem has been solved
with the following conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rad] 120579
2(119905119865) = 120587 [rad]
1205791(119905119868) = 5 [rads] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(77)
The initial values of the multipliers have been set to zerowhereas their final value is left free Notice that no finalconditions have been imposed on 120579
1and 120579
1 The same
considerations done in previous section hold in this caseas well The technique described in Section 51 must beapplied twice leading to the differential constraint (74) andto the Hamiltonian (75) Figure 8 shows the sequence ofconfigurations of the robot at times 11989632with 119896 = 0 1 32and Figure 9 depicts the control and state variables ofthe optimal solution obtained with a discretization of theinterval [119905
119868 119905119865] into 64 subintervals In particular we get that
1205791(119905119865) = 617172 [rad] and 120579
1(119905119865) = 900163 [rads] To
check the consistency of the obtained value of 1205791(119905119865) with
the holonomic constraint consider (58) We can calculate
the constant 1198881using the initial conditions of the problem
obtaining
1198881= minus 120579
1(119905119868) (120572 + 2120573 cos 120579
2(119905119868))
minus 1205792(119905119868) (120575 + 120573 cos 120579
2(119905119868)) = minus225
(78)
Since 1198881= 0 (58) corresponds in this case to the differential
constraint
119889120578 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792+ 1198881119889119905 = 0
(79)
Equation (79) can be rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2 (119905))
120572 + 2120573 cos 1205792 (119905)
1198891205792minus
1198881
120572 + 2120573 cos 1205792 (119905)
119889119905 (80)
To check the obtained value of 1205791(119905119865) 1198891205791is numerically
integrated between 1205791(119905119868) and 120579
1(119905119865) using the interpolated
numerical optimal solution obtained for 1205792(119905) We get that
1205791(119905119865) = 618705 This value is close to 617172
To check the consistency of the obtained value of 1205791(119905119865)
with the constraint (58) using the computed value 1198881= minus225
and the final conditions 1205792(119905119865) = 0 120579
2(119905119865) = 120587 of the problem
we obtain
1205791(119905119865) =
minus (120575 + 120573 cos 1205792(119905119865))
120572 + 2120573 cos 1205792(119905119865)
1205792(119905119865)
minus1198881
120572 + 2120573 cos 1205792(119905119865)= 9
(81)
This value is very close to the value of 1205791(119905119865) obtained
numerically
73 Computational Issues If the optimal control problem has119898 variables and the time interval [119905
119868 119905119865] has been discretized
into 119873 subintervals the resulting set of difference equations(38) has119898times(119873minus1) equations and119898times(119873minus1) variables plusthe equations and variables due to transversality conditionsFeasible solutions have been used as initial guesses of thealgorithm
The solution of the nonlinear system of difference equa-tions (38) has been obtained using a damped Newtonalgorithm within a line search methodology implementedin Mathematica 7 under Mac OS X operating system (see[22 23] for more details)
8 Conclusion
In this paper the trajectory planning problem for planarunderactuated robot manipulators with two revolute jointswithout gravity has been studied This problem is solved asan optimal control problem based on a numerical resolutionof an unconstrained variational calculus reformulation of theoptimal control problem in which the dynamic equation ofthe mechanical system is regarded as a constraint It hasbeen shown that this reformulation method based on special
14 Abstract and Applied Analysis
10 20 30 40 50 60
minus1
minus08
minus06
minus04
minus02
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
minus15
minus1
minus05
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
(d) 1205792
10 20 30 40 50 60
minus15
minus10
5
10
15
minus5
(e) 1199062
Figure 7 Control and state variables of the optimal solution of problem 3 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 119906
2(119905119868) = 0Nm 120579
1(119905119865) = free
1205792(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads]The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectivelyThe corresponding
sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 6
Figure 8 Sequence of configurations of the robot manipulator at times 11989632 with 119896 = 0 1 32 corresponding to the optimal solutionof problem 4 an initial value problem for an underactuated 119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad]
1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained
with a discretization of [119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control
and state variables are represented in Figure 9
Abstract and Applied Analysis 15
10 20 30 40 50 60
1
2
3
4
5
6
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
4
5
6
7
8
9
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
6
(d) 1205792
10 20 30 40 50 60minus5
5
10
15
20
(e) 1199062
Figure 9 Control and state variables of the optimal solution of problem 4 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 8
derivative multipliers is able to tackle both integrable andnonintegrable differential constraints of the dynamic modelsof underactuated planar horizontal robot manipulators withtwo revolute joints This method can be seamlessly appliedin the presence of additional constraints on the mechanicalsystem
References
[1] A De Luca S Iannitti R Mattone and G Oriolo ldquoUnderactu-ated manipulators control properties and techniquesrdquoMachineIntelligence and Robotic Control vol 4 no 3 pp 113ndash125 2002
[2] G A Bliss Lectures on the Calculus of Variations University ofChicago Press Chicago Ill USA 1946
[3] J Gregory and C Lin Constrained Optimization in the Calculusof Variations and Optimal Control theory Chapman amp Hall1996
[4] W-S Koon and J E Marsden ldquoOptimal control for holonomicand nonholonomic mechanical systems with symmetry andLagrangian reductionrdquo SIAM Journal on Control and Optimiza-tion vol 35 no 3 pp 901ndash929 1997
[5] A M Bloch Nonholonomic Mechanics and Control SpringerNew York NY USA 2003
[6] I I Hussein and A M Bloch ldquoOptimal control of underactu-ated nonholonomic mechanical systemsrdquo IEEE Transactions onAutomatic Control vol 53 no 3 pp 668ndash682 2008
[7] X Z Lai J H She S X Yang andMWu ldquoComprehensive uni-fied control strategy for underactuated two-link manipulatorsrdquoIEEE Transactions on Systems Man and Cybernetics B vol 39no 2 pp 389ndash398 2009
[8] J P Ordaz-Oliver O J Santos-Sanchez and V Lopez-MoralesldquoToward a generalized sub-optimal control method of underac-tuated systemsrdquo Optimal Control Applications amp Methods vol33 no 3 pp 338ndash351 2012
16 Abstract and Applied Analysis
[9] R Seifried ldquoTwo approaches for feedforward control andoptimal design of underactuatedmultibody systemsrdquoMultibodySystem Dynamics vol 27 no 1 pp 75ndash93 2012
[10] M Buss O von Stryk R Bulirsch and G Schmidt ldquoTowardshybrid optimal controlrdquo atmdashAutomatisierungstechnik vol 48no 9 pp 448ndash459 2000
[11] M Buss M Glocker M Hardt O von Stryk R Bulirsch andG Schmidt ldquoNonlinear hybrid dynamical systems modelingoptimal control and applicationsrdquo in Modelling Analysis andDesign of Hybrid Systems S Engell G Frehse and E SchniederEds vol 279 of Lecture Notes in Control and InformationScience pp 331ndash335 Springer 2002
[12] G Oriolo and Y Nakamura ldquoControl of mechanical systemswith second-order nonholonomic constraints underactuatedmanipulatorsrdquo in Proceedings of the 30th IEEE Conference onDecision and Control pp 2398ndash2403 December 1991
[13] T J Tarn M Zhang and A Serrani ldquoNew integrability condi-tions for differential constraintsrdquo Systems and Control Lettersvol 49 no 5 pp 335ndash345 2003
[14] H J Sussmann ldquoA general theorem on local controllabilityrdquoSIAM Journal on Control and Optimization vol 25 no 1 pp158ndash194 1987
[15] F Bullo A D Lewis and K M Lynch ldquoControllable kinematicreductions for mechanical systems concepts computationaltools and examplesrdquo in Proceedings of International Symposiumon Mathematical Theory of Networks and Systems 2002
[16] F Bullo and A D Lewis ldquoLow-order controllability and kine-matic reductions for affine connection control systemsrdquo SIAMJournal on Control andOptimization vol 44 no 3 pp 885ndash9082006
[17] A D Lewis and R MMurray ldquoConfiguration controllability ofsimple mechanical control systemsrdquo SIAM Journal on Controland Optimization vol 35 no 3 pp 766ndash790 1997
[18] F Bullo and K M Lynch ldquoKinematic controllability for decou-pled trajectory planning in underactuatedmechanical systemsrdquoIEEE Transactions on Robotics and Automation vol 17 no 4 pp402ndash412 2001
[19] M R Hestenes Calculus of Variations and Optimcl ControlTheory John Wiley amp Sons 1966
[20] J Gregory ldquoA new systematic method for efficiently solvingholonomic (and nonholonomic) constraint problemsrdquo Analysisand Applications vol 8 no 1 pp 85ndash98 2010
[21] J Gregory and R S Wang ldquoDiscrete variable methods forthe m-dependent variable nonlinear extremal problem in thecalculus of variationsrdquo SIAM Journal onNumerical Analysis vol27 no 2 pp 470ndash487 1990
[22] Wolfram Research 2012[23] J J More and D J Thuente ldquoLine search algorithms with guar-
anteed sufficient decreaserdquo ACM Transactions on MathematicalSoftware vol 20 no 3 pp 286ndash307 1994
Figure 5 Control and state variables of the optimal solution of problem 2 an initial value problem for a119877119877 robotmanipulator with boundaryconditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = minus1205872 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = 1205872 [rad] 120579
2(119905119865) = minus1205872 [rad] 120579
1(119905119865) = free
and 1205792(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of configurations of the
robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 4
The initial values of the control variable and of themultipliershave been set to zero whereas their final value is left free
Since there is a holonomic constraint that relates thevalues of the angles 120579
1and 120579
2 without integrating (58) we
are not able to find the value of 1205791(119905119865) consistent with 120579
1(119905119868)
Therefore no final conditions have been imposed on 1205791
From the initial conditions of the problem we obtain 1198881=
0 Equation (58) with 1198881= 0 corresponds to the homogeneous
differential constraint
119889120577 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792= 0 (60)
The differential 119889120577 is not exact However it becomes an exactdifferential if multiplied by the factor 1(120572 + 2120573 cos 120579
2) This
operation does not alter the differential equation (60) Inthis case there does exist a function 120577 whose differentialcoincides with the expression 119889120577(120572 + 2120573 cos 120579
2) Due to
the existence of this function the integral of 119889120577 between
two points depends only on these points and not on theintegration path Equation (60) rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2)
120572 + 2120573 cos 1205792
1198891205792
(61)
can be integrated by separating variables The correspondingholonomic constraint has the following expression
1205791= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1205792
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1205792
2+ 1198882
(62)
To express this optimal control problem in the form ofa basic optimal control problem we first have to convert (57)
12 Abstract and Applied Analysis
into a first-order differential model introducing the followingchange of variables
1199091= 1205791 119909
3= 1205791
1199092= 1205792 119909
4= 1205792
(63)
with the following additional relations
1199091015840
1= 1199093
1199091015840
2= 1199094
(64)
Thus the optimal control problem is to minimize
int
1
0
1199062
2119889119905 (65)
subject to the constraints1199091
= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1199092
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1199092
2+ 1198882
(66)
(120575 + 120573 cos 1205792) 1199091015840
3+ 1205751199091015840
4+ 120573 sin119909
21199092
3= 1199062 (67)
and the additional constraints (64) To reformulate thisoptimal control problem as an unconstrained calculus ofvariations problem let X be
X = [
[
1198831
sdot sdot sdot
1198839
]
]
(68)
such that119883119894= 119909119894 119894 = 1 4
1198831015840
5= 1199061 119883
5(119905119868) = 0
(69)
where 11988310158406with 119883
6(119905119868) = 0 is the multiplier associated with
the holonomic constraint (66) 11988310158407with 119883
7(119905119868) = 0 is the
multipliers associated with the differential constraint (67)and 1198831015840
8with 119883
8(119905119868) = 0 and 1198831015840
9with 119883
9(119905119868) = 0 are the
multipliers associatedwith the additional equality constraints(64)
Thus the holonomic constraint of the problem can berewritten as follows
120593 (119905X)
= 1198831minus((120572 minus 2120575)
times tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1198832
2)]]
]
)
times (radic41205732 minus 1205722)
minus1
+1198832
2= 0
(70)
Now the technique described in Section 51 to deal withholonomic constraints can be applied to 120593(119905X) and thisholonomic constraint is replaced by
120593119905+ 1205931198831198831015840= 0
120593 (0 119883 (0)) = 0
(71)
From the initial conditions of the problem the latter equationreduces to the equality 0 = 0 whereas the former takes thefollowing form
(120572 + 2120573 cos (1198832))1198833+ (120575 + 120573 cos (119883
2))1198834= 0 (72)
The corresponding Hamiltonian is
1198661= 11988310158402
5+ 1198831015840
6((120572 + 2120573 cos (119883
2))1198833
+ (120575 + 120573 cos (1198832))1198834)
+1198831015840
7(120573 sin (119883
2)1198832
3+ (120575 + 120573 cos (119883
2))1198831015840
3
+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(73)
It is not difficult to check that matrix 1198661X1015840X1015840
is singularThis is due to the fact that to handle our optimal controlproblemwhich involves second-order differential constraintswe converted them into first-order differential constraintsTherefore we apply again the technique of Section 51obtaining the identity 0 = 0 and the following constraint
minus 120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4= 0
(74)
The corresponding Hamiltonian is
1198662= 11988310158402
5+ 1198831015840
6
times (minus120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4)
+ 1198831015840
7(120573 sin (119883
2)1198832
3+(120575+120573 cos (119883
2))1198831015840
3+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(75)
It is not difficult to check that matrix 1198662X1015840X1015840
in this case is notsingular since its determinant is
Substituting the values of120572120573 and 120575 this expression becomesdet(119866
2X1015840X1015840) = 1128(43 minus 2 cos(2119883
2))2 which is always
positive for any real value1198832 Figure 6 shows the sequence of
configurations of the robot at times 11989632with 119896 = 0 1 32and Figure 7 depicts control and state variables of the optimal
Abstract and Applied Analysis 13
Figure 6 Sequence of configurations of the robot manipulator attimes 11989632 with 119896 = 0 1 32 corresponding to the optimal sol-ution of problem 3 an initial value problem for an underactuated119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) =
0 [rad] 1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads]
1205791(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2
(119905119865) = 0 [rads] obtained with a discretization of [119905
119868 119905119865] into 64
subintervals The initial and final times are 119905119868= 0 and 119905
119865=
1 [s] respectively The corresponding control and state variables arerepresented in Figure 7
solution obtained with a discretization of the interval [119905119868 119905119865]
into 64 subintervalsIn particular we get 120579
1(119905119865) = minus110248 [rad] To check the
consistency of this result with the holonomic constraint (62)since 120579
1(119905119868) = 1205792(119905119868) = 0 [rads] we get from (58) that 119888
1= 0
and using the initial condition 1205791(119905119868) = 1205792(119905119868) = 0 [rad] we
get from (62) that 1198882= 0 Having established the value of the
constant 1198882 we obtain from the same equation for 120579
2(119905119865) =
120587 [rad] that 1205791(119905119865) = minus110248 [rad] which coincides with
the value of 1205791(119905119865) obtained numerically
722 Problem 4 Initial Value Problem with Nonzero InitialVelocity 120579
1 Another initial value problem has been solved
with the following conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rad] 120579
2(119905119865) = 120587 [rad]
1205791(119905119868) = 5 [rads] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(77)
The initial values of the multipliers have been set to zerowhereas their final value is left free Notice that no finalconditions have been imposed on 120579
1and 120579
1 The same
considerations done in previous section hold in this caseas well The technique described in Section 51 must beapplied twice leading to the differential constraint (74) andto the Hamiltonian (75) Figure 8 shows the sequence ofconfigurations of the robot at times 11989632with 119896 = 0 1 32and Figure 9 depicts the control and state variables ofthe optimal solution obtained with a discretization of theinterval [119905
119868 119905119865] into 64 subintervals In particular we get that
1205791(119905119865) = 617172 [rad] and 120579
1(119905119865) = 900163 [rads] To
check the consistency of the obtained value of 1205791(119905119865) with
the holonomic constraint consider (58) We can calculate
the constant 1198881using the initial conditions of the problem
obtaining
1198881= minus 120579
1(119905119868) (120572 + 2120573 cos 120579
2(119905119868))
minus 1205792(119905119868) (120575 + 120573 cos 120579
2(119905119868)) = minus225
(78)
Since 1198881= 0 (58) corresponds in this case to the differential
constraint
119889120578 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792+ 1198881119889119905 = 0
(79)
Equation (79) can be rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2 (119905))
120572 + 2120573 cos 1205792 (119905)
1198891205792minus
1198881
120572 + 2120573 cos 1205792 (119905)
119889119905 (80)
To check the obtained value of 1205791(119905119865) 1198891205791is numerically
integrated between 1205791(119905119868) and 120579
1(119905119865) using the interpolated
numerical optimal solution obtained for 1205792(119905) We get that
1205791(119905119865) = 618705 This value is close to 617172
To check the consistency of the obtained value of 1205791(119905119865)
with the constraint (58) using the computed value 1198881= minus225
and the final conditions 1205792(119905119865) = 0 120579
2(119905119865) = 120587 of the problem
we obtain
1205791(119905119865) =
minus (120575 + 120573 cos 1205792(119905119865))
120572 + 2120573 cos 1205792(119905119865)
1205792(119905119865)
minus1198881
120572 + 2120573 cos 1205792(119905119865)= 9
(81)
This value is very close to the value of 1205791(119905119865) obtained
numerically
73 Computational Issues If the optimal control problem has119898 variables and the time interval [119905
119868 119905119865] has been discretized
into 119873 subintervals the resulting set of difference equations(38) has119898times(119873minus1) equations and119898times(119873minus1) variables plusthe equations and variables due to transversality conditionsFeasible solutions have been used as initial guesses of thealgorithm
The solution of the nonlinear system of difference equa-tions (38) has been obtained using a damped Newtonalgorithm within a line search methodology implementedin Mathematica 7 under Mac OS X operating system (see[22 23] for more details)
8 Conclusion
In this paper the trajectory planning problem for planarunderactuated robot manipulators with two revolute jointswithout gravity has been studied This problem is solved asan optimal control problem based on a numerical resolutionof an unconstrained variational calculus reformulation of theoptimal control problem in which the dynamic equation ofthe mechanical system is regarded as a constraint It hasbeen shown that this reformulation method based on special
14 Abstract and Applied Analysis
10 20 30 40 50 60
minus1
minus08
minus06
minus04
minus02
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
minus15
minus1
minus05
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
(d) 1205792
10 20 30 40 50 60
minus15
minus10
5
10
15
minus5
(e) 1199062
Figure 7 Control and state variables of the optimal solution of problem 3 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 119906
2(119905119868) = 0Nm 120579
1(119905119865) = free
1205792(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads]The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectivelyThe corresponding
sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 6
Figure 8 Sequence of configurations of the robot manipulator at times 11989632 with 119896 = 0 1 32 corresponding to the optimal solutionof problem 4 an initial value problem for an underactuated 119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad]
1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained
with a discretization of [119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control
and state variables are represented in Figure 9
Abstract and Applied Analysis 15
10 20 30 40 50 60
1
2
3
4
5
6
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
4
5
6
7
8
9
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
6
(d) 1205792
10 20 30 40 50 60minus5
5
10
15
20
(e) 1199062
Figure 9 Control and state variables of the optimal solution of problem 4 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 8
derivative multipliers is able to tackle both integrable andnonintegrable differential constraints of the dynamic modelsof underactuated planar horizontal robot manipulators withtwo revolute joints This method can be seamlessly appliedin the presence of additional constraints on the mechanicalsystem
References
[1] A De Luca S Iannitti R Mattone and G Oriolo ldquoUnderactu-ated manipulators control properties and techniquesrdquoMachineIntelligence and Robotic Control vol 4 no 3 pp 113ndash125 2002
[2] G A Bliss Lectures on the Calculus of Variations University ofChicago Press Chicago Ill USA 1946
[3] J Gregory and C Lin Constrained Optimization in the Calculusof Variations and Optimal Control theory Chapman amp Hall1996
[4] W-S Koon and J E Marsden ldquoOptimal control for holonomicand nonholonomic mechanical systems with symmetry andLagrangian reductionrdquo SIAM Journal on Control and Optimiza-tion vol 35 no 3 pp 901ndash929 1997
[5] A M Bloch Nonholonomic Mechanics and Control SpringerNew York NY USA 2003
[6] I I Hussein and A M Bloch ldquoOptimal control of underactu-ated nonholonomic mechanical systemsrdquo IEEE Transactions onAutomatic Control vol 53 no 3 pp 668ndash682 2008
[7] X Z Lai J H She S X Yang andMWu ldquoComprehensive uni-fied control strategy for underactuated two-link manipulatorsrdquoIEEE Transactions on Systems Man and Cybernetics B vol 39no 2 pp 389ndash398 2009
[8] J P Ordaz-Oliver O J Santos-Sanchez and V Lopez-MoralesldquoToward a generalized sub-optimal control method of underac-tuated systemsrdquo Optimal Control Applications amp Methods vol33 no 3 pp 338ndash351 2012
16 Abstract and Applied Analysis
[9] R Seifried ldquoTwo approaches for feedforward control andoptimal design of underactuatedmultibody systemsrdquoMultibodySystem Dynamics vol 27 no 1 pp 75ndash93 2012
[10] M Buss O von Stryk R Bulirsch and G Schmidt ldquoTowardshybrid optimal controlrdquo atmdashAutomatisierungstechnik vol 48no 9 pp 448ndash459 2000
[11] M Buss M Glocker M Hardt O von Stryk R Bulirsch andG Schmidt ldquoNonlinear hybrid dynamical systems modelingoptimal control and applicationsrdquo in Modelling Analysis andDesign of Hybrid Systems S Engell G Frehse and E SchniederEds vol 279 of Lecture Notes in Control and InformationScience pp 331ndash335 Springer 2002
[12] G Oriolo and Y Nakamura ldquoControl of mechanical systemswith second-order nonholonomic constraints underactuatedmanipulatorsrdquo in Proceedings of the 30th IEEE Conference onDecision and Control pp 2398ndash2403 December 1991
[13] T J Tarn M Zhang and A Serrani ldquoNew integrability condi-tions for differential constraintsrdquo Systems and Control Lettersvol 49 no 5 pp 335ndash345 2003
[14] H J Sussmann ldquoA general theorem on local controllabilityrdquoSIAM Journal on Control and Optimization vol 25 no 1 pp158ndash194 1987
[15] F Bullo A D Lewis and K M Lynch ldquoControllable kinematicreductions for mechanical systems concepts computationaltools and examplesrdquo in Proceedings of International Symposiumon Mathematical Theory of Networks and Systems 2002
[16] F Bullo and A D Lewis ldquoLow-order controllability and kine-matic reductions for affine connection control systemsrdquo SIAMJournal on Control andOptimization vol 44 no 3 pp 885ndash9082006
[17] A D Lewis and R MMurray ldquoConfiguration controllability ofsimple mechanical control systemsrdquo SIAM Journal on Controland Optimization vol 35 no 3 pp 766ndash790 1997
[18] F Bullo and K M Lynch ldquoKinematic controllability for decou-pled trajectory planning in underactuatedmechanical systemsrdquoIEEE Transactions on Robotics and Automation vol 17 no 4 pp402ndash412 2001
[19] M R Hestenes Calculus of Variations and Optimcl ControlTheory John Wiley amp Sons 1966
[20] J Gregory ldquoA new systematic method for efficiently solvingholonomic (and nonholonomic) constraint problemsrdquo Analysisand Applications vol 8 no 1 pp 85ndash98 2010
[21] J Gregory and R S Wang ldquoDiscrete variable methods forthe m-dependent variable nonlinear extremal problem in thecalculus of variationsrdquo SIAM Journal onNumerical Analysis vol27 no 2 pp 470ndash487 1990
[22] Wolfram Research 2012[23] J J More and D J Thuente ldquoLine search algorithms with guar-
anteed sufficient decreaserdquo ACM Transactions on MathematicalSoftware vol 20 no 3 pp 286ndash307 1994
into a first-order differential model introducing the followingchange of variables
1199091= 1205791 119909
3= 1205791
1199092= 1205792 119909
4= 1205792
(63)
with the following additional relations
1199091015840
1= 1199093
1199091015840
2= 1199094
(64)
Thus the optimal control problem is to minimize
int
1
0
1199062
2119889119905 (65)
subject to the constraints1199091
= (120572 minus 2120575) tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1199092
2)]]
]
times (radic41205732 minus 1205722)
minus1
minus1199092
2+ 1198882
(66)
(120575 + 120573 cos 1205792) 1199091015840
3+ 1205751199091015840
4+ 120573 sin119909
21199092
3= 1199062 (67)
and the additional constraints (64) To reformulate thisoptimal control problem as an unconstrained calculus ofvariations problem let X be
X = [
[
1198831
sdot sdot sdot
1198839
]
]
(68)
such that119883119894= 119909119894 119894 = 1 4
1198831015840
5= 1199061 119883
5(119905119868) = 0
(69)
where 11988310158406with 119883
6(119905119868) = 0 is the multiplier associated with
the holonomic constraint (66) 11988310158407with 119883
7(119905119868) = 0 is the
multipliers associated with the differential constraint (67)and 1198831015840
8with 119883
8(119905119868) = 0 and 1198831015840
9with 119883
9(119905119868) = 0 are the
multipliers associatedwith the additional equality constraints(64)
Thus the holonomic constraint of the problem can berewritten as follows
120593 (119905X)
= 1198831minus((120572 minus 2120575)
times tanhminus1 [[
[
2120573 minus 120572
radic41205732 minus 1205722tan(1198832
2)]]
]
)
times (radic41205732 minus 1205722)
minus1
+1198832
2= 0
(70)
Now the technique described in Section 51 to deal withholonomic constraints can be applied to 120593(119905X) and thisholonomic constraint is replaced by
120593119905+ 1205931198831198831015840= 0
120593 (0 119883 (0)) = 0
(71)
From the initial conditions of the problem the latter equationreduces to the equality 0 = 0 whereas the former takes thefollowing form
(120572 + 2120573 cos (1198832))1198833+ (120575 + 120573 cos (119883
2))1198834= 0 (72)
The corresponding Hamiltonian is
1198661= 11988310158402
5+ 1198831015840
6((120572 + 2120573 cos (119883
2))1198833
+ (120575 + 120573 cos (1198832))1198834)
+1198831015840
7(120573 sin (119883
2)1198832
3+ (120575 + 120573 cos (119883
2))1198831015840
3
+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(73)
It is not difficult to check that matrix 1198661X1015840X1015840
is singularThis is due to the fact that to handle our optimal controlproblemwhich involves second-order differential constraintswe converted them into first-order differential constraintsTherefore we apply again the technique of Section 51obtaining the identity 0 = 0 and the following constraint
minus 120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4= 0
(74)
The corresponding Hamiltonian is
1198662= 11988310158402
5+ 1198831015840
6
times (minus120573 sin (1198832)11988331198834minus 120573 sin (119883
2)1198834(1198833+ 1198834)
+ (120572 + 2120573 cos (1198832))1198831015840
3+ (120575 + 120573 cos (119883
2))1198831015840
4)
+ 1198831015840
7(120573 sin (119883
2)1198832
3+(120575+120573 cos (119883
2))1198831015840
3+ 1205751198831015840
4minus 1198831015840
5)
+ 1198831015840
8(1198831015840
1minus 1198833) + 119883
1015840
9(1198831015840
2minus 1198834)
(75)
It is not difficult to check that matrix 1198662X1015840X1015840
in this case is notsingular since its determinant is
Substituting the values of120572120573 and 120575 this expression becomesdet(119866
2X1015840X1015840) = 1128(43 minus 2 cos(2119883
2))2 which is always
positive for any real value1198832 Figure 6 shows the sequence of
configurations of the robot at times 11989632with 119896 = 0 1 32and Figure 7 depicts control and state variables of the optimal
Abstract and Applied Analysis 13
Figure 6 Sequence of configurations of the robot manipulator attimes 11989632 with 119896 = 0 1 32 corresponding to the optimal sol-ution of problem 3 an initial value problem for an underactuated119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) =
0 [rad] 1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads]
1205791(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2
(119905119865) = 0 [rads] obtained with a discretization of [119905
119868 119905119865] into 64
subintervals The initial and final times are 119905119868= 0 and 119905
119865=
1 [s] respectively The corresponding control and state variables arerepresented in Figure 7
solution obtained with a discretization of the interval [119905119868 119905119865]
into 64 subintervalsIn particular we get 120579
1(119905119865) = minus110248 [rad] To check the
consistency of this result with the holonomic constraint (62)since 120579
1(119905119868) = 1205792(119905119868) = 0 [rads] we get from (58) that 119888
1= 0
and using the initial condition 1205791(119905119868) = 1205792(119905119868) = 0 [rad] we
get from (62) that 1198882= 0 Having established the value of the
constant 1198882 we obtain from the same equation for 120579
2(119905119865) =
120587 [rad] that 1205791(119905119865) = minus110248 [rad] which coincides with
the value of 1205791(119905119865) obtained numerically
722 Problem 4 Initial Value Problem with Nonzero InitialVelocity 120579
1 Another initial value problem has been solved
with the following conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rad] 120579
2(119905119865) = 120587 [rad]
1205791(119905119868) = 5 [rads] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(77)
The initial values of the multipliers have been set to zerowhereas their final value is left free Notice that no finalconditions have been imposed on 120579
1and 120579
1 The same
considerations done in previous section hold in this caseas well The technique described in Section 51 must beapplied twice leading to the differential constraint (74) andto the Hamiltonian (75) Figure 8 shows the sequence ofconfigurations of the robot at times 11989632with 119896 = 0 1 32and Figure 9 depicts the control and state variables ofthe optimal solution obtained with a discretization of theinterval [119905
119868 119905119865] into 64 subintervals In particular we get that
1205791(119905119865) = 617172 [rad] and 120579
1(119905119865) = 900163 [rads] To
check the consistency of the obtained value of 1205791(119905119865) with
the holonomic constraint consider (58) We can calculate
the constant 1198881using the initial conditions of the problem
obtaining
1198881= minus 120579
1(119905119868) (120572 + 2120573 cos 120579
2(119905119868))
minus 1205792(119905119868) (120575 + 120573 cos 120579
2(119905119868)) = minus225
(78)
Since 1198881= 0 (58) corresponds in this case to the differential
constraint
119889120578 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792+ 1198881119889119905 = 0
(79)
Equation (79) can be rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2 (119905))
120572 + 2120573 cos 1205792 (119905)
1198891205792minus
1198881
120572 + 2120573 cos 1205792 (119905)
119889119905 (80)
To check the obtained value of 1205791(119905119865) 1198891205791is numerically
integrated between 1205791(119905119868) and 120579
1(119905119865) using the interpolated
numerical optimal solution obtained for 1205792(119905) We get that
1205791(119905119865) = 618705 This value is close to 617172
To check the consistency of the obtained value of 1205791(119905119865)
with the constraint (58) using the computed value 1198881= minus225
and the final conditions 1205792(119905119865) = 0 120579
2(119905119865) = 120587 of the problem
we obtain
1205791(119905119865) =
minus (120575 + 120573 cos 1205792(119905119865))
120572 + 2120573 cos 1205792(119905119865)
1205792(119905119865)
minus1198881
120572 + 2120573 cos 1205792(119905119865)= 9
(81)
This value is very close to the value of 1205791(119905119865) obtained
numerically
73 Computational Issues If the optimal control problem has119898 variables and the time interval [119905
119868 119905119865] has been discretized
into 119873 subintervals the resulting set of difference equations(38) has119898times(119873minus1) equations and119898times(119873minus1) variables plusthe equations and variables due to transversality conditionsFeasible solutions have been used as initial guesses of thealgorithm
The solution of the nonlinear system of difference equa-tions (38) has been obtained using a damped Newtonalgorithm within a line search methodology implementedin Mathematica 7 under Mac OS X operating system (see[22 23] for more details)
8 Conclusion
In this paper the trajectory planning problem for planarunderactuated robot manipulators with two revolute jointswithout gravity has been studied This problem is solved asan optimal control problem based on a numerical resolutionof an unconstrained variational calculus reformulation of theoptimal control problem in which the dynamic equation ofthe mechanical system is regarded as a constraint It hasbeen shown that this reformulation method based on special
14 Abstract and Applied Analysis
10 20 30 40 50 60
minus1
minus08
minus06
minus04
minus02
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
minus15
minus1
minus05
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
(d) 1205792
10 20 30 40 50 60
minus15
minus10
5
10
15
minus5
(e) 1199062
Figure 7 Control and state variables of the optimal solution of problem 3 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 119906
2(119905119868) = 0Nm 120579
1(119905119865) = free
1205792(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads]The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectivelyThe corresponding
sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 6
Figure 8 Sequence of configurations of the robot manipulator at times 11989632 with 119896 = 0 1 32 corresponding to the optimal solutionof problem 4 an initial value problem for an underactuated 119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad]
1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained
with a discretization of [119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control
and state variables are represented in Figure 9
Abstract and Applied Analysis 15
10 20 30 40 50 60
1
2
3
4
5
6
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
4
5
6
7
8
9
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
6
(d) 1205792
10 20 30 40 50 60minus5
5
10
15
20
(e) 1199062
Figure 9 Control and state variables of the optimal solution of problem 4 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 8
derivative multipliers is able to tackle both integrable andnonintegrable differential constraints of the dynamic modelsof underactuated planar horizontal robot manipulators withtwo revolute joints This method can be seamlessly appliedin the presence of additional constraints on the mechanicalsystem
References
[1] A De Luca S Iannitti R Mattone and G Oriolo ldquoUnderactu-ated manipulators control properties and techniquesrdquoMachineIntelligence and Robotic Control vol 4 no 3 pp 113ndash125 2002
[2] G A Bliss Lectures on the Calculus of Variations University ofChicago Press Chicago Ill USA 1946
[3] J Gregory and C Lin Constrained Optimization in the Calculusof Variations and Optimal Control theory Chapman amp Hall1996
[4] W-S Koon and J E Marsden ldquoOptimal control for holonomicand nonholonomic mechanical systems with symmetry andLagrangian reductionrdquo SIAM Journal on Control and Optimiza-tion vol 35 no 3 pp 901ndash929 1997
[5] A M Bloch Nonholonomic Mechanics and Control SpringerNew York NY USA 2003
[6] I I Hussein and A M Bloch ldquoOptimal control of underactu-ated nonholonomic mechanical systemsrdquo IEEE Transactions onAutomatic Control vol 53 no 3 pp 668ndash682 2008
[7] X Z Lai J H She S X Yang andMWu ldquoComprehensive uni-fied control strategy for underactuated two-link manipulatorsrdquoIEEE Transactions on Systems Man and Cybernetics B vol 39no 2 pp 389ndash398 2009
[8] J P Ordaz-Oliver O J Santos-Sanchez and V Lopez-MoralesldquoToward a generalized sub-optimal control method of underac-tuated systemsrdquo Optimal Control Applications amp Methods vol33 no 3 pp 338ndash351 2012
16 Abstract and Applied Analysis
[9] R Seifried ldquoTwo approaches for feedforward control andoptimal design of underactuatedmultibody systemsrdquoMultibodySystem Dynamics vol 27 no 1 pp 75ndash93 2012
[10] M Buss O von Stryk R Bulirsch and G Schmidt ldquoTowardshybrid optimal controlrdquo atmdashAutomatisierungstechnik vol 48no 9 pp 448ndash459 2000
[11] M Buss M Glocker M Hardt O von Stryk R Bulirsch andG Schmidt ldquoNonlinear hybrid dynamical systems modelingoptimal control and applicationsrdquo in Modelling Analysis andDesign of Hybrid Systems S Engell G Frehse and E SchniederEds vol 279 of Lecture Notes in Control and InformationScience pp 331ndash335 Springer 2002
[12] G Oriolo and Y Nakamura ldquoControl of mechanical systemswith second-order nonholonomic constraints underactuatedmanipulatorsrdquo in Proceedings of the 30th IEEE Conference onDecision and Control pp 2398ndash2403 December 1991
[13] T J Tarn M Zhang and A Serrani ldquoNew integrability condi-tions for differential constraintsrdquo Systems and Control Lettersvol 49 no 5 pp 335ndash345 2003
[14] H J Sussmann ldquoA general theorem on local controllabilityrdquoSIAM Journal on Control and Optimization vol 25 no 1 pp158ndash194 1987
[15] F Bullo A D Lewis and K M Lynch ldquoControllable kinematicreductions for mechanical systems concepts computationaltools and examplesrdquo in Proceedings of International Symposiumon Mathematical Theory of Networks and Systems 2002
[16] F Bullo and A D Lewis ldquoLow-order controllability and kine-matic reductions for affine connection control systemsrdquo SIAMJournal on Control andOptimization vol 44 no 3 pp 885ndash9082006
[17] A D Lewis and R MMurray ldquoConfiguration controllability ofsimple mechanical control systemsrdquo SIAM Journal on Controland Optimization vol 35 no 3 pp 766ndash790 1997
[18] F Bullo and K M Lynch ldquoKinematic controllability for decou-pled trajectory planning in underactuatedmechanical systemsrdquoIEEE Transactions on Robotics and Automation vol 17 no 4 pp402ndash412 2001
[19] M R Hestenes Calculus of Variations and Optimcl ControlTheory John Wiley amp Sons 1966
[20] J Gregory ldquoA new systematic method for efficiently solvingholonomic (and nonholonomic) constraint problemsrdquo Analysisand Applications vol 8 no 1 pp 85ndash98 2010
[21] J Gregory and R S Wang ldquoDiscrete variable methods forthe m-dependent variable nonlinear extremal problem in thecalculus of variationsrdquo SIAM Journal onNumerical Analysis vol27 no 2 pp 470ndash487 1990
[22] Wolfram Research 2012[23] J J More and D J Thuente ldquoLine search algorithms with guar-
anteed sufficient decreaserdquo ACM Transactions on MathematicalSoftware vol 20 no 3 pp 286ndash307 1994
Figure 6 Sequence of configurations of the robot manipulator attimes 11989632 with 119896 = 0 1 32 corresponding to the optimal sol-ution of problem 3 an initial value problem for an underactuated119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) =
0 [rad] 1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads]
1205791(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2
(119905119865) = 0 [rads] obtained with a discretization of [119905
119868 119905119865] into 64
subintervals The initial and final times are 119905119868= 0 and 119905
119865=
1 [s] respectively The corresponding control and state variables arerepresented in Figure 7
solution obtained with a discretization of the interval [119905119868 119905119865]
into 64 subintervalsIn particular we get 120579
1(119905119865) = minus110248 [rad] To check the
consistency of this result with the holonomic constraint (62)since 120579
1(119905119868) = 1205792(119905119868) = 0 [rads] we get from (58) that 119888
1= 0
and using the initial condition 1205791(119905119868) = 1205792(119905119868) = 0 [rad] we
get from (62) that 1198882= 0 Having established the value of the
constant 1198882 we obtain from the same equation for 120579
2(119905119865) =
120587 [rad] that 1205791(119905119865) = minus110248 [rad] which coincides with
the value of 1205791(119905119865) obtained numerically
722 Problem 4 Initial Value Problem with Nonzero InitialVelocity 120579
1 Another initial value problem has been solved
with the following conditions
1205791(119905119868) = 0 [rad] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rad] 120579
2(119905119865) = 120587 [rad]
1205791(119905119868) = 5 [rads] 120579
1(119905119865) = free
1205792(119905119868) = 0 [rads] 120579
2(119905119865) = 0 [rads]
(77)
The initial values of the multipliers have been set to zerowhereas their final value is left free Notice that no finalconditions have been imposed on 120579
1and 120579
1 The same
considerations done in previous section hold in this caseas well The technique described in Section 51 must beapplied twice leading to the differential constraint (74) andto the Hamiltonian (75) Figure 8 shows the sequence ofconfigurations of the robot at times 11989632with 119896 = 0 1 32and Figure 9 depicts the control and state variables ofthe optimal solution obtained with a discretization of theinterval [119905
119868 119905119865] into 64 subintervals In particular we get that
1205791(119905119865) = 617172 [rad] and 120579
1(119905119865) = 900163 [rads] To
check the consistency of the obtained value of 1205791(119905119865) with
the holonomic constraint consider (58) We can calculate
the constant 1198881using the initial conditions of the problem
obtaining
1198881= minus 120579
1(119905119868) (120572 + 2120573 cos 120579
2(119905119868))
minus 1205792(119905119868) (120575 + 120573 cos 120579
2(119905119868)) = minus225
(78)
Since 1198881= 0 (58) corresponds in this case to the differential
constraint
119889120578 = (120572 + 2120573 cos 1205792) 1198891205791+ (120575 + 120573 cos 120579
2) 1198891205792+ 1198881119889119905 = 0
(79)
Equation (79) can be rewritten in this form
1198891205791=minus (120575 + 120573 cos 120579
2 (119905))
120572 + 2120573 cos 1205792 (119905)
1198891205792minus
1198881
120572 + 2120573 cos 1205792 (119905)
119889119905 (80)
To check the obtained value of 1205791(119905119865) 1198891205791is numerically
integrated between 1205791(119905119868) and 120579
1(119905119865) using the interpolated
numerical optimal solution obtained for 1205792(119905) We get that
1205791(119905119865) = 618705 This value is close to 617172
To check the consistency of the obtained value of 1205791(119905119865)
with the constraint (58) using the computed value 1198881= minus225
and the final conditions 1205792(119905119865) = 0 120579
2(119905119865) = 120587 of the problem
we obtain
1205791(119905119865) =
minus (120575 + 120573 cos 1205792(119905119865))
120572 + 2120573 cos 1205792(119905119865)
1205792(119905119865)
minus1198881
120572 + 2120573 cos 1205792(119905119865)= 9
(81)
This value is very close to the value of 1205791(119905119865) obtained
numerically
73 Computational Issues If the optimal control problem has119898 variables and the time interval [119905
119868 119905119865] has been discretized
into 119873 subintervals the resulting set of difference equations(38) has119898times(119873minus1) equations and119898times(119873minus1) variables plusthe equations and variables due to transversality conditionsFeasible solutions have been used as initial guesses of thealgorithm
The solution of the nonlinear system of difference equa-tions (38) has been obtained using a damped Newtonalgorithm within a line search methodology implementedin Mathematica 7 under Mac OS X operating system (see[22 23] for more details)
8 Conclusion
In this paper the trajectory planning problem for planarunderactuated robot manipulators with two revolute jointswithout gravity has been studied This problem is solved asan optimal control problem based on a numerical resolutionof an unconstrained variational calculus reformulation of theoptimal control problem in which the dynamic equation ofthe mechanical system is regarded as a constraint It hasbeen shown that this reformulation method based on special
14 Abstract and Applied Analysis
10 20 30 40 50 60
minus1
minus08
minus06
minus04
minus02
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
minus15
minus1
minus05
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
(d) 1205792
10 20 30 40 50 60
minus15
minus10
5
10
15
minus5
(e) 1199062
Figure 7 Control and state variables of the optimal solution of problem 3 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 119906
2(119905119868) = 0Nm 120579
1(119905119865) = free
1205792(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads]The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectivelyThe corresponding
sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 6
Figure 8 Sequence of configurations of the robot manipulator at times 11989632 with 119896 = 0 1 32 corresponding to the optimal solutionof problem 4 an initial value problem for an underactuated 119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad]
1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained
with a discretization of [119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control
and state variables are represented in Figure 9
Abstract and Applied Analysis 15
10 20 30 40 50 60
1
2
3
4
5
6
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
4
5
6
7
8
9
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
6
(d) 1205792
10 20 30 40 50 60minus5
5
10
15
20
(e) 1199062
Figure 9 Control and state variables of the optimal solution of problem 4 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 8
derivative multipliers is able to tackle both integrable andnonintegrable differential constraints of the dynamic modelsof underactuated planar horizontal robot manipulators withtwo revolute joints This method can be seamlessly appliedin the presence of additional constraints on the mechanicalsystem
References
[1] A De Luca S Iannitti R Mattone and G Oriolo ldquoUnderactu-ated manipulators control properties and techniquesrdquoMachineIntelligence and Robotic Control vol 4 no 3 pp 113ndash125 2002
[2] G A Bliss Lectures on the Calculus of Variations University ofChicago Press Chicago Ill USA 1946
[3] J Gregory and C Lin Constrained Optimization in the Calculusof Variations and Optimal Control theory Chapman amp Hall1996
[4] W-S Koon and J E Marsden ldquoOptimal control for holonomicand nonholonomic mechanical systems with symmetry andLagrangian reductionrdquo SIAM Journal on Control and Optimiza-tion vol 35 no 3 pp 901ndash929 1997
[5] A M Bloch Nonholonomic Mechanics and Control SpringerNew York NY USA 2003
[6] I I Hussein and A M Bloch ldquoOptimal control of underactu-ated nonholonomic mechanical systemsrdquo IEEE Transactions onAutomatic Control vol 53 no 3 pp 668ndash682 2008
[7] X Z Lai J H She S X Yang andMWu ldquoComprehensive uni-fied control strategy for underactuated two-link manipulatorsrdquoIEEE Transactions on Systems Man and Cybernetics B vol 39no 2 pp 389ndash398 2009
[8] J P Ordaz-Oliver O J Santos-Sanchez and V Lopez-MoralesldquoToward a generalized sub-optimal control method of underac-tuated systemsrdquo Optimal Control Applications amp Methods vol33 no 3 pp 338ndash351 2012
16 Abstract and Applied Analysis
[9] R Seifried ldquoTwo approaches for feedforward control andoptimal design of underactuatedmultibody systemsrdquoMultibodySystem Dynamics vol 27 no 1 pp 75ndash93 2012
[10] M Buss O von Stryk R Bulirsch and G Schmidt ldquoTowardshybrid optimal controlrdquo atmdashAutomatisierungstechnik vol 48no 9 pp 448ndash459 2000
[11] M Buss M Glocker M Hardt O von Stryk R Bulirsch andG Schmidt ldquoNonlinear hybrid dynamical systems modelingoptimal control and applicationsrdquo in Modelling Analysis andDesign of Hybrid Systems S Engell G Frehse and E SchniederEds vol 279 of Lecture Notes in Control and InformationScience pp 331ndash335 Springer 2002
[12] G Oriolo and Y Nakamura ldquoControl of mechanical systemswith second-order nonholonomic constraints underactuatedmanipulatorsrdquo in Proceedings of the 30th IEEE Conference onDecision and Control pp 2398ndash2403 December 1991
[13] T J Tarn M Zhang and A Serrani ldquoNew integrability condi-tions for differential constraintsrdquo Systems and Control Lettersvol 49 no 5 pp 335ndash345 2003
[14] H J Sussmann ldquoA general theorem on local controllabilityrdquoSIAM Journal on Control and Optimization vol 25 no 1 pp158ndash194 1987
[15] F Bullo A D Lewis and K M Lynch ldquoControllable kinematicreductions for mechanical systems concepts computationaltools and examplesrdquo in Proceedings of International Symposiumon Mathematical Theory of Networks and Systems 2002
[16] F Bullo and A D Lewis ldquoLow-order controllability and kine-matic reductions for affine connection control systemsrdquo SIAMJournal on Control andOptimization vol 44 no 3 pp 885ndash9082006
[17] A D Lewis and R MMurray ldquoConfiguration controllability ofsimple mechanical control systemsrdquo SIAM Journal on Controland Optimization vol 35 no 3 pp 766ndash790 1997
[18] F Bullo and K M Lynch ldquoKinematic controllability for decou-pled trajectory planning in underactuatedmechanical systemsrdquoIEEE Transactions on Robotics and Automation vol 17 no 4 pp402ndash412 2001
[19] M R Hestenes Calculus of Variations and Optimcl ControlTheory John Wiley amp Sons 1966
[20] J Gregory ldquoA new systematic method for efficiently solvingholonomic (and nonholonomic) constraint problemsrdquo Analysisand Applications vol 8 no 1 pp 85ndash98 2010
[21] J Gregory and R S Wang ldquoDiscrete variable methods forthe m-dependent variable nonlinear extremal problem in thecalculus of variationsrdquo SIAM Journal onNumerical Analysis vol27 no 2 pp 470ndash487 1990
[22] Wolfram Research 2012[23] J J More and D J Thuente ldquoLine search algorithms with guar-
anteed sufficient decreaserdquo ACM Transactions on MathematicalSoftware vol 20 no 3 pp 286ndash307 1994
Figure 7 Control and state variables of the optimal solution of problem 3 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 0 [rads] 120579
2(119905119868) = 0 [rads] 119906
2(119905119868) = 0Nm 120579
1(119905119865) = free
1205792(119905119865) = 120587 [rad] 120579
1(119905119865) = 0 [rads] and 120579
2(119905119865) = 0 [rads]The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectivelyThe corresponding
sequence of configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 6
Figure 8 Sequence of configurations of the robot manipulator at times 11989632 with 119896 = 0 1 32 corresponding to the optimal solutionof problem 4 an initial value problem for an underactuated 119877119877 planar robot manipulator with boundary conditions 120579
1(119905119868) = 0 [rad]
1205792(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad] 120579
1(119905119865) = free and 120579
2(119905119865) = 0 [rads] obtained
with a discretization of [119905119868 119905119865] into 64 intervals The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding control
and state variables are represented in Figure 9
Abstract and Applied Analysis 15
10 20 30 40 50 60
1
2
3
4
5
6
(a) 1205791
10 20 30 40 50 60
05
1
15
2
25
3
(b) 1205792
10 20 30 40 50 60
4
5
6
7
8
9
(c) 1205791
10 20 30 40 50 60
1
2
3
4
5
6
(d) 1205792
10 20 30 40 50 60minus5
5
10
15
20
(e) 1199062
Figure 9 Control and state variables of the optimal solution of problem 4 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 8
derivative multipliers is able to tackle both integrable andnonintegrable differential constraints of the dynamic modelsof underactuated planar horizontal robot manipulators withtwo revolute joints This method can be seamlessly appliedin the presence of additional constraints on the mechanicalsystem
References
[1] A De Luca S Iannitti R Mattone and G Oriolo ldquoUnderactu-ated manipulators control properties and techniquesrdquoMachineIntelligence and Robotic Control vol 4 no 3 pp 113ndash125 2002
[2] G A Bliss Lectures on the Calculus of Variations University ofChicago Press Chicago Ill USA 1946
[3] J Gregory and C Lin Constrained Optimization in the Calculusof Variations and Optimal Control theory Chapman amp Hall1996
[4] W-S Koon and J E Marsden ldquoOptimal control for holonomicand nonholonomic mechanical systems with symmetry andLagrangian reductionrdquo SIAM Journal on Control and Optimiza-tion vol 35 no 3 pp 901ndash929 1997
[5] A M Bloch Nonholonomic Mechanics and Control SpringerNew York NY USA 2003
[6] I I Hussein and A M Bloch ldquoOptimal control of underactu-ated nonholonomic mechanical systemsrdquo IEEE Transactions onAutomatic Control vol 53 no 3 pp 668ndash682 2008
[7] X Z Lai J H She S X Yang andMWu ldquoComprehensive uni-fied control strategy for underactuated two-link manipulatorsrdquoIEEE Transactions on Systems Man and Cybernetics B vol 39no 2 pp 389ndash398 2009
[8] J P Ordaz-Oliver O J Santos-Sanchez and V Lopez-MoralesldquoToward a generalized sub-optimal control method of underac-tuated systemsrdquo Optimal Control Applications amp Methods vol33 no 3 pp 338ndash351 2012
16 Abstract and Applied Analysis
[9] R Seifried ldquoTwo approaches for feedforward control andoptimal design of underactuatedmultibody systemsrdquoMultibodySystem Dynamics vol 27 no 1 pp 75ndash93 2012
[10] M Buss O von Stryk R Bulirsch and G Schmidt ldquoTowardshybrid optimal controlrdquo atmdashAutomatisierungstechnik vol 48no 9 pp 448ndash459 2000
[11] M Buss M Glocker M Hardt O von Stryk R Bulirsch andG Schmidt ldquoNonlinear hybrid dynamical systems modelingoptimal control and applicationsrdquo in Modelling Analysis andDesign of Hybrid Systems S Engell G Frehse and E SchniederEds vol 279 of Lecture Notes in Control and InformationScience pp 331ndash335 Springer 2002
[12] G Oriolo and Y Nakamura ldquoControl of mechanical systemswith second-order nonholonomic constraints underactuatedmanipulatorsrdquo in Proceedings of the 30th IEEE Conference onDecision and Control pp 2398ndash2403 December 1991
[13] T J Tarn M Zhang and A Serrani ldquoNew integrability condi-tions for differential constraintsrdquo Systems and Control Lettersvol 49 no 5 pp 335ndash345 2003
[14] H J Sussmann ldquoA general theorem on local controllabilityrdquoSIAM Journal on Control and Optimization vol 25 no 1 pp158ndash194 1987
[15] F Bullo A D Lewis and K M Lynch ldquoControllable kinematicreductions for mechanical systems concepts computationaltools and examplesrdquo in Proceedings of International Symposiumon Mathematical Theory of Networks and Systems 2002
[16] F Bullo and A D Lewis ldquoLow-order controllability and kine-matic reductions for affine connection control systemsrdquo SIAMJournal on Control andOptimization vol 44 no 3 pp 885ndash9082006
[17] A D Lewis and R MMurray ldquoConfiguration controllability ofsimple mechanical control systemsrdquo SIAM Journal on Controland Optimization vol 35 no 3 pp 766ndash790 1997
[18] F Bullo and K M Lynch ldquoKinematic controllability for decou-pled trajectory planning in underactuatedmechanical systemsrdquoIEEE Transactions on Robotics and Automation vol 17 no 4 pp402ndash412 2001
[19] M R Hestenes Calculus of Variations and Optimcl ControlTheory John Wiley amp Sons 1966
[20] J Gregory ldquoA new systematic method for efficiently solvingholonomic (and nonholonomic) constraint problemsrdquo Analysisand Applications vol 8 no 1 pp 85ndash98 2010
[21] J Gregory and R S Wang ldquoDiscrete variable methods forthe m-dependent variable nonlinear extremal problem in thecalculus of variationsrdquo SIAM Journal onNumerical Analysis vol27 no 2 pp 470ndash487 1990
[22] Wolfram Research 2012[23] J J More and D J Thuente ldquoLine search algorithms with guar-
anteed sufficient decreaserdquo ACM Transactions on MathematicalSoftware vol 20 no 3 pp 286ndash307 1994
Figure 9 Control and state variables of the optimal solution of problem 4 an initial value problem for an underactuated 119877119877 planar robotmanipulator with boundary conditions 120579
1(119905119868) = 0 [rad] 120579
2(119905119868) = 0 [rad] 120579
1(119905119868) = 5 [rads] 120579
2(119905119868) = 0 [rads] 120579
1(119905119865) = free 120579
2(119905119865) = 120587 [rad]
1205791(119905119865) = free and 120579
2(119905119865) = 0 [rads] The initial and final times are 119905
119868= 0 and 119905
119865= 1 [s] respectively The corresponding sequence of
configurations of the robot manipulator at times 119896(132) with 119896 = 0 1 32 is represented in Figure 8
derivative multipliers is able to tackle both integrable andnonintegrable differential constraints of the dynamic modelsof underactuated planar horizontal robot manipulators withtwo revolute joints This method can be seamlessly appliedin the presence of additional constraints on the mechanicalsystem
References
[1] A De Luca S Iannitti R Mattone and G Oriolo ldquoUnderactu-ated manipulators control properties and techniquesrdquoMachineIntelligence and Robotic Control vol 4 no 3 pp 113ndash125 2002
[2] G A Bliss Lectures on the Calculus of Variations University ofChicago Press Chicago Ill USA 1946
[3] J Gregory and C Lin Constrained Optimization in the Calculusof Variations and Optimal Control theory Chapman amp Hall1996
[4] W-S Koon and J E Marsden ldquoOptimal control for holonomicand nonholonomic mechanical systems with symmetry andLagrangian reductionrdquo SIAM Journal on Control and Optimiza-tion vol 35 no 3 pp 901ndash929 1997
[5] A M Bloch Nonholonomic Mechanics and Control SpringerNew York NY USA 2003
[6] I I Hussein and A M Bloch ldquoOptimal control of underactu-ated nonholonomic mechanical systemsrdquo IEEE Transactions onAutomatic Control vol 53 no 3 pp 668ndash682 2008
[7] X Z Lai J H She S X Yang andMWu ldquoComprehensive uni-fied control strategy for underactuated two-link manipulatorsrdquoIEEE Transactions on Systems Man and Cybernetics B vol 39no 2 pp 389ndash398 2009
[8] J P Ordaz-Oliver O J Santos-Sanchez and V Lopez-MoralesldquoToward a generalized sub-optimal control method of underac-tuated systemsrdquo Optimal Control Applications amp Methods vol33 no 3 pp 338ndash351 2012
16 Abstract and Applied Analysis
[9] R Seifried ldquoTwo approaches for feedforward control andoptimal design of underactuatedmultibody systemsrdquoMultibodySystem Dynamics vol 27 no 1 pp 75ndash93 2012
[10] M Buss O von Stryk R Bulirsch and G Schmidt ldquoTowardshybrid optimal controlrdquo atmdashAutomatisierungstechnik vol 48no 9 pp 448ndash459 2000
[11] M Buss M Glocker M Hardt O von Stryk R Bulirsch andG Schmidt ldquoNonlinear hybrid dynamical systems modelingoptimal control and applicationsrdquo in Modelling Analysis andDesign of Hybrid Systems S Engell G Frehse and E SchniederEds vol 279 of Lecture Notes in Control and InformationScience pp 331ndash335 Springer 2002
[12] G Oriolo and Y Nakamura ldquoControl of mechanical systemswith second-order nonholonomic constraints underactuatedmanipulatorsrdquo in Proceedings of the 30th IEEE Conference onDecision and Control pp 2398ndash2403 December 1991
[13] T J Tarn M Zhang and A Serrani ldquoNew integrability condi-tions for differential constraintsrdquo Systems and Control Lettersvol 49 no 5 pp 335ndash345 2003
[14] H J Sussmann ldquoA general theorem on local controllabilityrdquoSIAM Journal on Control and Optimization vol 25 no 1 pp158ndash194 1987
[15] F Bullo A D Lewis and K M Lynch ldquoControllable kinematicreductions for mechanical systems concepts computationaltools and examplesrdquo in Proceedings of International Symposiumon Mathematical Theory of Networks and Systems 2002
[16] F Bullo and A D Lewis ldquoLow-order controllability and kine-matic reductions for affine connection control systemsrdquo SIAMJournal on Control andOptimization vol 44 no 3 pp 885ndash9082006
[17] A D Lewis and R MMurray ldquoConfiguration controllability ofsimple mechanical control systemsrdquo SIAM Journal on Controland Optimization vol 35 no 3 pp 766ndash790 1997
[18] F Bullo and K M Lynch ldquoKinematic controllability for decou-pled trajectory planning in underactuatedmechanical systemsrdquoIEEE Transactions on Robotics and Automation vol 17 no 4 pp402ndash412 2001
[19] M R Hestenes Calculus of Variations and Optimcl ControlTheory John Wiley amp Sons 1966
[20] J Gregory ldquoA new systematic method for efficiently solvingholonomic (and nonholonomic) constraint problemsrdquo Analysisand Applications vol 8 no 1 pp 85ndash98 2010
[21] J Gregory and R S Wang ldquoDiscrete variable methods forthe m-dependent variable nonlinear extremal problem in thecalculus of variationsrdquo SIAM Journal onNumerical Analysis vol27 no 2 pp 470ndash487 1990
[22] Wolfram Research 2012[23] J J More and D J Thuente ldquoLine search algorithms with guar-
anteed sufficient decreaserdquo ACM Transactions on MathematicalSoftware vol 20 no 3 pp 286ndash307 1994
[9] R Seifried ldquoTwo approaches for feedforward control andoptimal design of underactuatedmultibody systemsrdquoMultibodySystem Dynamics vol 27 no 1 pp 75ndash93 2012
[10] M Buss O von Stryk R Bulirsch and G Schmidt ldquoTowardshybrid optimal controlrdquo atmdashAutomatisierungstechnik vol 48no 9 pp 448ndash459 2000
[11] M Buss M Glocker M Hardt O von Stryk R Bulirsch andG Schmidt ldquoNonlinear hybrid dynamical systems modelingoptimal control and applicationsrdquo in Modelling Analysis andDesign of Hybrid Systems S Engell G Frehse and E SchniederEds vol 279 of Lecture Notes in Control and InformationScience pp 331ndash335 Springer 2002
[12] G Oriolo and Y Nakamura ldquoControl of mechanical systemswith second-order nonholonomic constraints underactuatedmanipulatorsrdquo in Proceedings of the 30th IEEE Conference onDecision and Control pp 2398ndash2403 December 1991
[13] T J Tarn M Zhang and A Serrani ldquoNew integrability condi-tions for differential constraintsrdquo Systems and Control Lettersvol 49 no 5 pp 335ndash345 2003
[14] H J Sussmann ldquoA general theorem on local controllabilityrdquoSIAM Journal on Control and Optimization vol 25 no 1 pp158ndash194 1987
[15] F Bullo A D Lewis and K M Lynch ldquoControllable kinematicreductions for mechanical systems concepts computationaltools and examplesrdquo in Proceedings of International Symposiumon Mathematical Theory of Networks and Systems 2002
[16] F Bullo and A D Lewis ldquoLow-order controllability and kine-matic reductions for affine connection control systemsrdquo SIAMJournal on Control andOptimization vol 44 no 3 pp 885ndash9082006
[17] A D Lewis and R MMurray ldquoConfiguration controllability ofsimple mechanical control systemsrdquo SIAM Journal on Controland Optimization vol 35 no 3 pp 766ndash790 1997
[18] F Bullo and K M Lynch ldquoKinematic controllability for decou-pled trajectory planning in underactuatedmechanical systemsrdquoIEEE Transactions on Robotics and Automation vol 17 no 4 pp402ndash412 2001
[19] M R Hestenes Calculus of Variations and Optimcl ControlTheory John Wiley amp Sons 1966
[20] J Gregory ldquoA new systematic method for efficiently solvingholonomic (and nonholonomic) constraint problemsrdquo Analysisand Applications vol 8 no 1 pp 85ndash98 2010
[21] J Gregory and R S Wang ldquoDiscrete variable methods forthe m-dependent variable nonlinear extremal problem in thecalculus of variationsrdquo SIAM Journal onNumerical Analysis vol27 no 2 pp 470ndash487 1990
[22] Wolfram Research 2012[23] J J More and D J Thuente ldquoLine search algorithms with guar-
anteed sufficient decreaserdquo ACM Transactions on MathematicalSoftware vol 20 no 3 pp 286ndash307 1994