Fast Trajectory Planning for VGT Manipulator via ... - SAGE Journals

International Journal of Advanced Robotic Systems

ARTICLE

Fast Trajectory Planning for VGT Manipulator via Convex Optimization Regular Paper

Yongsheng Yang1*, Bosen Jiang1 and Shiqiang Hu1 1 School of Aeronautics and Astronautics, Shanghai Jiao Tong University, Shanghai, China *Corresponding author(s) E-mail: [email protected] Received 03 May 2014; Accepted 03 March 2015 DOI: 10.5772/60506 © 2015 Author(s). Licensee InTech. This is an open access article distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

Abstract

l cross longeron length

Variable-geometry truss (VGT) can be used as joints in a large, lightweight, high load-bearing manipulator for many industrial applications. This paper introduces the modelling process of a multisection three-degree-freedom double octahedral VGT manipulator and proposes a new point-to-point trajectory planning algorithm for a VGT manipulator with a nonlinear convex optimization ap‐ proach. The trajectory planning problem is converted to a quadratic convex optimization problem, and if some parameters are appropriately chosen the proposed algo‐ rithm is proved to be of global convergence and to have a super-linear convergence rate. The effectiveness of the proposed algorithm is illustrated by numerical simulation.

O0 the coordinates of the centre of the bottom plane

Keywords VGT, Kinematic Model, Trajectory Planning

Ot the coordinates of the centre of the top plane ^ the normal vectors of the bottom plane n 0 ^ = n, n, n n t x y z

T

the normal vectors of the top plane

^ U mid-plane normal

r the length from O0 to the middle actuation plane along vector n^ 0 θi (i = 1, 2, 3) the cross longeron face angles

N the length from the midpoint of the three sides in the bottom to Gi(i=1, 2, 3) si , ci (i = 1, 2, 3)sinθi and cosθi

Symbols M the actuation plane

Pi , Qi , Ri (i = 1, 2, 3) intermediate variables to calculate θi

in inverse kinematic problem

Oti (i = 1, 2 … , n)Ot of the i -th module in the coordinate

li(i=1, 2, 3) length of the i-th actuator

system of the i -th module

Gi(i=1, 2, 3) the i-th node for the low actuation plane

^i = n i, n i, n i n t x y z

s VGT mid-plane joint offset

^ of the i -th module in the coordinate system (i = 1, 2 … , n)n t

l0 fixed batten length

T

of the i -th module

Int J Adv Robot Syst, 2015, 12:136 | doi: 10.5772/60506

1

Oi = O xi , O yi , Ozi

coordinate system

TO t

of the i -th module in the ground

^ (i = 1, 2 … , n)n ^ of the i -th module in the ground coordi‐ n i t

nate system

αi (i = 1, 2 … , n) the first deflection angle about y axis of the

i -th module

βi (i = 1, 2 … , n) the second deflection angle about z axis of

the i -th module

T i (i = 1, 2 … , n) the transformation matrix of the i -th

pk parameter used in quadratic programming subproblem ωk parameter used in quadratic programming subproblem dk d for the k -th iteration d the optimization variable of quadratic programming

subproblem

qk (d ) the objective function of quadratic programming

subproblem

X k X for the k -th iteration

module

λk = λk1, λk2, … , λk2n+1 λ for the k -th iteration

lsi (i = 1, 2 … , n) the length of the rod seen as the i -th module

αk the stepsize for the k -th iteration

of VGT

St = S xt , S yt , S zt

T

the ideal position wanted

lb the radius of the obstacle Ob the centre of the obstacle lsmax , lsmin the given maximum and minimum lengths of

lsi ki (i = 1, 2 … , n) the impact factors of the i -th rod

X the optimization variable of the optimization problem Ai constant matrix used to represent Oi by X f ( X ) the objective function

d *, λk*, X * the minimum value for dk , λk , X k

Pσ ( X ) the penalty function σ the penalty parameter σk the penalty parameter for the k -th iteration δ, τ parameters to calculate σk P '(X k , σk ; dk ) the directional derivative of P ( X k , σk ) along

direction dk

ε1, ε2 the parameters used to stop the iteration ρ the parameter to calculate the sizestep

gi (X ) the i -th constraint function

Er, Ea the relative error and the absolute error of the simulation result

L ( X , λ ) the Lagrangian function of the optimization

Bi , Ci , Di , Z matrixes used in the proof for Theorem 1

problems

λ = (λ 1, … , λ 2n+1) the Lagrange multiplier T

g(X ) the vector of the constraint function df the gradient of the objective function f ( X )

O zero matrix I unit matrix i

the columns from the (3i − 2) -th column of matrix Z to the (3i) -th column of matrix Z Z

d gi the gradient of the constraint function gi ( X ) H ( X ) = ∇ g(X )T the gradient of g ( X ) ∇ L ( X , λ ) the gradient of L ( X , λ ) ∇ X L ( X , λ ) the gradient of L ( X , λ ) about X ∇λ L ( X , λ ) the gradient of L ( X , λ ) about λ N ( X , λ ) the Jacobi matrix of ∇ L ( X , λ ) W ( X , λ ) the gradient of ∇ L ( X , λ ) ∇

2

XX

L ( X , λ ) the gradient of ∇ X L ( X , λ ) about X

2

∇ f the gradient of the objective function df ∇2 gi the gradient of the constraint function d gi k the iteration number zk parameter used in quadratic programming subproblem 2


1. Introduction Variable-geometry truss (VGT) manipulator is a kind of hyper-redundant manipulator with a very large degree of kinematic redundancy. It can be used in the fields of remote, unstructured and hazardous environments such as space, undersea exploration and mining. The VGT possesses a high stiffness-to-mass ratio and provides both structural integrity and function dexterity. Koryo Miura proposed the concept of VGT in 1984 [1] and several papers discussed the design of different structures for a VGT manipulator, such as Rafael AvileHs’s paper in 2000 [2], K. Hanahara’s paper in 2008 [3] and Sven Rost’s paper in 2011 [4]. In 1991, Frank Naccarato and Peter Hughes began to discuss the kinematic problem of a VGT manipulator [5], and further research on the kinematic

problem has been undertaken by Robert L. Williams II in 1994 [6] and 1998 [7], Yao Jin and Fang Hai-rong in 1995 [8] and Regina Sun-Kyung Lee in 2000 [9]. However, these papers are all focused on the forward and inverse prob‐ lems, and there are few papers discussing the trajectory planning of VGT manipulator with multiple modules.

There has previously been several papers discussing the trajectory planning of hyper-redundant manipulators, including David S. Lees’ paper in 1996 [10], Vasily Cher‐ nonozhkin’s paper in 2006 [11] and Chien-Chou Lin’s paper in 2012 [12]. However, these papers are only focused on the trajectory planning problem in the two-dimensional plane of the manipulators. The latest research is focused on threedimensional hyper-redundant manipulator trajectory planning, including papers by Maria da Graça Marcos in ed to VGT. 2010 [13] and in 2011 [14]; Samer Yahya also wrote a.paper on trajectory planning in 2011 [15]. However, these papers point-to-point trajectory planning e applied tomake VGT. . an the manipulator reach the given point by using ee-dimensional VGT manipulator extended Jacobi matrix. On one hand, it greatly increases a point-to-point trajectory planning the running time of the manipulators, but on the other, due ng a nonlinear convex optimization a three-dimensional VGT manipulator to the different structures between VGT and the hyperredundant manipulators mentioned above, the recursive ory planning problem is converted ed using a nonlinear convex optimization matrix cannot be applied to VGT.

lengths are changeable. We also use l0 to represent the lengths of the rods in bottom plane and top plane and use l to represent the lengths of the rest rods, both l0 and l are constant. The three actuators compose a plane, which is called the actuation plane, and a single VGT manipulator is symmetric about the plane. We use Gi (i = 1, 2, 3) to repre‐ sent the nodes for the actuation plane. In the actual situa‐ tion, due to the limitation of the mechanical structure, the actuation plane has thickness which is represented by s . There also needs some other parameters to figure out the kinematic problem. As is shown in Figure 2, we use O0 and Ot to represent the centre of the bottom plane and the centre of the top plane. n^ 0 and n^ t are used to represent the normal vectors of the bottom plane and the top plane; ^ U is used to represent the unit vector from the bottom plane to the top plane and is also the normal vector to the actuation plane. Finally, we introduce r to represent the length from O0 to the middle actuation plane along vector n^ 0.

optimization problem, and some rajectory planning problem is ifconverted In this paper, a point-to-point trajectory planning algo‐ onvex optimization problem, andVGT if some ropriately chosen, the proposed rithm for a three-dimensional manipulator model is proposed using a nonlinear convex optimization approach. appropriately chosen, the proposed ed to be of global convergence and The trajectory planning problem is converted to a quadratic eonvergence proved toconvex berate. ofoptimization global convergence andparameters are problem, and if some appropriately chosen, the proposed algorithm can be near convergence rate. as

proved to be of global convergence and to have a superlin‐ ear convergence rate. section, the follows. In the next

tlined as follows. In outlined the next section, the This paper isVGT as follows. In the next section, the double octahedral manipulator l of a double octahedral manipulator kinematic model ofVGT a double octahedral VGT manipulator 3 is devoted point-to-point is discussed. to Section 3 is devoted to point-to-point trajec‐ ection 3 is devoted to point-to-point tory planning for the VGT manipulator, and the simulation or manipulator, andand thethe isFigure ingthe for VGT the VGT manipulator, single VGT manipulator module result is shown in Section 4. The conclusion drawn in1 1The Figure 1. The single VGTmanipulator manipulator module Figure The single VGT module Section 5. own in Section 4. The conclusion is t is shown in Section 4. The conclusion is n 5. 2. Kinematic Model of a Double Octahedral VGT Module In this section, we introduce kinematic problems by using

del of a Double Octahedral VGT Module. a Double Octahedral VGT Module. the geometry relationship for the single module of the VGT

manipulator. The kinematic problems of a VGT module consist of three subproblems: the forward kinematic we introduce kinematic problems problem using problem, the inverse kinematic and the extensible oduce kinematic problems by by using elationship kinematic for theproblem. singleAtmodule ofthethe first, we give description of the ship for the single module of the VGT manipulator module. or. The kinematic problems of a VGT

he kinematic problems of a VGT of three2.1 subproblems: forward Description of the VGT the manipulator module three subproblems: the forward em, the inverse kinematic problem and In order to explain kinematic problems, at first we should he inverse kinematic problem andVGT manipulator inematic problem. first, we introduce theAt structure of the give single the module. atic problem. At first, we give the e VGT manipulator module. As is shown in Figure 1, a single VGT manipulator mod‐ T manipulator module.

ule is composed of three kinds of rods: actuators, top and 2 Parameters used for the kinematic problem Figure rods. The lengths of the Figure 2. Parameters usedfor for thethe kinematic problem used kinematic problem actuators’2 Parameters actuatorsmodule. are represented by li (i = 1, 2, 3) and the Figure manipulator

f the VGT manipulator module. bottom rods and other linked

GT

ain kinematic problems, at first we should tructure of the single VGTwe manipulator ematic problems, at first should

re of the single VGT manipulator

1,2,3 , which are the cross The other parameters Yongsheng Yang, Bosen Jiang and Shiqiang Hu: 3 longeron angles, used the Optimization intermediate Trajectory Planning forare VGT Manipulator via Convex 1,2,3 as , which are the cros The otherFastface parameters parameters. We also use to represent the length from longeron face angles, are used as the intermediat the midpoint of the three sides in the bottom to

Figure 2 Parameters used for the kinematic problem

1,2,3 , which are the cross The other parameters longeron face angles, are used as the intermediate The We other also parameters the cross 1, 2,represent 3), which arethe parameters. useθi (i =to length from 12l 2 - 3l02 c3 + c1 c3c1 + 2 longeron face angles, are used as the intermediate param‐ the midpoint of the three sides in the bottom to 3l0 eters. We also use N to represent the length from the the bottom to Gi (i = 1, can 2, 3) and /4 .in The symbols be seen in 1,2,3 andmidpoint of the three sides 2 2 ( ) 12l 2 - 3l02 Figure 3. N = l − l0 / 4 . The symbols can be seen in Figure 3. 2 2 2 -

12l0 l 2 - 3l03

12l 2 - 3l02 3l0

s3 s1

(8l - l0 - 4l3 ) = 0.

(1)

There are eight solutions for θi (i = 1, 2, 3) in (1), but by taking

the actual situation into consideration, one optimal solution for θi (i = 1, 2, 3) is selected, so θi (i = 1, 2, 3) are figured out. We realize that for the single module, O0 = 0, 0, 0 T, Ot can

be calculated with (2-4).

(

)

Ot = 2G1 × Uˆ + s Uˆ + O0 .

Uˆ =

(G - G ) ´ (G - G ) (G - G ) ´ (G - G ) 2

2

Figure 3 The intermediate Figure 3. The intermediateparameters parameters θi (i = 1, 2, 3) 1,2,3 By using these symbols and the symmetry of a single VGT

For the forward kinematic problem of a single VGT

realize the process, the cross longeron face angles

θi (i = 1, 2, 3) are used as the intermediate parameters. We

2

c1 + c2 -

-

2 0

12l - 3l 3l0 12l 2 - 3l02 2

3 0

12l0 l - 3l

2

c1c2 + 2

2 0

12l - 3l 3l0

s1s2

3

1

1

.

(3)

2

ì ü ï ï Ns 2 ï ï ïï l0 ïï N G2 = í - c2 ý , ï 4 3 2 ï ï l ï N 3 ï- 0 + c2 ï ïî 4 ïþ 2

3) and Ot . To the output module, the inputsproblem are li (i = 1, 2,for 2.2 Forward kinematic single VGTis module.

figured out by (1) [6],

1

3

ì ü Ns1 ï ï ï l0 ï G1 = í+ Nc1 ý , ï 2 3 ï ï ï 0 î þ

By using manipulator these symbols the symmetry of a single module, weand will introduce the forward, the inverse and module, the extensible problems inthe the forward, VGT manipulator wekinematic will introduce remaining part of Section 2. the inverse and the extensible kinematic problems in the Forward problem remaining2.2part ofkinematic Section 2. for single VGT module

For the forward kinematic problem of a single VGT use si , ci to represent sinθi , cosθi , and by using the geom‐ module, the areof single 1,2,3 theknown output is . etry inputs relationship module and and the l (i = 1, 2, 3) θ (i = 1, 2, 3) , the intermediate parameters can be i To realizei the process, the cross longeron face angles

1

(2)

ì ü ï ï Ns 3 ï ï ïï l0 N ïï G3 = í - c3 ý . ï4 3 2 ï ïl 3N ï ï0 c ï 2 3 þï îï 4

(4)

By using (1-4), the forward kinematic problem for the single VGT module is solved.

(8l 2 - l02 - 4l12 ) = 0,

2.3 Inverse kinematic problem for single VGT module 2

c2 + c3 -

2 0

12l - 3l 3l0

2

c2 c3 + 2

2 0

12l - 3l 3l0

s2 s3

For the inverse kinematic problem of a single VGT module, the input is Ot and the outputs are li (i = 1, 2, 3). To realize

the process, the cross longeron face angles θi (i = 1, 2, 3) are

-

4

12l 2 - 3l02 12l0 l 2 - 3l03

(8l 2 - l02 - 4l22 ) = 0,


also used as the intermediate parameters. By considering the relationship of the vectors in Figure 2, n^ t and r can be calculated by (5),

uuuuuv æ O0Ot nˆ t = ç 2nˆ 0 × ç Ot - O0 è

2

uuuuuv ö OO 0 t ÷ ÷ Ot - O0 ø

there is only one solution left, and by using the solution, Gi (i = 1, 2, 3) can be calculated by (4) and then li (i = 1, 2, 3) is

- nˆ 0 .

figured out as follows:

2

l1 = G2 - G1 , 2

2

r=

Ot - O0 uuuuuv2 . 2nˆ 0 × O0Ot

l2 = G3 - G2 ,

(5)

2

l3 = G1 - G3 .

(7)

2

where n^ 0 is 1, 0, 0

T

for single VGT module. To figure out

^ = n, n, n θi (i = 1, 2, 3), we assume n t x y z

T.

are calculated by (6) [6].

So, θi (i = 1, 2, 3)

By using (4-7), li (i = 1, 2, 3) can be figured out by Ot and the inverse kinematic problem for a single VGT module is solved. 2.4 Extensible kinematic problem for multiple VGT modules

q i = 2tan

-Qi ± Pi 2 + Qi 2 - Ri 2 -1 Ri - Pi

, i = 1,2,3.

(6)

Sections 2.2 and 2.3 discuss kinematic problems for a single VGT module, and this section extends the kinematic problem to multiple VGT modules. In the calculation of the extensible kinematic problem, we use Oti and n^ it to represent Ot and n^ t of the i -th module in

where

the local coordinate system of the i -th module. Moreover, we use Oi and n^ i to represent Ot and n^ t of the i -th module

P1 = Nny ,

in the ground coordinate system. We set the initial condi‐ tions as follows.

Q1 = N (1 + nx )

T

R1 = -

ny L0 2 3

- r ( 1 + nx ) + s

Ot0 = O0 = éë0, 0, 0 ùû ,

1 + nx , 2

T

nˆ t0 = nˆ 0 = éë1, 0, 0 ùû , é1 0 0ù ê ú T0 = ê0 1 0 ú . ê0 0 1 ú ë û

æ1 3 ö P2 = - N ç ny n ÷, ç2 2 z ÷ø è Q2 = N ( 1 + nx ) ,

Ot1 = O1 ,

nˆ t1 = nˆ 1 ,

ny L0

nL 1 + nx R2 = - z 0 - r ( 1 + nx ) + s , 4 2 4 3 æ1 3 ö P3 = - N ç ny + n ÷, ç2 2 z ÷ø è

To figure out the transformation matrix for multiple modules, we introduce the new variables αi (i = 1, … , n) and

βi (i = 1, … , n) which are defined in Figure 4 and can be

calculated by (8):

æ ni a i = arctan ç zi çn è x

Q3 = N ( 1 + nx ) ,

R3 =

ny L0 4 3

+

æ nyi b i = arctan ç ç ni 2 + ni 2 z è x

nz L0 1 + nx - r ( 1 + nx ) + s . 4 2

There are eight different combinations for θi according to

(6), but by taking the actual situation into consideration,

where

nxi , nyi , nzi

T

ö ÷÷ , ø

i = 1,¼ , n,

ö ÷ , i = 1,¼ , n. ÷ ø

(8)

^ i (i = 1, 2, … , n). By using the =n t

method developed by Craig in 1989 [16], we can calculate the transformation matrix from (9).

Yongsheng Yang, Bosen Jiang and Shiqiang Hu: Fast Trajectory Planning for VGT Manipulator via Convex Optimization

5

o figure out the transformation matrix for multiple modules, we introduce the new variables 1, … , nd 1, … , which are defined in Figure 2 4 and ∙ can ‖ e calculated by (8): … ormation matrix for multiple … æ e new variables 1, … , nˆ i = ç 2nˆ i -1 × ç è

re defined in Figure 4 and can

… …

,

, ‖ ‖ ‖ (1 , Then, by using (4-7), , , from uuuuuuuv uuuuuuuv , -are -th out, an ö Omodule OO O b ± bfigured - 4 ac ÷ - nˆ , 1, … , . is ÷ O - O for multiple 2a O - O problem modules ø i -1

i

i

i -1 2

i

2

i -1

i -1

i -1 2

i

from first module to th Then, by using (4-7), n,ˆ 3. VGT Trajectory = (,TPoint-to-Point T¼ T ) nˆ the , -th module are figured out, and the inverse kinemat problem for multiple section, introduce the O =modules ¼Tthis -solved. O ) + O we , (T TIn ) (Ois trajectory planning for the n 3. Point-to-Point VGT Trajectory Planning. = 1,¼ , n ) . programming ( inonlinear (11) approac -1

i t

0 1

0 1

i -1

i -1

i

-1

i t

i

i -1

0 t

Then, by using (4-7), l1, l2, l3 from the first module to the In this section, we introduce the solution of point-to-poin 3.1 Description of the problem. n -th module are figured out, and the inverse kinematic trajectory planning formodules the is solved. n-module VGT with problem for multiple For simplicity, the -th module nonlinear programming approach. 3. Point-to-Point VGT Trajectory Planning rod, and the length of the rod is: In this section, we introduce the solution of point-to-point

3.1 Description of the problem. trajectory planning for the n-module VGT with a nonlinear programming approach. ‖ ‖ , Figure 4. Details for extensible kinematic problem gure 4 Details for extensible kinematic problem For simplicity, the -th of VGT can be seen as 3.1 Description of the module problem écosa cosb - sinb sina cosb ù rod, and the length of the The planning problem rodtrajectory is: of VGT can be seen as a rod, For simplicity, the i -th module ê ú T = , cos a sin b cos b sin a sin b ê ú 1,(9)… , and , the length of the rod is: i

i

i

i

i

ê - sina i ë ( i = 1,2,¼, n) .

i

0

i

i

i

i

i

cosa i

ú û

‖

kinematic problem

‖ , (8) ls = O - O , i = 1,¼, n. i

,

i

i - 12

1, … ,

1, … , . The trajectory planning problem can be shown in Figure 5.in Figure The trajectory planning problem can be shown

To the forward kinematic problem of the i -th VGT module, we can use l1, l2, l3 from the first module to the i -th

1, … , ,

module to figure out Otj ( j = 1, 2, … , i) and n^ jt( j = 1, 2, .., i)

by (2-5). By (8) and (9), T j ( j = 1, 2, .., i) is figured out from ^ j ( j = 1, 2, .., i). Therefore, we can calculate O and n ^ by (10): n t i i

(8) … , where , , 1,2, . By using the , 1, … , . method developed by Craig in 1989 [16], we can calculate O = T matrix T ¼T O +from T T ¼T (9). O +¼+ T O , he transformation i

0 1

i -1

i t

0 1

i-2

nˆ = T T ¼T nˆ 1,2, … , . By using the g in 1989 [16], we can calculate 0 ( i = 1,2,¼, n) . rom (9). i

0 1

i -1 t

0

1 t

i i -1 t

(9) (10)

1,2, … ,

.

Figure 5 Trajectory planning problem

To the inverse kinematic problem for multiple modules, at If we use to represent the id o the forward kinematic 2, … , n) by theof first we should calculate Oi (i = 1,problem giventhe ideal -th VGT can represent the trajectory plann position, and this problem, which is called a point-to-point module, we can use , , from (9) the first module to the trajectory planning problem, will be discussed in the next planning problem with obstacle h modulesection. to After figure , them andFigure 5. Trajectory Oi (i = 1, 2, … , n), 1,2, calculating weFigure can… use 5 Trajectory planning problem with obstacle 1,2, …i , out . ^ i (i = 1, 2, … , n) by figure out 2, … , (9), n) and n we use St to represent the ideal position wanted, we can t (i = 1,and t 2, . . , byto(2-5). ByO(8) 1,2, . . , is Iffigured ‖ as follows,‖ (8-9,11): represent the trajectory planning problem 1,2, . . , . Therefore, we can calculate ut from If we use to represent the ideal position wanted, w ic problem of the -th VGT Int J Adv Robot Syst, 2015, 12:136 | doi: 10.5772/60506 6 nd (10): can represent the trajectory planning problem as follows , . . fromby the first module to the , ut 1,2, … , and … … ⋯ , ,

min f ( X ) = St - On S

2 2

n

+ åki lsi 2 , i = 1,2,¼ , n.

St - On

2

Oi - Ob

2

2

= X T AnT An X - 2StT An X + StT St .

(15)

= X T AiT Ai X - 2ObT Ai X + ObT Ob .

(16)

i =1

2

2

s.t. lsn £ lsmax ,

2

Problem (12) finally becomes 2

2

ls1 ³ lsmin ,

lsi + 12 ³ lsi 2 ,

Oi - Ob

2 2

min f ( X )

i = 1,3,¼ , n - 1.

- lb 2 ³ 0,

i = 1,2,¼ , n.

n é ù T = X T ê AnT An + åki ( Ai - Ai -1 ) ( Ai - Ai -1 ) ú X i =1 ëê ûú

(12)

-2StT An X + StT St ,

where lsmax and lsmin are the given maximum and minimum

lengths of lsi and are determined in accordance with

s.t. g j ( X ) ³ 0,

module structure parameters via a single module forward kinematic calculation in Section 2, ki (i = 1, 2, … , n) are the weighing

factors

of

each rod and T is the final output.

X = (O1)T , (O2)T , … , (Oi )T , … (On )T

Remark 1: In the proposed trajectory planning problem, we wish that the distance between St and On is minimized. If

i = 1,¼ , n

j = 1,¼ ,2n + 1

(17)

where g1 ( X ) = - X T ( An - An -1 )

T

(A

- An -1 ) X + lsmax 2 ,

n

the obtained distance between St and On is zero, that means

the VGT reaches the desired point in the space. Moreover, we set the constraints such that the lengths of the lower rods are less than the upper ones. In this formulation, the proposed optimization problem can be cast into a convex problem. Furthermore, consider the structure of a single VGT module: the length of every rod should be in certain range, so these constraints are also added to the formula‐ tion. Finally, we add an obstacle constraint in the proposed optimization problem to give VGT the capability to avoid the obstacle area.

g2 ( X ) = X T A1T A1X - lsmin 2 ,

gl ( X ) = X T [( Al -1 - Al - 2 )

T

- ( Al - 2 - Al - 3 )

T

(A

l-2

(A

l -1

- Al - 2 )

- Al - 3 )]X ,

l = 3,¼ , n + 1 3.2 Trajectory planning algorithm gm ( X ) = X T Am - n -1T Am - n -1X - ObT Am - n -1X

Assume matrixes Ai ∈ R 3×3n and ìï éO , O ,¼ , I ,¼ , O ù , i = 1,2,.., n. û Ai = í ë 3´ 3 n O , i = 0. ïî

(13)

0 0 0 1 0 0 where O = 0 0 0 , I = 0 1 0 and I is the i -th matrix to Ai 0 0 0 0 0 1 when i ≠ 0, so Oi = Ai ⋅ X and

lsi 2 = X T ( Ai - Ai -1 )

T

Meanwhile

( A - A ) X, i

i -1

i = 1,2,¼n.

(14)

+ObT Ob - lb2 , m = n + 2,¼ ,2n + 1 Because f (X ) and gj (X ) are all second-order continuously

differentiable real functions, the Lagrangian function of problem (17) is L(X,l ) = f (X) -

2n+1

ål g (X) = f ( X ) - l g ( X ). i =1 T

j

j

(18)


7

where λ = (λ 1, … , λ 2n+1)T is the Lagrange multiplier and g ( X ) = g1( X ), g2( X ), … , g2n+1( X ) T . The gradient of the

objective function f ( X ) is

and n

Ñ 2 f = 2 AnT An + 2åki ( Ai - Ai -1 ) ( Ai - Ai -1 ). T

(24)

i =1

n

df = 2[ AnT An + åki ( Ai - Ai -1 )

T

i =1

( A - A )]X - 2 A S . T n t

i -1

i

(19) 2n+1

ål Ñ g

and the gradient of the constraint functions gj ( X ) are dg1 = -2 ( An - An -1 )

= -2l 1 ( An - An -1 )

T

(A

- An -1 ) X , n

T

j

2

j

j =1

(A

n

- An -1 ) + 2l 2 A1T A1

n+1

+2ål l [( Al -1 - Al - 2 )

dg2 = 2 A1T A1X ,

(A

T

l -1

l=3

dgl = 2[( Al -1 - Al - 2 )

T

- ( Al - 2 - Al - 3 )

T

(A

l -1

(A

l-2

- Al - 2 )

- ( Al - 2 - Al - 3 )

T

- Al - 3 )]X , +2

2n+1

ål

m=n+ 2

l = 3,¼ , n + 1,

m

(A

- Al - 2 )

- Al - 3 )]

l-2

Am - n -1T Am - n -1 .

(25)

Then

dgm = 2 Am - n -1T Am - n -1X - 2 Am - n -1T Ob ,

W (X,l ) m = n + 2,¼ ,2n + 1.

(20) = Ñ2 f -

Assume

2n+1

ål Ñ g j

2

j =1

H ( X ) = Ñg ( X ) = [dg1 , dg2 ,¼ , dg2 n + 1 ]T . T

(

)

= 2 1 - l 2 n + 1 AnT An

and according to the Kuhn-Tucker (KT) condition, we have

(

+2 l 1 + kn éÑ X L ( X , l ) ù édf - H ( X )T l ) ù ú = 0. ú=ê ÑL ( X , l ) = ê ú ê Ñl L ( X , l ) ú ê -g ( X ) ë û ë û

j

(21)

n -1

)( A

- An -1 )

T

n

(

+2å[ ki + l i + 2 i =1

(A

n

- An -1 ) - 2l 2 A1T A1

)( A - A ) ( A - A ) T

i

i -1

i

i -1

So, the Jacobi matrix of ∇ L ( X , λ ) is -2l i + 2 ( Ai + 1 - Ai )

T

éW ( X , l ) - H ( X ) N (X,l ) = ê 0 êë - H ( X )

T

ù ú, úû

i +1

- Ai ) - 2l i + n + 1 AiT Ai ].

(26)

(22) In general, we use the Newton method to figure out nonlinear equation (21), so to the given point zk = (X k , λk ),

where from (18)

the format for the Newton method is

W ( X , l ) = Ñ 2 XX L ( X , l ) = Ñ2 f -

2n+1

z k + 1 = z k + pk ,

ål Ñ g . j =1

8

(A

j

2

j


(23) where pk = (dk , ωk ) satisfies

(27)

N ( X k , lk ) pk = -ÑL ( X k , lk ) ,

(28)

Now use λk = λk1, λk2, … , λk2n+1 to represent the Lagrange multiplier for the k -th iteration and use P '(X k , σk ; dk ) to

represent the directional derivative of P ( X k , σk ) along

that is

direction dk . Then, the point-to-point trajectory planning

algorithm is shown as follows. T éW X , l édf X - H ( X k )T lk ù - H ( X k ) ù é dk ù ê ( k k) úê ú = -ê ( k ) ú. ê H (X ) ú ëêwk ûú g( X k ) 0 ëê ûú k ë û

(29)

Step 1 Initialize St , lb, Ob.

By solving (29), a suitable descent direction can be obtained. In fact, equation (29) can be transformed to a quadratic programming subproblem. Consider the expansions of (29), where T

d

(30)

Step 3 Solve problem (31), find the optimal solution (dk , λk*). Step 4 If dk 1 ≤ ε1 and (g(X k ))−1 ≤ ε2, transfer to Step 8; other‐

wise, transfer to Step 5.

Step 5 Determine σk by (33). Step 6 Make mk the smallest non-negative integer which

T 1 T d W ( X k , lk ) d + Ñf ( X k ) d. 2

s.t. g ( X k ) + H ( X k ) d ³ 0.

X0 Î R 3 n , l0 Î R6 n + 3 ,h Î ( 0,1 / 2 ) , r Î ( 0,1) ,s 0 Î ( 0,1) ,

W (X 0, λ0) by (26). Make k : = 0.

Equation (30) is the KT condition for the quadratic pro‐ gramming subproblem (31) as follows min qk ( d ) =

Step 2 Initialize

0 ≤ ε1, ε2 ≤ 1, 0 < δ < 1, . Select appropriate ki and calculate

W ( X k , lk ) dk + df ( X k ) = H ( X k ) (wk + lk ) , g ( X k ) + H ( X k ) dk = 0.

Algorithm 1:

satisfies the following inequality

(31)

(

)

P X k + r mdk ,s k - P ( X k ,s k ) £ hr m P' ( X k ,s k ; dk ) m

By solving a quadratic programming subproblem (31), we get the minimum point (d *, λk*). From (30), we can see that *

d = dk is the search direction for the k -th iteration of

problem (17) and λk +1 = λk + αk ωk = λk + αk (λk* − λk ) is the

Lagrange multiplier for the (k + 1)-th iteration of problem (17) where αk is the stepsize for the k -th iteration of problem (17).

The search direction dk calculated by (31) is used to be the

descent direction of penalty function (32),

make αk : = ρ k , X k +1 : = X k + αk dk , λk +1 : = λk + αk (λk* − λk ). Step 7 Calculate W (X k +1, λk +1) by (26) and make k : = k + 1, transfer to Step 3.

Step 8 Make X = X k and set O0, n^ 0 and obtain Oi by X = O1T , O2T , … , Oi T , … On T

T.

Make i : = 1.

Step 9 If i > n , break; Else, Use Oi and Oi−1 to calculate Oti by

(11), transfer to Step 10.

Step 10 Use Oti to calculate l1, l2, l3 of the i -th module by

(4-7). Update i : = i + 1, transfer to step 9. é ù Ps ( X ) = f ( X ) + s -1 ê å éë gi ( X ) ùû ú . úû ëê i =1 2n+1

(32)

where gi ( X ) − = max {0, − gi (S )} and penalty parameter σ > 0.

The penalty parameter σ is obtained by the following rules

Remark 2: Because of the particularity of the VGT trajectory planning problem, the original problem is converted to a quadratic convex optimization problem, and in Algorithm 1 the Hessian matrix of subproblem (31) is W (X k , λk ),

which can be written in simple form. It can be shown that W (X k , λk ) is a positive definite by appropriately choosing

ki ; Algorithm1 is convergent and of superlinear conver‐

ìï s k -1 , s -1 ³ t + d . k -1 sk = í -1 s + t d 2 , ) k--11 < t + d . ïî( where δ > 0 is a constant and τ = λk ∞ .

gence.

(33) 3.3 Convergence and convergence rate analysis The global convergence of Algorithm 1 is given by the following theorem. Yongsheng Yang, Bosen Jiang and Shiqiang Hu: Fast Trajectory Planning for VGT Manipulator via Convex Optimization

9

Theorem 1: For the optimization problem (17), if existing constants 0 < K 1 ≤ K 2 which make W (X k , λk ) satisfies K1 d £ dT W ( X k , lk ) d £ K 2 d , "d. 2

2

2

2

(34)

4. Simulation Simulation of trajectory planning with an obstacle for threemodule VGT is shown in this part. The initial parameters needed are shown in Table 1. We use O x3, O y3, O z3 to represent the coordinates of O3

Algorithm 1 is of global convergence.

and use S xt , S yt , S zt to represent the coordinates of St . Then, the relative error Er is calculated by

Proof: See Appendix A. □ Remark 3: From theorem 1, it can be seen that equation (34) is needed to prove the convergence. However, in practice, it is difficult to be sure whether it is satisfied.

1 10 Er = 10 å j =1

æ Ox3 - Sxt çç è Sxt

2

ö æ Oy3 - Syt ÷÷ + çç ø è Syt

2

2

ö æ Oz3 - Szt ÷÷ + çç ø è Szt

ö ÷÷ . ø

To make W ( X k , λk ) easy to check in practice, a sufficient condition for global convergence of VGT trajectory plan‐ ning is given by the following theorem.

And if S yt = 0, make (O y3 − S yt ) / S yt = 0. The absolute error Ea is calculated by

Theorem 2: If ki (i = 1, 2, … , n ) in problem (17) satisfies Ea =

k1 + k2 - lk2 + lk4 - lkn + 2 > k2 - lk3 + lk4 ,

1 10 10 å j =1

k2 + k3 - lk3 - lkn + 3 + lk5

> k2 - lk3 + lk4 + k3 + lk5 - lk4 ,

kn + 1 - lkn - lk2 n + kn + lk1

(Ox

- Sxt ) + ( Oy3 - Syt ) + ( Oz3 - Szt ) . 2

3

2

2

Parameter

Value(cm)

s

82

l0

475

l

475

lsmax

850

lsmin

740

Table 1. Initial Parameters for VGT n k

n+1 k

> kn - 1 - l + l

1 k

n+1 k

+ kn + l - l

,

According to Algorithm 1, the steps of the simulation for a single point St = 2433, − 85, 100

1 k

n+1 k

kn + l - l

2n+1 k

-l

1 k

n+1 k

+ 1 > kn + l - l

,

T

are shown as follows.

Step 1 Initialize T

T

St = éë 2433, -85, 100 ùû , Ob = éë 400,400,400 ùû , lb = 300.

k j + k j + 1 - lkj + 1 - lkj + n + 1 + lkj + 3

Step 2 Set

> k j + lkj + 2 - lkj + 1 + k j + 1 + lkj + 3 - lkj + 2 ,

T

j = 3,4,¼ , n - 2.

X0 = éë800, 0, 0, 1600, 0, 0, 2400, 0, 0 ùû ,

(35)

Problem (17) is of global convergence.

T

l0 = éë0.003, 0.003, 0.003, 0.003, 0.003, 0.003, 0.003 ùû ,

Proof: See Appendix B. □

h = 0.1, r = 0.5,s 0 = 0.8, e 1 = 1 ´ e -4 ,e 2 = 1 ´ e -5 ,

For the convergence rate of Algorithm 1, we have the following theorem. Theorem 3: If ki (i = 1, 2, … , n ) in Algorithm 1 satisfies the

condition of theorem 2, problem (17) is of a superlinear convergence rate. Proof: See Appendix C. □ 10


δ = 0.05, ki = 0.01

and calculate W (X 0, λ0) by (26). Make

k : = 0.

Step 3 Solve problem (31), find the optimal solution (dk , λk*).

Step 4 If dk 1 ≤ ε1 and (g(Sk ))−1 ≤ ε2, transfer to Step 8; other‐

wise, transfer to Step 5.

Step 5 Determine σk by (33). Step 6 Make mk the smallest non-negative integer which

From the simulation result, the relative error is 0.54% and the absolute error is 8.15218mm. The simulation is run on a HP computer with a Core2 Duo 2.93-GHz CPU running the Windows 7 operating system and the software Matlab 2013b. For Algorithm 1, the average running time is 0.233s.

satisfies the following inequality

Postion for x-aixs 2600

(

)

P X k + r mdk ,s k - P ( X k ,s k ) £ hr m P' ( X k ,s k ; dk ) ,

Given position Simulation output

2550 2500

m

αk : = ρ k , X k +1 : = X k + αk dk , λk +1 : = λk + αk (λk* − λk ).

Step 7 Calculate W (X k +1, λk +1) by (26) and make k : = k + 1,

Position/(mm)

Make

transfer to Step 3.

2450 2400 2350 2300

Step 8 Set O0 = 0, 0, 0 , n^ 0 = 1, 0, 0 T , and obtain T

2250 2200

O1 = [808.31, -28.24, 33.22]T ,

1

2

3

4

5 6 Sampling points

7

8

9

10

7

8

9

10

7

8

9

10

Figure 6. The x-axis position results

O2 = [1616.61, -56.48, 66.45]T , Postion for y-aixs 150


O3 = [2424.92, -84.72, 99.67]T . 100

Make i : = 1. Step 9 If i > 3, break; Else, Use Oi and Oi−1 to calculate

by (11), transfer to Step 10.

Position/(mm)

50

Oti

Step 10 Use Oti to calculate l1, l2, l3 of the i -th module by

-50

(4-7). Update i : = i + 1, transfer to Step 9.

-100

To demonstrate the effectiveness of the proposed method, we choose ten different St in the simulation. The simulation outputs are calculated by (1-4, 10) and the results for ten points are shown in Table 2, Figure 6, Figure 7 and Figure 8. Given positions 1 2

2300, 2350,

− 150, − 126,

0

-150 1

2

3

4

Figure 7. The y-axis position results

Simulation output

− 150

T

− 126

T T

2292.36, 2342.19,

− 149.50 − 125.58

T

− 114.62,

− 104.65

T

− 84.72,

− 99.67

T

− 119.60

T

− 125.58,

3

2450,

− 115,

− 105

4

2500,

− 85,

− 100

T

2491.69,

5

2380, 50 ,

− 120

T

2372.09, 49.83,

2441.86,

Postion for z-aixs T

− 149.50,

5 6 Sampling points

150


100

6 7

2480, 150, 0 2560, 0, 0

2471.76, 149.50, 0

T

8

2240, 20,

9

2433, − 128, 59

10

T

− 20

2356, − 23, 123

2550, 0, 0 T T T

Table 2. Results for simulation

2232.56, 19.93, 2424.92, 2348.17,

− 127.57,

T

Position/(mm)

50

0

-50

T

-100

− 19.93

T

− 58.80

− 22.92, 122.59

T

-150 1

T

2

3

4

5 6 Sampling points

Figure 8. The z-axis position results


11

5. Conclusions The point-to-point trajectory planning for VGT is discussed in this paper. A new fast trajectory planning algorithm for a VGT manipulator via convex optimization is proposed. The trajectory planning problem is converted to a quadratic convex optimization problem, and if some parameters are appropriately chosen, the proposed algorithm is proved to be of global convergence and to have a superlinear conver‐ gence rate. Simulation results in Section 4 show that the proposed algorithm is effective for the trajectory planning of VGT. 6. References [1] Miura K (1984) Variable geometry truss concept. Institute of Space and Astronautical Science report. 1984 January 1. [2] Avilés R, et al (2000). A finite element approach to the position problems in open-loop variable geometry trusses. Finite elements in analysis and design 34.3-4: 233-255. [3] Hanahara K, and Tada Y (2008) Variable geometry Truss with SMA Wire Actuators (Basic Considera‐ tion on Kinematical and Mechanical Characteris‐ tics). 19th International Conference of Adaptive Structures and Technologies, October 6-9, 2008, Ascona Switzerland. [4] Rost S, et al. (2011) On the joint design and hydraulic actuation of octahedron VGT robot manipulators. 2011 IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM 2011). 2011 July 3. Budapest, Hungary. IEEE. pp. 92-97. [5] Naccarato F and Peter H (1991) Inverse kinematics of variable geometry truss manipulators. Journal of Robotic Systems 8.2: 249-266. [6] Williams RL (1994) Kinematic modeling of a double octahedral Variable Geometry Truss (VGT) as an extensible gimbal. NASA report. NASATM-109127, NAS 1.15: 109127. [7] Williams RL and Hexter ER (1998) Maximizing kinematic motion for a 3-dof VGT module. Journal of Mechanical Design 120.2: 333-336. [8] Yao J and Fang H (1995) Forward displacement analysis of the decahedral variable geometry truss manipulator. Robotics and Autonomous systems 15.3: 173-178. [9] Lee RS (2000) Kinematic control experiments with trussarm, a variable-geometry-truss manipulator [PhD thesis]. Institute for Aerospace Studies: University of Toronto.

12


[10] Lees DS and Chirikjian GS. (1996) A combinatorial approach to trajectory planning for binary manip‐ ulators. Robotics and Automation, IEEE Interna‐ tional Conference on Volume: 3: p2749-2754. 22 Apr 1996. Minneapolis, MN. [11] Chernonozhkin V (2006) Binary manipulator motion planning [PhD thesis]. Available from: http://www.mayr.in.tum.de/konferenzen/Jass06/ courses/5/Papers/Chernonozhkin.pdf. [12] Lin CC, Chen KC, Chuang WJ (2012) Motion planning using a memetic evolution algorithm for swarm robots. Int. J. Adv. Robot. Syst. 9: 25. [13] Maria da Graça Marcos, JA Tenreiro Machado and T-P Azevedo-Perdicoúlis (2010). An evolutionary approach for the motion planning of redundant and hyper-redundant manipulators. Nonlinear Dynam‐ ics 60.1-2: 115-129. [14] Yahya S, Moghavvemi M and Mohamed HAF (2011). Geometrical approach of planar hyperredundant manipulators: Inverse kinematics, path planning and workspace. Simulation Modelling Practice and Theory 19.1: 406-422. [15] Maria da Graça Marcos, JA Tenreiro Machado and T-P Azevedo-Perdicoúlis (2011). A fractional approach for the motion planning of redundant and hyper-redundant manipulators. Signal Processing 91.3: 562-570. [16] Craig JJ (2005) Introduction to robotics: mechanics and control. Upper Saddle River, NJ: Pearson/ Prentice Hall. pp. 144-146. [17] Han SP (1977) A globally convergent method for nonlinear programming. Journal of Optimization Theory and Applications 22.3: 297-309. [18] Jian JB, Chen QF and Huang ZW (2014) A superli‐ nearly convergent SQP method without bounded‐ ness assumptions on any of the iterative sequences. Journal of Computational and Applied Mathemat‐ ics 263: 115-128. [19] Sundaram RK (1996) A first course in optimization theory. Cambridge University Press. pp.194-198. [20] Elsner L (1973) A remark on simultaneous inclu‐ sions of the zeros of a polynomial by Gershgorin's theorem. Numerische Mathematik 21.5: 425-427. [21] Jian JB (2008) A quadratically approximate framework for constrained optimization. global and local convergence, Acta Math. Sin. (Engl. Ser.) 24.5: 771-788.

Substitute (41) into (40), and we have

Appendix A The proof for Theorem 1 is shown as follows. Proof: In Algorithm 1, k is the iteration step, and we assume K 0 is the set for k . Assume Algorithm 1 is not convergent, K 0 is then an infinite

set. Without loss of generality, we may assume

P' ( X k ,s k ; dk ) £ - K1h1 dk , "k Î K0 . 2

(42)

According to the continuity of Pσ ( X ), P '( X k , σk ; dk ) is bounded, so there exist η¯ > 0 satisfies P ( X k + a k dk ,s k ) - P ( X k ,s k )

lim X k = X , kÎK0 k ®¥

£ a k P' ( X k ,s k ; dk ) lim lk = l , kÎK0 k ®¥

£ -h , "k Î K0 .

¯ , σ¯ ). lim P ( X k , σk ) = P ( X

(36)

k∈K 0 k →∞

To every iteration, there exists a constant η¯ , so according to (43), adding η¯ in every iteration together, there is

According to Lemma 2.1 in [18] and the positive definite‐ ness of W ( X k , λk ) given in (34), for each k , there exists a dk

¥

h £ å éë P ( X å ò

for subproblem (31).

k K0

lim dk kÎK0 k ®¥

2

= 0,

k =1

,s k ) - P ( X k + a k dk ,s k ) ùû

¼ + éë P ( X k ,s k ) - P ( X ,s ) ùû

( )

df X - H X

T

(38)

l = 0.

which satisfies the KT condition of (17). Therefore, by considering the positive definiteness of W ( X k , λk ) given in

(34), Algorithm 1 is of global convergence [19, Section 7.7] which contradicts the assumption.

(

)

= P ( X1 ,s 1 ) - P X ,s < +¥ .

Since η¯ > 0 and

∑ η¯ < + ∞, K kK 0

0

(44)

is a finite set that contradicts

the assumption that K 0 is an infinite set, so Algorithm 1 is

of global convergence. □

So, we have dk

2

³ h1 > 0, "k Î K0 .

(39)

Appendix B The proof for Theorem 2 is shown as follows.

where η1 is a constant. According to [18, Lemma 2.3,

equation 2.13], there is

P ( X k ,s k ; dk ) £ -d W ( X k , lk ) dk , "k Î K0 . '

k

= éë P ( X1 ,s 1 ) - P ( X2 ,s 2 ) ùû +

(37)

then from (29), we have

( )

(43)

T k

"k Î K0 .

2 2

n -1 1 W ( X , l ) = Ak1 + Akn + åDi . 2 i=2

(40) where

Considering (34) and (39), we have -dkT W ( X k , lk ) dk £ - K1 dk

Proof: According to (26),

(

)

(

)

Akn = 1 - l 2 n + 1 Bn + l 1 + kn Cn ,

£ - K1h1 dk , 2

(41)

(

)

Ak1 = k1 - l 2 + l 3 - l n + 2 B1 - l 3C 2 , Yongsheng Yang, Bosen Jiang and Shiqiang Hu: Fast Trajectory Planning for VGT Manipulator via Convex Optimization

13

(

The columns of Di (i = 2, … , n − 1) are as follows

)

Di = é ki + l i + 2 Ci - l i + 2Ci + 1 - l i + n + 1Bi ù , i = 2,¼ , n - 1, ë û

i -1 ~ i + 1 [ Di ]( ) ( ) =

T

Bi = Ai Ai , i = 2,¼ , n - 1, Ci = ( Ai - Ai -1 )

(

( A - A ) , i = 2,¼, n - 1.

T

i

i -1

Here we introduce Z k to represent the columns from the (3k − 2) -th column of matrix Z to the (3k) -th column of matrix Z , and we use O to represent the zero matrix and I to represent the unit matrix. From (13), is é O ( 3 i - 3 )´ ( 3 i - 3 ) O ( 3 i - 3 )´ 3 O ( 3 i - 3 )´ ( 3 n - 3 i ) ù ê ú 3´ 3 i - 3 3´ 3 n - 3 i ) ú Bi = ê O ( ) I 3´ 3 O ( ê ú 3 n - 3 i )´ ( 3 i - 3 ) 3 n - 3 i )´ 3 3 n - 3 i )´ ( 3 n - 3 i ) ( ( ( êO ú O O ë û

é ù O 3´ 3 O 3´ 3 O 3´ 3 ú 1 Lê M M ú M M Lê ê ú 3´ 3 i+2 3´ 3 i+2 l + k I l + k I M i i - 1L ê i ú ê ú i L ê - k + l i + 2 I 3´ 3 ki - l i + n + 1 I 3´ 3 l i + 2 × I 3´ 3 ú , i 3´ 3 ú i+2 i + 1L ê M l i + 2 × I 3´ 3 - l × I ú ê M Lê M ú M M ê ú 3´ 3 n L 3´ 3 O 3´ 3 O êë úû O i = 2 ,¼ , n - 1.

3 n´ 3 n

(

the columns of

é n -1 ù ê åDi ú ëê i = 2 ûú

3 n´ 3 n

1~ 2

,

(n − 2) -th column of Ak1 + Akn are zero matrix, and

14

é ù O 3´ 3 O 3´ 3 1 Lê ú M M ú M Lê ê ú M M M Lê ú 3´ 3 3´ 3 = ú. O O n - 2L êê ú 1 3´ 3 1 3´ 3 - l + kn I ú n - 1L ê l + k n I ê ú n 1 3 3 2 1 1 3 3 ´ + ´ n L ê- l + k I 1-l + l + kn I ú n ë û

(

) (


(

)

) (

(

)

)

(

)

(

)

)

j = 3,4,¼ , n - 2.

The columns from the third column of Ak1 + Akn to the

)

(

(

)

(

)

é ù 4 3´ 3 - k 2 + l 4 I 3´ 3 1L ê k 2 + l I ú n+ 3 5 3´ 3 2L êê - k2 + l 4 I 3´ 3 k2 - l + k3 + l I úú M Lê l 4 - k3 - l 5 I 3´ 3 úú M ê = M Lê ú, M M ú M Lê M ê ú M M Lê ú M M ú nL êê 3´ 3 3´ 3 ú O O ë û

é ù O 3´ 3 ú 1 Lê M ê ú M Lê ú j +1 j+2 3´ 3 l k l I j ú j - 1L ê j n -1 é ù ê j + n+1 j +1 j+3 3´ 3 ú I ú - l + k j +1 + l ê åDi ú = i L ê k j - l ëê i = 2 úû j + 1L ê ú j+2 j+3 3´ 3 I l - k j +1 - l ê ú M Lê ú M ê ú n Lê ú 3´ 3 O ë û

n+ 2 2 I 3´ 3 l 3 × I 3´ 3 ù 1L éê k1 - l - l ú 2L ê - l 3 × I 3´ 3 ú l 3 × I 3´ 3 ê ú 3L ê O 3´ 3 ú O 3´ 3 [ Ak1 + Akn ]1~2 ú M Lê M M ê ú M Lê M ú M ú 3´ 3 3´ 3 nL ê O O êë úû

)

are as follows

i

(

So, the columns of Ak1 + Akn are shown as follows

(

∑D i=2

i = 2,¼ , n - 1.

n - 1) ~ n

) (

) )

n−1

,

3i - 6 ´3 3i - 6 ´ 3n - 3i ) ù é O ( 3 i - 6 )´ ( 3 i - 6 ) O ( 3 i - 6 )´ 3 O( ) O( ) ( ê ú 3´ 3 n - 3 i ) ê O 3´ ( 3 i - 6 ) ú I 3´ 3 - I 3´ 3 O ( ú Ci = ê 3´ 3 n - 3 i ) ê O 3´ ( 3 i - 6 ) ú - I 3´ 3 I 3´ 3 O ( ê 3n - 3i ´ 3i - 6 ú 3 n - 3 i )´ 3 3 n - 3 i )´ 3 3 n - 3 i )´ ( 3 n - 3 i ) ( ) ( ) ( ( ( êëO úû O O O

[ Ak1 + Akn ](

(

The other columns of Di (i = 2, … , n − 1) are zero matrix. So,

i = 2,¼ , n - 1,

(

)

)

é n -1 ù ê åDi ú ëê i = 2 ûú

( n -1) ~ n

é ù O 3´ 3 O 3´ 3 ú 1 Lê M ú M M Lê ê ú M M M Lê ú M ú. M = M Lê ê n ú 3´ 3 n+1 l k l I ú M n - 2L ê n -1 ê ú n - 1L ê k - l 2 n - l n I 3 ´ 3 l n + 1 × I 3 ´ 3 ú n -1 n Lê - l n + 1 × I 3´ 3 ú l n + 1 × I 3´ 3 êë úû

( (

) )

So, the columns of W ( X , λ ) / 2 are as follows

(

é W (X,l ) ù ê ú 2 êë úû

1~ 2

)

(

)

(

)

é ù 2 4 n+ 2 - k 2 + l 3 - l 4 I 3´ 3 I 3´ 3 1L ê k1 + k2 - l + l - l ú + ´ 3 5 3 3 3 n ê k2 + k3 - l + l - l I ú 2L ê - k 2 + l 3 - l 4 I 3´ 3 ú ú M Lê l 4 - k 3 - l 5 I 3´ 3 M ê ú = M Lê ú, M M ê ú ML M ê ú M M Lê ú M M ê ú nL ê 3´ 3 ú O 3´ 3 O ë û

(

)

(

)

é ù O 3´ 3 1 Lê ú M ê ú M Lê ú i +1 i+2 3´ 3 k I l l i ú i - 1L ê i ê ú i n i i + + 1 + 1 + 3 3 ´ 3 é ù - l + ki + 1 + l I ú ë W ( X , l ) / 2 û = i L ê ki - l ú i + 1L ê l i + 2 - ki + 1 - l i + 3 I 3´ 3 ê ú M Lê ú M ú n L êê 3´ 3 ú O ë û

(

(

)

(

)

)

by combining (47) and (48), we will have λi > 0, which means the matrix Z n×n is positive definite. Therefore, equation (48) can be used as a criterion for the positive definiteness of a matrix. To make W ( X , λ ) positive definite, we substitute the elements of W ( X , λ ) to (48), and then equation (35) is obtained. According to Theorem 1, Algo‐ rithm 1 is of global convergence. □ Appendix C The proof for Theorem 3 is shown as follows. Proof: According to Theorem 2, if ki (i = 1, 2, … , n ) satisfy

(35), matrix W ( X k , λk ) will be positive definite and then there is (d *, λ *) satisfying subproblem (31), which is W ( X k , lk ) dk + df ( X k ) = H ( X k ) (wk + lk ) . T

H ( X k ) dk = - g ( X k ) .

i = 3,4,¼ , n - 2.

é W (X,l ) ù ê ú 2 úû êë

( n -1) ~ n

é ù O 3´ 3 O 3´ 3 1 Lê ú M ê ú M M Lê ú M M ú M Lê M ê ú M = M Lê ú n n+1 3´ 3 I l - kn - 1 - l M ú M Lê ê ú n 1 + 1 3 ´ 3 I - kn - l + l n - 1L ê kn + kn -1 + l 1 - l n - l 2 n I 3´ 3 ú ê ú n n 2 + 1 1 + 1 3 ´ 3 n Lê n+1 1 3´ 3 k I 1 + + l l l ú I - kn - l + l n ë û

(

(

)

(

)

)

(

(

)

)

Similar to the Theorem 3.3 in [21], we assume

(

Pk = I - H ( X k ) H ( X k ) H ( X k ) T

n

j =1 j¹i

j =1 j¹i

= - Pk df ( X k )

where aii are the diagonal elements of matrix Z and aij are

j =1 j¹i

(46)

(47)

)

(51)

(

(

))

Pk W ( X k , lk ) - W X * , l * dk

n

j =1 j¹i

) (

By using the third equation of (51) is

equation (46) is satisfied. Therefore, if aii > å aij i , j = 1,2,¼ , n.

)(

using the Taylor expansion of df ( X k ).

n

j =1 j¹i

(

= - Pk W X * , l * X k - X * + o X k - X * 22 .

The first equation of (51) is obtained by (49); the second equation of (51) is obtained by the condition Pk ⋅ H ( X k )T = 0 ; The third equation of (51) is obtained

So if λi satisfy

li ³ aii - å aij i , j = 1,2,¼ , n.

(50)

T = - Pk édf ( X k ) - H ( X k ) (wk + lk ) ù ëê ûú

(45)

n

H ( Xk ) .

Pk W ( X k , lk ) dk

the non-diagonal elements of matrix Z . So, we have

li ³ aii - å aij i , j = 1,2,¼ , n.

)

-1

From (49), is

which are the eigenvalues of any matrix Z n×n satisfy n

T

Then Pk ⋅ H ( X k )T = H ( X k ) ⋅ Pk = O and Pk is symmetrical.

According to Gershgorin’s disc theorem [20], we can see λi

li - aii £ åaij £ å aij , i , j = 1,2,¼ , n.

(49)

(48)

(

)(

) (

)

= - Pk W X * , l * X k + dk - X * + o X k - X * 22 ,

(52)


15

that is

= - g ( Xk )

(

)(

)

Pk W X * , l * X k + dk - X *

(

))

(

(

)

= - Pk W ( X k , lk ) - W X * , l * dk + o X k - X * 22 .

(53) 2 ù æ ö = - H ( Xk ) Xk - X * - o ç Xk - X * ÷ ú . 2ø è û

(

*

When X k → X , there is

lim

(

))

(

- Pk W ( X k , lk ) - W X * , l * dk

(54)

= 0.

dk

k ®¥

which means

(

(

*

solution for an appropriate equality constrained quadratic programming, which means g ( X *) = 0. The third equation of (59) is obtained by using the Taylor expansion of g ( X k ).

*

)) d

k

(

= o dk

2

).

the left side of the equation we can get

(55)

(

lim

dk

= 1.

2

(56)

(

éP W X* ,l * ê k ê H (X ) k ë

2

From (56) and (55), we have

lim

(

))

(

- Pk W ( X k , lk ) - W X * , l * dk Xk - X

k ®¥

(

2

))

(

dk

(

= 0.

(57)

k ®¥

ú û

k

)

+ dk - X * =

))

(

( )

(

)(

Pk W X * , l * X k + dk - X * Xk - X *

(

(

))

Xk - X

= 0.

2

( ) ( )

æ * * çH X H X è

(

é P*W X * , l * G* = ê ê H(X * ) ë

2

- Pk W ( X k , lk ) - W X * , l * dk *

)

T

(58)

From (49), we also have H ( X k ) dk Int J Adv Robot Syst, 2015, 12:136 | doi: 10.5772/60506

T

)ùú . ú û

-1

( )

ö * ÷ H X . ø

(62)

(63)

So for ∀ d satisfies G *d = 0, we have

(

)

P * W X * , l * d = 0.

16

(61)

Define

P* = I - H X *

k ®¥

(60)

2

From (53) and (57), is

lim

)ùú ( X

é-P W X , l - W X * , l * d ù 2 ( k k) æ ö kú ê k + o ç Xk - X * ÷ . 2ø ê ú è 0 ë û

*

- Pk W ( X k , lk ) - W X * , l * dk

k ®¥

)

2 æ ö H ( X k ) X k + dk - X * = o ç X k - X * ÷ . 2ø è

So, by combining (53) and (60), we have

Xk - X *

k ®¥

= lim

(59)

The first equation of (59) is obtained by (49); the second equation of (59) is obtained since the direction dk is a

and when X k → X *, there is also

= lim

)

By moving − H ( X k )( X k − X *) in the third equation of (59) to

lim - Pk W ( X k , lk ) - W X , l k ®¥

( ))

(

= - g ( Xk ) - g X *

( )

H X * d = 0.

(64)

(65)

which means P *W ( X *, λ *) must be full rank. From (58), we have X k + dk − X * = o ( X k − X *2), that is

Premultiply d T by (64), there is

(

)

dT P * W X * , l * d = 0.

(66)

From (62) and (65), we also have P *d = d and according to the symmetry of P *, equation (66) becomes

(

)

dT W X * , l * d = 0.

(67)

lim k ®¥

X k + dk - X * Xk - X *

2

= 0.

(68)

2

Therefore, problem (17) is of superlinear convergence rate. □

If Theorem 2 is satisfied, W ( X *, λ *) will be positive definite, so from (67), d = 0. Equation (64) has only a zero solution,


17