Fast Trajectory Planning for VGT Manipulator via Convex Optimization

Abstract

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 approach. 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 super-linear convergence rate. The effectiveness of the proposed algorithm is illustrated by numerical simulation.

Keywords

VGT Kinematic Model Trajectory Planning

Symbols

M the actuation plane

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

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

s VGT mid-plane joint offset

l₀ fixed batten length

l cross longeron length

O₀ the coordinates of the centre of the bottom plane

O_t the coordinates of the centre of the top plane

${\hat{n}}_{0}$ the normal vectors of the bottom plane

${\hat{n}}_{t} = {[n_{x}, n_{y}, n_{z}]}^{T}$ the normal vectors of the top plane

$\hat{U}$ mid-plane normal

r the length from O₀ to the middle actuation plane along vector ${\hat{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 G_i(i=1, 2, 3)

s_i, c_i(i = 1, 2, 3)sin θ_i and cosθ_i

P_i, Q_i, R_i(i = 1, 2, 3) intermediate variables to calculate θ_i in inverse kinematic problem

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

${\hat{n}}_{t}^{i} = {[n_{x}^{i}, n_{y}^{i}, n_{z}^{i}]}^{T}$

$(i = 1, 2 \dots, n)$ ${\hat{n}}_{t}$ of the i -th module in the coordinate system of the i -th module

O_i = [O_xi, O_yi, O_zi]^T O_t of the i -th module in the ground coordinate system

${\hat{n}}_{i} (i = 1, 2 \dots, n)$ ${\hat{n}}_{t}$ of the i -th module in the ground coordinate 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 module

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

S_t=[Sx_t, Sy_t, Sz_t]^T the ideal position wanted

l_b the radius of the obstacle

O_b the centre of the obstacle

ls_max, ls_min the given maximum and minimum lengths of ls_i

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

X the optimization variable of the optimization problem

A_i constant matrix used to represent O_i by X

f(X) the objective function

g_i(X) the i -th constraint function

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

λ = (λ¹, …, λ²ⁿ⁺¹)^T the Lagrange multiplier

g(X) the vector of the constraint function

df the gradient of the objective function f(X)

dg_i the gradient of the constraint function g_i(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, A)the Jacobi matrix of ∇L (X, λ)

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

∇²_XX L (X, λ) the gradient of ∇_X L (X, λ) about X

∇² f the gradient of the objective function df

∇² g_i the gradient of the constraint function dg_i

k the iteration number

z_k parameter used in quadratic programming subproblem

p_k parameter used in quadratic programming subproblem

ω_k parameter used in quadratic programming subproblem

d_kd for the k -th iteration

d the optimization variable of quadratic programming subproblem

q_k(d) the objective function of quadratic programming subproblem

X_kX for the k -th iteration

λ_k = [λ_k¹ λ_k², … λ_k²ⁿ⁺¹]λ for the k -th iteration

α_k the step size for the k -th iteration

d^*, λ_k^*, X^* the minimum value for d_k, λ_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; d_k) the directional derivative of P(X_k, Σ_k) along direction d_k

ε₁ ε₂ the parameters used to stop the iteration

ρ the parameter to calculate the sizestep

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

B_i, C_i, D_i, Z matrixes used in the proof for Theorem 1

O zero matrix

I unit matrix

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

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 problems, 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 Chernonozhkin'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 three-dimensional hyper-redundant manipulator trajectory planning, including papers by Maria da Graça Marcos in 2010 [13] and in 2011 [14]; Samer Yahya also wrote a paper on trajectory planning in 2011 [15]. However, these papers make the manipulator reach the given point by using an extended Jacobi matrix. On one hand, it greatly increases the running time of the manipulators, but on the other, due to the different structures between VGT and the hyper-redundant manipulators mentioned above, the recursive matrix cannot be applied to VGT.

In this paper, a point-to-point trajectory planning algorithm for a three-dimensional VGT manipulator model is proposed using a nonlinear convex optimization approach. The trajectory planning problem is converted to a quadratic convex optimization problem, and if some parameters are appropriately chosen, the proposed algorithm can be proved to be of global convergence and to have a superlinear convergence rate.

This paper is outlined as follows. In the next section, the kinematic model of a double octahedral VGT manipulator is discussed. Section 3 is devoted to point-to-point trajectory planning for the VGT manipulator, and the simulation result is shown in Section 4. The conclusion is drawn in Section 5.

2. Kinematic Model of a Double Octahedral VGT Module

In this section, we introduce kinematic problems by using 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 problem, the inverse kinematic problem and the extensible kinematic problem. At first, we give the description of the VGT manipulator module.

2.1 Description of the VGT manipulator module

In order to explain kinematic problems, at first we should introduce the structure of the single VGT manipulator module.

As is shown in Figure 1, a single VGT manipulator module is composed of three kinds of rods: actuators, top and bottom rods and other linked rods. The lengths of the actuators are represented by l_i(i = 1, 2, 3) and the actuators' lengths are changeable. We also use l₀ to represent the lengths of the rods in bottom plane and top plane and use / to represent the lengths of the rest rods, both l₀ and / 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 G_i(i = 1, 2, 3) to represent the nodes for the actuation plane. In the actual situation, 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 O₀ and O_t to represent the centre of the bottom plane and the centre of the top plane. ${\hat{n}}_{0}$ and ${\hat{n}}_{t}$ are used to represent the normal vectors of the bottom plane and the top plane; $\hat{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 O₀ to the middle actuation plane along vector ${\hat{n}}_{0}$ .

Figure 1.

The single VGT manipulator module

Figure 2.

Parameters used for the kinematic problem

The other parameters θ_i(i = 1, 2, 3), which are the cross longeron face angles, are used as the intermediate parameters. We also use N to represent the length from the midpoint of the three sides in the bottom to G_i(i = 1, 2, 3) and $G_{i} (i = 1, 2, 3)$ and $N = \sqrt{l^{2} - (l_{0}^{2} / 4)}$ . The symbols can be seen in Figure 3.

Figure 3.

The intermediate parameters θ_i{i = 1, 2, 3)

By using these symbols and the symmetry of a single VGT manipulator module, we will introduce the forward, the inverse and the extensible kinematic problems in the remaining part of Section 2.

2.2 Forward kinematic problem for single VGT module

For the forward kinematic problem of a single VGT module, the inputs are l_i(i = 1, 2, 3) and the output is O_t. To realize the process, the cross longeron face angles θ_i(i = 1, 2, 3) are used as the intermediate parameters. We use s_i, c_i to represent sinθ_i cosθ_i, and by using the geometry relationship of single module and the known l_i(i = 1, 2, 3), the intermediate parameters θ_i(i = 1, 2, 3) can be figured out by (1) [6],

\begin{matrix} c_{1} + c_{2} - \frac{\sqrt{12 l^{2} - 3 l_{0}^{2}}}{3 l_{0}} c_{1} c_{2} + 2 \frac{\sqrt{12 l^{2} - 3 l_{0}^{2}}}{3 l_{0}} s_{1} s_{2} \\ - \frac{\sqrt{12 l^{2} - 3 l_{0}^{2}}}{12 l_{0} l^{2} - 3 l_{0}^{3}} (8 l^{2} - l_{0}^{2} - 4 l_{1}^{2}) = 0, \\ c_{2} + c_{3} - \frac{\sqrt{12 l^{2} - 3 l_{0}^{2}}}{3 l_{0}} c_{2} c_{3} + 2 \frac{\sqrt{12 l^{2} - 3 l_{0}^{2}}}{3 l_{0}} s_{2} s_{3} \\ - \frac{\sqrt{12 l^{2} - 3 l_{0}^{2}}}{12 l_{0} l^{2} - 3 l_{0}^{3}} (8 l^{2} - l_{0}^{2} - 4 l_{2}^{2}) = 0, \\ c_{3} + c_{1} - \frac{\sqrt{12 l^{2} - 3 l_{0}^{2}}}{3 l_{0}} c_{3} c_{1} + 2 \frac{\sqrt{12 l^{2} - 3 l_{0}^{2}}}{3 l_{0}} s_{3} s_{1} \\ - \frac{\sqrt{12 l^{2} - 3 l_{0}^{2}}}{12 l_{0} l^{2} - 3 l_{0}^{3}} (8 l^{2} - l_{0}^{2} - 4 l_{3}^{2}) = 0. \end{matrix}

(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, O₀=[0, 0, 0]^T, O_t can be calculated with (2–4).

O_{t} = (2 G_{1} \cdot \hat{U} + s) \hat{U} + O_{0} .

(2)

\hat{U} = \frac{(G_{2} - G_{1}) \times (G_{3} - G_{1})}{{‖ (G_{2} - G_{1}) \times (G_{3} - G_{1}) ‖}_{2}} .

(3)

\begin{matrix} G_{1} = {\begin{matrix} N s_{1} \\ - \frac{l_{0}}{2 \sqrt{3}} + N c_{1} \\ 0 \end{matrix}}, \\ G_{2} = {\begin{matrix} N s_{2} \\ \frac{l_{0}}{4 \sqrt{3}} - \frac{N}{2} c_{2} \\ - \frac{l_{0}}{4} + \frac{\sqrt{3} N}{2} c_{2} \end{matrix}}, \\ G_{3} = {\begin{matrix} N s_{3} \\ \frac{l_{0}}{4 \sqrt{3}} - \frac{N}{2} c_{3} \\ \frac{l_{0}}{4} - \frac{\sqrt{3} N}{2} c_{3} \end{matrix}} . \end{matrix}

(4)

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

2.3 Inverse kinematic problem for single VGT module

For the inverse kinematic problem of a single VGT module, the input is O_t and the outputs are l_i(i = 1, 2, 3). To realize the process, the cross longeron face angles θ_i(i = 1, 2, 3) are also used as the intermediate parameters. By considering the relationship of the vectors in Figure 2, ${\hat{n}}_{t}$ and r can be calculated by (5),

\begin{matrix} {\hat{n}}_{t} = (2 {\hat{n}}_{0} \cdot \frac{\overset{⇀}{O_{0} O_{t}}}{{‖ O_{t} - O_{0} ‖}_{2}}) \frac{\overset{⇀}{O_{0} O_{t}}}{{‖ O_{t} - O_{0} ‖}_{2}} - {\hat{n}}_{0} . \\ r = \frac{{‖ O_{t} - O_{0} ‖}_{2}^{2}}{2 {\hat{n}}_{0} \cdot \overset{⇀}{O_{0} O_{t}}} . \end{matrix}

(5)

where ${\hat{n}}_{0}$ is [1, 0, 0]^T for single VGT module. To figure out θ_i(i = 1, 2, 3), we assume ${\hat{n}}_{t} = {[n_{x}, n_{y}, n_{z}]}^{T}$ . So, θ_i(i = 1, 2, 3) are calculated by (6) [6].

θ_{i} = 2 t a n^{- 1} \frac{- Q_{i} \pm \sqrt{P_{i}^{2} + Q_{i}^{2} - R_{i}^{2}}}{R_{i} - P_{i}}, i = 1, 2, 3.

(6)

where

\begin{matrix} P_{1} = N n_{y}, \\ Q_{1} = N (1 + n_{x}) \\ R_{1} = - \frac{n_{y} L_{0}}{2 \sqrt{3}} - r (1 + n_{x}) + s \sqrt{\frac{1 + n_{x}}{2}}, \\ P_{2} = - N (\frac{1}{2} n_{y} - \frac{\sqrt{3}}{2} n_{z}), \\ Q_{2} = N (1 + n_{x}), \\ R_{2} = \frac{n_{y} L_{0}}{4 \sqrt{3}} - \frac{n_{z} L_{0}}{4} - r (1 + n_{x}) + s \sqrt{\frac{1 + n_{x}}{2}}, \\ P_{3} = - N (\frac{1}{2} n_{y} + \frac{\sqrt{3}}{2} n_{z}), \\ Q_{3} = N (1 + n_{x}), \\ R_{3} = \frac{n_{y} L_{0}}{4 \sqrt{3}} + \frac{n_{z} L_{0}}{4} - r (1 + n_{x}) + s \sqrt{\frac{1 + n_{x}}{2}} . \end{matrix}

There are eight different combinations for θ_i according to (6), but by taking the actual situation into consideration, there is only one solution left, and by using the solution, G_i(i = 1, 2, 3) can be calculated by (4) and then l_i(i = 1, 2, 3) is figured out as follows:

\begin{matrix} l_{1} = {‖ G_{2} - G_{1} ‖}_{2}, \\ l_{2} = {‖ G_{3} - G_{2} ‖}_{2}, \\ l_{3} = {‖ G_{1} - G_{3} ‖}_{2} . \end{matrix}

(7)

By using (4-7), l_i(i = 1, 2, 3) can be figured out by O_t and the inverse kinematic problem for a single VGT module is solved.

2.4 Extensible kinematic problem for multiple VGT modules

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 O_tⁱ and ${\hat{n}}_{t}^{i}$ to represent O_t and ${\hat{n}}_{t}$ of the i -th module in the local coordinate system of the i -th module. Moreover, we use O_i and ${\hat{n}}_{i}$ to represent O_t and ${\hat{n}}_{t}$ of the i -th module in the ground coordinate system. We set the initial conditions as follows.

\begin{matrix} O_{t}^{0} = O_{0} = {[0, 0, 0]}^{T}, \\ {\hat{n}}_{t}^{0} = {\hat{n}}_{0} = {[1, 0, 0]}^{T}, \\ T_{0} = [\begin{matrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{matrix}] . \\ O_{t}^{1} = O_{1}, \\ {\hat{n}}_{t}^{1} = {\hat{n}}_{1}, \end{matrix}

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):

\begin{matrix} α_{i} = a r c t a n (\frac{n_{z}^{i}}{n_{x}^{i}}), i = 1, \dots, n, \\ β_{i} = a r c t a n (\frac{n_{y}^{i}}{\sqrt{n_{x}^{i}^{2} + n_{z}^{i}^{2}}}), i = 1, \dots, n . \end{matrix}

(8)

where ${[n_{x}^{i}, n_{y}^{i}, n_{z}^{i}]}^{T} = {\hat{n}}_{t}^{i} (i = 1, 2, \dots, n)$ . By using the method developed by Craig in 1989 [16], we can calculate the transformation matrix from (9).

Figure 4.

Details for extensible kinematic problem

\begin{array}{l} T_{i} = [\begin{matrix} c o s α_{i} c o s β_{i} & - s i n β_{i} & s i n α_{i} c o s β_{i} \\ c o s α_{i} s i n β_{i} & c o s β_{i} & s i n α_{i} s i n β_{i} \\ - s i n α_{i} & 0 & c o s α_{i} \end{matrix}] \\ (i = 1, 2, \dots, n) . \end{array}

(9)

To the forward kinematic problem of the i -th VGT module, we can use l₁l₂, l₃ from the first module to the i -th module to figure out O_t^j(j = 1, 2, …, i) and ${\hat{n}}_{t}^{j} (j = 1, 2, .., i)$ by (2–5). By (8) and (9), T_j(j = 1, 2, …, i) is figured out from ${\hat{n}}_{t}^{j} (j = 1, 2, .., i)$ . Therefore, we can calculate O_i and ${\hat{n}}_{t}^{j} (j = 1, 2, .., i)$ by (10):

\begin{matrix} O_{i} = T_{0} T_{1} \dots T_{i - 1} O_{t}^{i} + T_{0} T_{1} \dots T_{i - 2} O_{t}^{i - 1} + \dots + T_{0} O_{t}^{1}, \\ {\hat{n}}_{i} = T_{0} T_{1} \dots T_{i - 1} {\hat{n}}_{t}^{i} \\ (i = 1, 2, \dots, n) . \end{matrix}

(10)

To the inverse kinematic problem for multiple modules, at first we should calculate O_i(i = 1, 2, …, n) by the given ideal position, and this problem, which is called a point-to-point trajectory planning problem, will be discussed in the next section. After calculating O_i(i = 1, 2, …, n), we can use them to figure out O_t(i = 1, 2, …, n) and ${\hat{n}}_{t}^{i} (i = 1, 2, \dots, n)$ by (8-9,11):

\begin{matrix} {\hat{n}}_{i} = (2 {\hat{n}}_{i - 1} \cdot \frac{\overset{⇀}{O_{i} O_{i - 1}}}{{‖ O_{i} - O_{i - 1} ‖}_{2}}) \frac{\overset{⇀}{O_{i} O_{i - 1}}}{{‖ O_{i} - O_{i - 1} ‖}_{2}} - {\hat{n}}_{i - 1}, \frac{- b \pm \sqrt{b^{2} - 4 a c}}{2 a} \\ {\hat{n}}_{t}^{i} = {(T_{0} T_{1} \dots T_{i - 1})}^{- 1} {\hat{n}}_{i}, \\ O_{t}^{i} = {(T_{0} T_{1} \dots T_{i - 1})}^{- 1} (O_{i} - O_{i - 1}) + O_{t}^{0}, \\ (i = 1, \dots, n) . \end{matrix}

(11)

Then, by using (4–7), l₁ l₂, l₃ from the first module to the n -th module are figured out, and the inverse kinematic problem for multiple modules is solved.

3. Point-to-Point VGT Trajectory Planning

In this section, we introduce the solution of point-to-point trajectory planning for the n-module VGT with a nonlinear programming approach.

3.1 Description of the problem

For simplicity, the i -th module of VGT can be seen as a rod, and the length of the rod is:

l s_{i} = O_{i} - O_{i - 1}_{2}, i = 1, \dots, n .

The trajectory planning problem can be shown in Figure 5.

Figure 5.

Trajectory planning problem with obstacle

If we use S_t to represent the ideal position wanted, we can represent the trajectory planning problem as follows,

\begin{matrix} \min_{S} f (X) = {‖ S_{t} - O_{n} ‖}_{2}^{2} + \sum_{i = 1}^{n} k_{i} l s_{i}^{2}, i = 1, 2, \dots, n . \\ s . t . l s_{n}^{2} \leq l s_{m a x}^{2}, \\ l s_{1}^{2} \geq l s_{m i n}^{2}, \\ l s_{i + 1}^{2} \geq l s_{i}^{2}, i = 1, 3, \dots, n - 1. \\ {‖ O_{i} - O_{b} ‖}_{2}^{2} - l_{b}^{2} \geq 0, i = 1, 2, \dots, n . \end{matrix}

(12)

where ls_max and ls_min are the given maximum and minimum lengths of ls_i and are determined in accordance with module structure parameters via a single module forward kinematic calculation in Section 2, k_i(i = 1, 2, …, n) are the weighing factors of each rod and X = [(O₁)^T, (O₂)^T, …, (O_i)^T, … (O_n)^T]^T is the final output.

Remark 1: In the proposed trajectory planning problem, we wish that the distance between S_t and O_n is minimized. If the obtained distance between S_t and O_n 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 formulation. Finally, we add an obstacle constraint in the proposed optimization problem to give VGT the capability to avoid the obstacle area.

3.2 Trajectory planning algorithm

Assume matrixes A_i ∊ R^3×3n and

A_{i} = {\begin{matrix} [O, O, \dots, I, \dots, O], i = 1, 2, .., n . \\ O^{3 \times 3 n}, i = 0. \end{matrix}

(13)

where $O = [\begin{matrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{matrix}]$ , $I = [\begin{matrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{matrix}]$ and I is the i -th matrix to A_i when i ≠ 0, so O_i = A_i ⋅ X and

l s_{i}^{2} = X^{T} {(A_{i} - A_{i - 1})}^{T} (A_{i} - A_{i - 1}) X, i = 1, 2, \dots n .

(14)

Meanwhile

{‖ S_{t} - O_{n} ‖}_{2}^{2} = X^{T} A_{n}^{T} A_{n} X - 2 S_{t}^{T} A_{n} X + S_{t}^{T} S_{t} .

(15)

{‖ O_{i} - O_{b} ‖}_{2}^{2} = X^{T} A_{i}^{T} A_{i} X - 2 O_{b}^{T} A_{i} X + O_{b}^{T} O_{b} .

(16)

Problem (12) finally becomes

\begin{matrix} \min f (X) \\ = X^{T} [A_{n}^{T} A_{n} + \sum_{i = 1}^{n} k_{i} {(A_{i} - A_{i - 1})}^{T} (A_{i} - A_{i - 1})] X \\ - 2 S_{t}^{T} A_{n} X + S_{t}^{T} S_{t}, i = 1, \dots, n \\ s . t . g_{j} (X) \geq 0, j = 1, \dots, 2 n + 1 \end{matrix}

(17)

where

\begin{matrix} g_{1} (X) = - X^{T} {(A_{n} - A_{n - 1})}^{T} (A_{n} - A_{n - 1}) X + l s_{m a x}^{2}, \\ g_{2} (X) = X^{T} A_{1}^{T} A_{1} X - l s_{m i n}^{2}, \\ g_{l} (X) = X^{T} [{(A_{l - 1} - A_{l - 2})}^{T} (A_{l - 1} - A_{l - 2}) \\ - {(A_{l - 2} - A_{l - 3})}^{T} (A_{l - 2} - A_{l - 3})] X, \\ l = 3, \dots, n + 1 \\ g_{m} (X) = X^{T} A_{m - n - 1}^{T} A_{m - n - 1} X - O_{b}^{T} A_{m - n - 1} X \\ + O_{b}^{T} O_{b} - l_{b}^{2}, \\ m = n + 2, \dots, 2 n + 1 \end{matrix}

Because f(X) and g_j(X) are all second-order continuously differentiable real functions, the Lagrangian function of problem (17) is

\begin{array}{l} L (X, λ) = f (X) - \sum_{i = 1}^{2 n + 1} λ^{j} g_{j} (X) \\ = f (X) - λ^{T} g (X) . \end{array}

(18)

where λ = (λ¹, …, λ²ⁿ⁺¹)^T is the Lagrange multiplier and g(X)=[g₁(X), g₂(X), …, g_2n+1(X)]^T. The gradient of the objective function f(X) is

d f = 2 [A_{n}^{T} A_{n} + \sum_{i = 1}^{n} k_{i} {(A_{i} - A_{i - 1})}^{T} (A_{i} - A_{i - 1})] X - 2 A_{n}^{T} S_{t} .

(19)

and the gradient of the constraint functions g_j(X) are

\begin{matrix} d g_{1} = - 2 {(A_{n} - A_{n - 1})}^{T} (A_{n} - A_{n - 1}) X, \\ d g_{2} = 2 A_{1}^{T} A_{1} X, \\ d g_{l} = 2 [{(A_{l - 1} - A_{l - 2})}^{T} (A_{l - 1} - A_{l - 2}) \\ - {(A_{l - 2} - A_{l - 3})}^{T} (A_{l - 2} - A_{l - 3})] X, \\ l = 3, \dots, n + 1, \\ d g_{m} = 2 A_{m - n - 1}^{T} A_{m - n - 1} X - 2 A_{m - n - 1}^{T} O_{b}, \\ m = n + 2, \dots, 2 n + 1. \end{matrix}

(20)

Assume

H (X) = \nabla g {(X)}^{T} = {[d g_{1}, d g_{2}, \dots, d g_{2 n + 1}]}^{T} .

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

\nabla L (X, λ) = [\begin{matrix} \nabla_{X} L (X, λ) \\ \nabla_{λ} L (X, λ) \end{matrix}] = [\begin{matrix} d f - H {(X)}^{T} λ) \\ - g (X) \end{matrix}] = 0.

(21)

So, the Jacobi matrix of ∇ L (X, λ) is

N (X, λ) = [\begin{matrix} W (X, λ) & - H {(X)}^{T} \\ - H (X) & 0 \end{matrix}],

(22)

where from (18)

\begin{array}{l} W (X, λ) = \nabla^{2}_{X X} L (X, λ) \\ = \nabla^{2} f - \sum_{j = 1}^{2 n + 1} λ^{j} \nabla^{2} g_{j} . \end{array}

(23)

and

\nabla^{2} f = 2 A_{n}^{T} A_{n} + 2 \sum_{i = 1}^{n} k_{i} {(A_{i} - A_{i - 1})}^{T} (A_{i} - A_{i - 1}) .

(24)

\begin{matrix} \sum_{j = 1}^{2 n + 1} λ^{j} \nabla^{2} g_{j} \\ = - 2 λ^{1} {(A_{n} - A_{n - 1})}^{T} (A_{n} - A_{n - 1}) + 2 λ^{2} A_{1}^{T} A_{1} \\ + 2 \sum_{l = 3}^{n + 1} λ^{l} [{(A_{l - 1} - A_{l - 2})}^{T} (A_{l - 1} - A_{l - 2}) \\ - {(A_{l - 2} - A_{l - 3})}^{T} (A_{l - 2} - A_{l - 3})] \\ + 2 \sum_{m = n + 2}^{2 n + 1} λ^{m} A_{m - n - 1}^{T} A_{m - n - 1} . \end{matrix}

(25)

Then

\begin{matrix} W (X, λ) \\ = \nabla^{2} f - \sum_{j = 1}^{2 n + 1} λ^{j} \nabla^{2} g_{j} \\ = 2 (1 - λ^{2 n + 1}) A_{n}^{T} A_{n} \\ + 2 (λ^{1} + k_{n}) {(A_{n} - A_{n - 1})}^{T} (A_{n} - A_{n - 1}) - 2 λ^{2} A_{1}^{T} A_{1} \\ + 2 \sum_{i = 1}^{n - 1} [(k_{i} + λ^{i + 2}) {(A_{i} - A_{i - 1})}^{T} (A_{i} - A_{i - 1}) \\ - 2 λ^{i + 2} {(A_{i + 1} - A_{i})}^{T} (A_{i + 1} - A_{i}) - 2 λ^{i + n + 1} A_{i}^{T} A_{i}] . \end{matrix}

(26)

In general, we use the Newton method to figure out nonlinear equation (21), so to the given point z_k = (X_k, λ_k), the format for the Newton method is

z_{k + 1} = z_{k} + p_{k},

(27)

where p_k = (d_k, ω_k) satisfies

N (X_{k}, λ_{k}) p_{k} = - \nabla L (X_{k}, λ_{k}),

(28)

that is

[\begin{matrix} W (X_{k}, λ_{k}) & - H {(X_{k})}^{T} \\ H (X_{k}) & 0 \end{matrix}] [\begin{matrix} d_{k} \\ ω_{k} \end{matrix}] = - [\begin{matrix} d f (X_{k}) - H {(X_{k})}^{T} λ_{k} \\ g (X_{k}) \end{matrix}] .

(29)

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

\begin{matrix} W (X_{k}, λ_{k}) d_{k} + d f (X_{k}) = H {(X_{k})}^{T} (ω_{k} + λ_{k}), \\ g (X_{k}) + H (X_{k}) d_{k} = 0. \end{matrix}

(30)

Equation (30) is the KT condition for the quadratic programming subproblem (31) as follows

\begin{matrix} \min_{d} q_{k} (d) = \frac{1}{2} d^{T} W (X_{k}, λ_{k}) d + \nabla f {(X_{k})}^{T} d . \\ s . t . g (X_{k}) + H (X_{k}) d \geq 0. \end{matrix}

(31)

By solving a quadratic programming subproblem (31), we get the minimum point (d^*, λ_k^*). From (30), we can see that d^*=d_k 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 d_k calculated by (31) is used to be the descent direction of penalty function (32),

P_{σ} (X) = f (X) + σ^{- 1} [\sum_{i = 1}^{2 n + 1} | {[g_{i} (X)]}_{-} |] .

(32)

where [g_i(X)] = max{0, -g_i(S)} and penalty parameter Σ>0. The penalty parameter Σ is obtained by the following rules

σ_{k} = {\begin{matrix} σ_{k - 1}, \\ {(τ + 2 δ)}^{- 1}, \end{matrix} \begin{matrix} σ_{k - 1}^{- 1} \geq τ + δ . \\ σ_{k - 1}^{- 1} < τ + δ . \end{matrix}

(33)

where Δ > 0 is a constant and τ = λ_k∞.

Now use λ_k=[λ_k¹, λ_k², …, λ_kⁿ⁺¹] to represent the Lagrange multiplier for the k -th iteration and use P′ (X_k, Σ_k;d_k) to represent the directional derivative of P(X_k, Σ_k) along direction d_k. Then, the point-to-point trajectory planning algorithm is shown as follows.

Algorithm 1:

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 k_i; Algorithm1 is convergent and of superlinear convergence.

3.3 Convergence and convergence rate analysis

The global convergence of Algorithm 1 is given by the following theorem.

Theorem 1: For the optimization problem (17), if existing constants 0K₁≤K₂ which make W(X_k, λ_k) satisfies

K_{1} {‖ d ‖}_{2}^{2} \leq d^{T} W (X_{k}, λ_{k}) d \leq K_{2} {‖ d ‖}_{2}^{2}, \forall d .

(34)

Algorithm 1 is of global convergence.

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.

To make W(X_k, λ_k) easy to check in practice, a sufficient condition for global convergence of VGT trajectory planning is given by the following theorem.

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

\begin{matrix} | k_{1} + k_{2} - λ_{k}^{2} + λ_{k}^{4} - λ_{k}^{n + 2} | > | k_{2} - λ_{k}^{3} + λ_{k}^{4} |, \\ | k_{2} + k_{3} - λ_{k}^{3} - λ_{k}^{n + 3} + λ_{k}^{5} | \\ > | k_{2} - λ_{k}^{3} + λ_{k}^{4} | + | k_{3} + λ_{k}^{5} - λ_{k}^{4} |, \\ | k_{n + 1} - λ_{k}^{n} - λ_{k}^{2 n} + k_{n} + λ_{k}^{1} | \\ > | k_{n - 1} - λ_{k}^{n} + λ_{k}^{n + 1} | + | k_{n} + λ_{k}^{1} - λ_{k}^{n + 1} |, \\ | k_{n} + λ_{k}^{1} - λ_{k}^{n + 1} - λ_{k}^{2 n + 1} + 1 | > | k_{n} + λ_{k}^{1} - λ_{k}^{n + 1} |, \\ | k_{j} + k_{j + 1} - λ_{k}^{j + 1} - λ_{k}^{j + n + 1} + λ_{k}^{j + 3} | \\ > | k_{j} + λ_{k}^{j + 2} - λ_{k}^{j + 1} | + | k_{j + 1} + λ_{k}^{j + 3} - λ_{k}^{j + 2} |, \\ j = 3, 4, \dots, n - 2. \end{matrix}

(35)

Problem (17) is of global convergence.

Proof: See Appendix B.

For the convergence rate of Algorithm 1, we have the following theorem.

Theorem 3: If k_i(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.

4. Simulation

Simulation of trajectory planning with an obstacle for three-module VGT is shown in this part. The initial parameters needed are shown in Table 1.

We use [Ox₃, Oy₃, Oz₃] to represent the coordinates of O₃ and use [Sx_t, Sy_t, Sz_t] to represent the coordinates of S_t. Then, the relative error Er is calculated by

E r = \frac{1}{10} \sum_{j = 1}^{10} \sqrt{{(\frac{O x_{3} - S x_{t}}{S x_{t}})}^{2} + {(\frac{O y_{3} - S y_{t}}{S y_{t}})}^{2} + {(\frac{O z_{3} - S z_{t}}{S z_{t}})}^{2}} .

And if Sy_t=0, make (Oy₃-Sy_t)/Sy_t=0. The absolute error Ea is calculated by

E a = \frac{1}{10} \sum_{j = 1}^{10} \sqrt{{(O x_{3} - S x_{t})}^{2} + {(O y_{3} - S y_{t})}^{2} + {(O z_{3} - S z_{t})}^{2}} .

Table 1.
Initial Parameters for VGT

Parameter Value(cm)

s 82

l ₀ 475

l 475

ls _max 850

ls _min 740

Parameter	Value(cm)
s	82
l ₀	475
l	475
ls _max	850
ls _min	740

According to Algorithm 1, the steps of the simulation for a single point S_t =[2433, −85, 100]^T are shown as follows.

Step 1 Initialize

S_{t} = {[2433, - 85, 100]}^{T}, O_{b} = {[400, 400, 400]}^{T}, l_{b} = 300.

Step 2 Set

\begin{matrix} X_{0} = {[800, 0, 0, 1600, 0, 0, 2400, 0, 0]}^{T}, \\ λ_{0} = {[0.003, 0.003, 0.003, 0.003, 0.003, 0.003, 0.003]}^{T}, \\ η = 0.1, ρ = 0.5, σ_{0} = 0.8, ε_{1} = 1 \times e^{- 4}, ε_{2} = 1 \times e^{- 5}, \end{matrix}

Δ=0.05, k_i = 0.01 and calculate W(X₀, λ₀) by (26). Make k: =0.

Step 3 Solve problem (31), find the optimal solution (d_k, λ_k^*).

Step 4 If d_k1 ≤ε₁ and (g(S_k))₋₁ ≤ε₂, transfer to Step 8; otherwise, transfer to Step 5.

Step 5 Determine Σ_k by (33).

Step 6 Make m_k the smallest non-negative integer which satisfies the following inequality

P (X_{k} + ρ^{m} d_{k}, σ_{k}) - P (X_{k}, σ_{k}) \leq η ρ^{m} P^{'} (X_{k}, σ_{k}; d_{k}),

Make $α_{k} : = ρ^{m_{k}}$ , $X_{k + 1} : = X_{k} + α_{k} d_{k}$ , $λ_{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 Set $O_{0} = {[0, 0, 0]}^{T}, {\hat{n}}_{0} = {[1, 0, 0]}^{T},$ , and obtain

\begin{matrix} O_{1} = {[808.31, - 28.24, 33.22]}^{T}, \\ O_{2} = {[1616.61, - 56.48, 66.45]}^{T}, \\ O_{3} = {[2424.92, - 84.72, 99.67]}^{T} . \end{matrix}

Make i:=1.

Step 9 If i > 3, break; Else, Use O_i and O_i−1 to calculate O_tⁱ by (11), transfer to Step 10.

Step 10 Use O_tⁱ to calculate l₁ l₂, l₃ of the i -th module by (4–7). Update i:=i + 1, transfer to Step 9.

To demonstrate the effectiveness of the proposed method, we choose ten different S_t 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.

Table 2.

Results for simulation

	Given positions	Simulation output
1	[2300, −150, −150]^T	[2292.36, −149.50, −149.50]^T
2	[2350, −126, −126]^T	[2342.19, −125.58, −125.58]^T
3	[2450, −115, −105]^T	[2441.86, −114.62, −104.65]^T
4	[2500, −85, −100]^T	[2491.69, −84.72, −99.67]^T
5	[2380, 50, −120]^T	[2372.09, 49.83, −119.60]^T
6	[2480, 150, 0]^T	[2471.76, 149.50, 0]^T
7	[2560, 0, 0]^T	[2550, 0, 0]^T
8	[2240, 20, −20]^T	[2232.56, 19.93, −19.93]^T
9	[2433, −128, 59]^T	[2424.92, −127.57, −58.80]^T
10	[2356, −23, 123]^T	[2348.17, −22.92, 122.59]^T

From the simulation result, the relative error is 0.54% and the absolute error is 8.15218mm. The simulation is run on a TIP 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.

Figure 6.

The x-axis position results

Figure 7.

The y-axis position results

Figure 8.

The z-axis position results

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 convergence rate. Simulation results in Section 4 show that the proposed algorithm is effective for the trajectory planning of VGT.

Footnotes

Appendix A

The proof for Theorem 1 is shown as follows.

Proof: In Algorithm 1, k is the iteration step, and we assume K₀ is the set for k.

Assume Algorithm 1 is not convergent, K₀ is then an infinite set. Without loss of generality, we may assume

(36)

\begin{matrix} \lim_{\begin{matrix} k \in K_{0} \\ k \to \infty \end{matrix}} X_{k} = \bar{X}, \\ \lim_{\begin{matrix} k \in K_{0} \\ k \to \infty \end{matrix}} λ_{k} = \bar{λ}, \\ \lim_{\begin{matrix} k \in K_{0} \\ k \to \infty \end{matrix}} P (X_{k}, σ_{k}) = P (\bar{X}, \bar{σ}) . \end{matrix}

According to Temma 2.1 in [18] and the positive definiteness of W(X_k, λ_k) given in (34), for each k, there exists a d_k for subproblem (31).

(37)

\lim_{\begin{matrix} k \in K_{0} \\ k \to \infty \end{matrix}} {‖ d_{k} ‖}_{2} = 0,

then from (29), we have

(38)

d f (\bar{X}) - H {(\bar{X})}^{T} \bar{λ} = 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.

So, we have

(39)

{‖ d_{k} ‖}_{2} \geq η_{1} > 0, \forall k \in K_{0} .

where η₁ is a constant. According to [18, Temma 2.3, equation 2.13], there is

(40)

P^{'} (X_{k}, σ_{k}; d_{k}) \leq - d_{k}^{T} W (X_{k}, λ_{k}) d_{k}, \forall k \in K_{0} .

Considering (34) and (39), we have

(41)

\begin{matrix} - d_{k}^{T} W (X_{k}, λ_{k}) d_{k} \leq - K_{1} {‖ d_{k} ‖}_{2}^{2} \leq - K_{1} η_{1} {‖ d_{k} ‖}_{2}, \\ \forall k \in K_{0} . \end{matrix}

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

(42)

P^{'} (X_{k}, σ_{k}; d_{k}) \leq - K_{1} η_{1} {‖ d_{k} ‖}_{2}, \forall k \in K_{0} .

According to the continuity of P_Σ(X), P′(X_k, Σ_k; d_k) is bounded, so there exist $\bar{η} > 0$ satisfies

(43)

\begin{matrix} P (X_{k} + α_{k} d_{k}, σ_{k}) - P (X_{k}, σ_{k}) \\ \leq α_{k} P^{'} (X_{k}, σ_{k}; d_{k}) \\ \leq - \bar{η}, \forall k \in K_{0} . \end{matrix}

To every iteration, there exists a constant $\bar{η}$ , so according to (43), adding $\bar{η}$ in every iteration together, there is

(44)

\begin{matrix} \sum_{k ϵ K_{0}} \bar{η} \leq \sum_{k = 1}^{\infty} [P (X_{k}, σ_{k}) - P (X_{k} + α_{k} d_{k}, σ_{k})] \\ = [P (X_{1}, σ_{1}) - P (X_{2}, σ_{2})] + \\ \dots + [P (X_{k}, σ_{k}) - P (X, \bar{σ})] \\ = P (X_{1}, σ_{1}) - P (\bar{X}, \bar{σ}) < + \infty . \end{matrix}

Since $\bar{η} > 0$ and $\sum_{k ϵ K_{0}} \bar{η} < + \infty$ , $K_{0}$ , K₀ is a finite set that contradicts the assumption that K₀ is an infinite set, so Algorithm 1 is of global convergence.

Appendix B

The proof for Theorem 2 is shown as follows.

Proof: According to (26),

\frac{1}{2} W (X, λ) = A k_{1} + A k_{n} + \sum_{i = 2}^{n - 1} D_{i} .

where

\begin{matrix} A k_{n} = (1 - λ^{2 n + 1}) B_{n} + (λ^{1} + k_{n}) C_{n}, \\ A k_{1} = (k_{1} - λ^{2} + λ^{3} - λ^{n + 2}) B_{1} - λ^{3} C_{2}, \\ D_{i} = [(k_{i} + λ^{i + 2}) C_{i} - λ^{i + 2} C_{i + 1} - λ^{i + n + 1} B_{i}], i = 2, \dots, n - 1, \\ B_{i} = A_{i}^{T} A_{i}, i = 2, \dots, n - 1, \\ C_{i} = {(A_{i} - A_{i - 1})}^{T} (A_{i} - A_{i - 1}), i = 2, \dots, n - 1. \end{matrix}

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

\begin{matrix} B_{i} = {[\begin{matrix} O^{(3 i - 3) \times (3 i - 3)} & O^{(3 i - 3) \times 3} & O^{(3 i - 3) \times (3 n - 3 i)} \\ O^{3 \times (3 i - 3)} & I^{3 \times 3} & O^{3 \times (3 n - 3 i)} \\ O^{(3 n - 3 i) \times (3 i - 3)} & O^{(3 n - 3 i) \times 3} & O^{(3 n - 3 i) \times (3 n - 3 i)} \end{matrix}]}^{3 n \times 3 n}, \\ i = 2, \dots, n - 1, \\ C_{i} = {[\begin{matrix} \begin{matrix} O^{(3 i - 6) \times (3 i - 6)} & O^{(3 i - 6) \times 3} \\ O^{3 \times (3 i - 6)} & I^{3 \times 3} \end{matrix} & \begin{matrix} O^{(3 i - 6) \times 3} & O^{(3 i - 6) \times (3 n - 3 i)} \\ - I^{3 \times 3} & O^{3 \times (3 n - 3 i)} \end{matrix} \\ \begin{matrix} O^{3 \times (3 i - 6)} & - I^{3 \times 3} \\ O^{(3 n - 3 i) \times (3 i - 6)} & O^{(3 n - 3 i) \times 3} \end{matrix} & \begin{matrix} I^{3 \times 3} & O^{3 \times (3 n - 3 i)} \\ O^{(3 n - 3 i) \times 3} & O^{(3 n - 3 i) \times (3 n - 3 i)} \end{matrix} \end{matrix}]}^{3 n \times 3 n}, \\ i = 2, \dots, n - 1. \end{matrix}

So, the columns of [Ak₁ + Ak_n] are shown as follows

{[A k_{1} + A k_{n}]}^{1 \sim 2} \begin{matrix} 1 \\ 2 \\ 3 \\ ⋮ \\ ⋮ \\ n \end{matrix} \begin{matrix} \dots \\ \dots \\ \dots \\ \dots \\ \dots \\ \dots \end{matrix} [\begin{matrix} (k_{1} - λ^{2} - λ^{n + 2}) I^{3 \times 3} \\ λ^{3} \cdot I^{3 \times 3} \\ O^{3 \times 3} \\ ⋮ \\ ⋮ \\ O^{3 \times 3} \end{matrix} \begin{matrix} λ^{3} \cdot I^{3 \times 3} \\ - λ^{3} \cdot I^{3 \times 3} \\ O^{3 \times 3} \\ ⋮ \\ ⋮ \\ O^{3 \times 3} \end{matrix}]

The columns from the third column of [Ak₁ + Ak_n] to the (n−2) -th column of [Ak₁ + Ak_n] are zero matrix, and

{[A k_{1} + A k_{n}]}^{(n - 1) \sim n} = \begin{matrix} 1 \\ ⋮ \\ ⋮ \\ n - 2 \\ n - 1 \\ n \end{matrix} \begin{matrix} \dots \\ \dots \\ \dots \\ \dots \\ \dots \\ \dots \end{matrix} [\begin{matrix} O^{3 \times 3} \\ ⋮ \\ ⋮ \\ O^{3 \times 3} \\ (λ^{1} + k_{n}) I^{3 \times 3} \\ - (λ^{1} + k_{n}) I^{3 \times 3} \end{matrix} \begin{matrix} O^{3 \times 3} \\ ⋮ \\ ⋮ \\ O^{3 \times 3} \\ - (λ^{1} + k_{n}) I^{3 \times 3} \\ (1 - λ^{2 n + 1} + λ^{1} + k_{n}) I^{3 \times 3} \end{matrix}] .

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

\begin{array}{l} {[D_{i}]}^{(i - 1) \sim (i + 1)} = \\ \begin{matrix} 1 \\ ⋮ \\ i - 1 \\ i \\ i + 1 \\ ⋮ \\ n \end{matrix} \begin{matrix} \dots \\ \dots \\ \dots \\ \dots \\ \dots \\ \dots \\ \dots \end{matrix} [\begin{matrix} O^{3 \times 3} \\ ⋮ \\ (k_{i} + λ^{i + 2}) I^{3 \times 3} \\ - (k_{i} + λ^{i + 2}) I^{3 \times 3} \\ ⋮ \\ ⋮ \\ O^{3 \times 3} \end{matrix} \begin{matrix} O^{3 \times 3} \\ ⋮ \\ - (k_{i} + λ^{i + 2}) I^{3 \times 3} \\ (k_{i} - λ^{i + n + 1}) I^{3 \times 3} \\ λ^{i + 2} \cdot I^{3 \times 3} \\ ⋮ \\ O^{3 \times 3} \end{matrix} \begin{matrix} O^{3 \times 3} \\ ⋮ \\ ⋮ \\ λ^{i + 2} \cdot I^{3 \times 3} \\ - λ^{i + 2} \cdot I^{3 \times 3} \\ ⋮ \\ O^{3 \times 3} \end{matrix}], \\ i = 2, \dots, n - 1. \end{array}

The other columns of [D_i](i=2, …, n −1) $[\sum_{i = 2}^{n - 1} D_{i}]$ are zero matrix. So, the columns of are as follows

\begin{matrix} {[\sum_{i = 2}^{n - 1} D_{i}]}^{1 \sim 2} = \begin{matrix} 1 \\ 2 \\ ⋮ \\ ⋮ \\ ⋮ \\ ⋮ \\ n \end{matrix} \begin{matrix} \dots \\ \dots \\ \dots \\ \dots \\ \dots \\ \dots \\ \dots \end{matrix} [\begin{matrix} (k_{2} + λ^{4}) I^{3 \times 3} \\ - (k_{2} + λ^{4}) I^{3 \times 3} \\ ⋮ \\ ⋮ \\ ⋮ \\ ⋮ \\ O^{3 \times 3} \end{matrix} \begin{matrix} - (k_{2} + λ^{4}) I^{3 \times 3} \\ (k_{2} - λ^{n + 3} + k_{3} + λ^{5}) I^{3 \times 3} \\ (λ^{4} - k_{3} - λ^{5}) I^{3 \times 3} \\ ⋮ \\ ⋮ \\ ⋮ \\ O^{3 \times 3} \end{matrix}], \\ {[\sum_{i = 2}^{n - 1} D_{i}]}^{j} = \begin{matrix} 1 \\ ⋮ \\ j - 1 \\ i \\ j + 1 \\ ⋮ \\ n \end{matrix} \begin{matrix} \dots \\ \dots \\ \dots \\ \dots \\ \dots \\ \dots \\ \dots \end{matrix} [\begin{matrix} O^{3 \times 3} \\ ⋮ \\ (λ^{j + 1} - k_{j} - λ^{j + 2}) I^{3 \times 3} \\ (k_{j} - λ^{j + n + 1} - λ^{j + 1} + k_{j + 1} + λ^{j + 3}) I^{3 \times 3} \\ (λ^{j + 2} - k_{j + 1} - λ^{j + 3}) I^{3 \times 3} \\ ⋮ \\ O^{3 \times 3} \end{matrix}] \\ j = 3, 4, \dots, n - 2. \\ {[\sum_{i = 2}^{n - 1} D_{i}]}^{(n - 1) \sim n} = \begin{matrix} 1 \\ ⋮ \\ ⋮ \\ ⋮ \\ n - 2 \\ n - 1 \\ n \end{matrix} \begin{matrix} \dots \\ \dots \\ \dots \\ \dots \\ \dots \\ \dots \\ \dots \end{matrix} [\begin{matrix} O^{3 \times 3} \\ ⋮ \\ ⋮ \\ ⋮ \\ (λ^{n} - k_{n - 1} - λ^{n + 1}) I^{3 \times 3} \\ (k_{n - 1} - λ^{2 n} - λ^{n}) I^{3 \times 3} \\ λ^{n + 1} \cdot I^{3 \times 3} \end{matrix} \begin{matrix} O^{3 \times 3} \\ ⋮ \\ ⋮ \\ ⋮ \\ ⋮ \\ λ^{n + 1} \cdot I^{3 \times 3} \\ - λ^{n + 1} \cdot I^{3 \times 3} \end{matrix}] . \end{matrix}

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

\begin{matrix} {[\frac{W (X, λ)}{2}]}^{1 \sim 2} = \begin{matrix} 1 \\ 2 \\ ⋮ \\ ⋮ \\ ⋮ \\ ⋮ \\ n \end{matrix} \begin{matrix} \dots \\ \dots \\ \dots \\ \dots \\ \dots \\ \dots \\ \dots \end{matrix} [\begin{matrix} (k_{1} + k_{2} - λ^{2} + λ^{4} - λ^{n + 2}) I^{3 \times 3} \\ (- k_{2} + λ^{3} - λ^{4}) I^{3 \times 3} \\ ⋮ \\ ⋮ \\ ⋮ \\ ⋮ \\ O^{3 \times 3} \end{matrix} \begin{matrix} (- k_{2} + λ^{3} - λ^{4}) I^{3 \times 3} \\ (k_{2} + k_{3} - λ^{3} + λ^{5} - λ^{n + 3}) I^{3 \times 3} \\ (λ^{4} - k_{3} - λ^{5}) I^{3 \times 3} \\ ⋮ \\ ⋮ \\ ⋮ \\ O^{3 \times 3} \end{matrix}], \\ {[W (X, λ) / 2]}^{i} = \begin{matrix} 1 \\ ⋮ \\ i - 1 \\ i \\ i + 1 \\ ⋮ \\ n \end{matrix} \begin{matrix} \dots \\ \dots \\ \dots \\ \dots \\ \dots \\ \dots \\ \dots \end{matrix} [\begin{matrix} O^{3 \times 3} \\ ⋮ \\ (λ^{i + 1} - k_{i} - λ^{i + 2}) I^{3 \times 3} \\ (k_{i} - λ^{i + n + 1} - λ^{i + 1} + k_{i + 1} + λ^{i + 3}) I^{3 \times 3} \\ (λ^{i + 2} - k_{i + 1} - λ^{i + 3}) I^{3 \times 3} \\ ⋮ \\ O^{3 \times 3} \end{matrix}] \\ i = 3, 4, \dots, n - 2. \\ {[\frac{W (X, λ)}{2}]}^{(n - 1) \sim n} = \begin{matrix} 1 \\ ⋮ \\ ⋮ \\ ⋮ \\ ⋮ \\ n - 1 \\ n \end{matrix} \begin{matrix} \dots \\ \dots \\ \dots \\ \dots \\ \dots \\ \dots \\ \dots \end{matrix} [\begin{matrix} O^{3 \times 3} \\ ⋮ \\ ⋮ \\ ⋮ \\ (λ^{n} - k_{n - 1} - λ^{n + 1}) I^{3 \times 3} \\ (k_{n} + k_{n - 1} + λ^{1} - λ^{n} - λ^{2 n}) I^{3 \times 3} \\ (- k_{n} - λ^{1} + λ^{n + 1}) I^{3 \times 3} \end{matrix} \begin{matrix} O^{3 \times 3} \\ ⋮ \\ ⋮ \\ ⋮ \\ ⋮ \\ (- k_{n} - λ^{1} + λ^{n + 1}) I^{3 \times 3} \\ (1 - λ^{2 n + 1} + λ^{1} + k_{n} - λ^{n + 1}) I^{3 \times 3} \end{matrix}] \end{matrix}

According to Gershgorin's disc theorem [20], we can see λ_i which are the eigenvalues of any matrix Z^nxn satisfy

(45)

| λ_{i} - a_{i i} | \leq \sum_{\begin{matrix} j = 1 \\ j \neq i \end{matrix}}^{n} a_{i j} \leq \sum_{\begin{matrix} j = 1 \\ j \neq i \end{matrix}}^{n} | a_{i j} |, i, j = 1, 2, \dots, n .

where a_ii are the diagonal elements of matrix Z and a_ij are the non-diagonal elements of matrix Z. So, we have

(46)

λ_{i} \geq a_{i i} - \sum_{\begin{matrix} j = 1 \\ j \neq i \end{matrix}}^{n} | a_{i j} | i, j = 1, 2, \dots, n .

So if λ_i satisfy

(47)

λ_{i} \geq | a_{i i} | - \sum_{\begin{matrix} j = 1 \\ j \neq i \end{matrix}}^{n} | a_{i j} | i, j = 1, 2, \dots, n .

equation (46) is satisfied. Therefore, if

(48)

| a_{i i} | > \sum_{\begin{matrix} j = 1 \\ j \neq i \end{matrix}}^{n} | a_{i j} | i, j = 1, 2, \dots, n .

by combining (47) and (48), we will have λ_i>0, which means the matrix Z^nxn 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, Algorithm 1 is of global convergence.

Appendix C

The proof for Theorem 3 is shown as follows.

Proof: According to Theorem 2, if k_i(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

(49)

\begin{matrix} W (X_{k}, λ_{k}) d_{k} + d f (X_{k}) = H {(X_{k})}^{T} (ω_{k} + λ_{k}) . \\ H (X_{k}) d_{k} = - g (X_{k}) . \end{matrix}

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

(50)

P_{k} = I - H {(X_{k})}^{T} {(H (X_{k}) H {(X_{k})}^{T})}^{- 1} H (X_{k}) .

Then P_k ⋅ H(X_k)^T = H(X_k) ⋅ P_k=O and P_k is symmetrical. From (49), is

(51)

\begin{matrix} P_{k} W (X_{k}, λ_{k}) d_{k} \\ = - P_{k} [d f (X_{k}) - H {(X_{k})}^{T} (ω_{k} + λ_{k})] \\ = - P_{k} d f (X_{k}) \\ = - P_{k} W (X^{*}, λ^{*}) (X_{k} - X^{*}) + o (X_{k} - X^{*}_{2}^{2}) . \end{matrix}

The first equation of (51) is obtained by (49); the second equation of (51) is obtained by the condition P_k ⋅ H(X_k)^T=0; The third equation of (51) is obtained using the Taylor expansion of df(X_k).

By using the third equation of (51) is

(52)

\begin{matrix} P_{k} (W (X_{k}, λ_{k}) - W (X^{*}, λ^{*})) d_{k} \\ = - P_{k} W (X^{*}, λ^{*}) (X_{k} + d_{k} - X^{*}) + o (X_{k} - X^{*}_{2}^{2}), \end{matrix}

that is

(53)

\begin{matrix} P_{k} W (X^{*}, λ^{*}) (X_{k} + d_{k} - X^{*}) \\ = - P_{k} (W (X_{k}, λ_{k}) - W (X^{*}, λ^{*})) d_{k} + o (X_{k} - X^{*}_{2}^{2}) . \end{matrix}

When X_k → X^*, there is

(54)

\lim_{k \to \infty} \frac{- P_{k} (W (X_{k}, λ_{k}) - W (X^{*}, λ^{*})) d_{k}}{d_{k}} = 0.

which means

(55)

\lim_{k \to \infty} - P_{k} (W (X_{k}, λ_{k}) - W (X^{*}, λ^{*})) d_{k} = o ({‖ d_{k} ‖}_{2}) .

and when X_k → X^*, there is also

(56)

\underset{k \to \infty}{l i m} \frac{{‖ X_{k} - X^{*} ‖}_{2}}{{‖ d_{k} ‖}_{2}} = 1.

From (56) and (55), we have

(57)

\begin{matrix} \lim_{k \to \infty} \frac{- P_{k} (W (X_{k}, λ_{k}) - W (X^{*}, λ^{*})) d_{k}}{{‖ X_{k} - X^{*} ‖}_{2}} \\ = \lim_{k \to \infty} \frac{- P_{k} (W (X_{k}, λ_{k}) - W (X^{*}, λ^{*})) d_{k}}{{‖ d_{k} ‖}_{2}} = 0. \end{matrix}

From (53) and (57), is

(58)

\begin{matrix} \lim_{k \to \infty} \frac{P_{k} W (X^{*}, λ^{*}) (X_{k} + d_{k} - X^{*})}{{‖ X_{k} - X^{*} ‖}_{2}} \\ = \lim_{k \to \infty} \frac{- P_{k} (W (X_{k}, λ_{k}) - W (X^{*}, λ^{*})) d_{k}}{{‖ X_{k} - X^{*} ‖}_{2}} = 0. \end{matrix}

From (49), we also have

(59)

\begin{matrix} H (X_{k}) d_{k} \\ = - g (X_{k}) \\ = - (g (X_{k}) - g (X^{*})) \\ = - H (X_{k}) (X_{k} - X^{*}) - o ({‖ X_{k} - X^{*} ‖}_{2}^{2})] . \end{matrix}

The first equation of (59) is obtained by (49); the second equation of (59) is obtained since the direction d_k is a 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).

By moving -H (X_k)[X_k − X^*) in the third equation of (59) to the left side of the equation we can get

(60)

H (X_{k}) (X_{k} + d_{k} - X^{*}) = o ({‖ X_{k} - X^{*} ‖}_{2}^{2}) .

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

(61)

\begin{matrix} [\begin{matrix} P_{k} W (X^{*}, λ^{*}) \\ H (X_{k}) \end{matrix}] (X_{k} + d_{k} - X^{*}) = \\ [\begin{matrix} - P_{k} (W (X_{k}, λ_{k}) - W (X^{*}, λ^{*})) d_{k} \\ 0 \end{matrix}] + o ({‖ X_{k} - X^{*} ‖}_{2}^{2}) . \end{matrix}

Define

(62)

P^{*} = I - H {(X^{*})}^{T} {(H (X^{*}) H {(X^{*})}^{T})}^{- 1} H (X^{*}) .

(63)

G^{*} = [\begin{matrix} P^{*} W (X^{*}, λ^{*}) \\ H (X^{*}) \end{matrix}] .

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

(64)

P^{*} W (X^{*}, λ^{*}) d = 0.

(65)

H (X^{*}) d = 0.

Premultiply d ^T by (64), there is

(66)

d^{T} P^{*} W (X^{*}, λ^{*}) d = 0.

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

(67)

d^{T} W (X^{*}, λ^{*}) d = 0.

If Theorem 2 is satisfied, W(X^*, λ^*) will be positive definite, so from (67), d = 0. Equation (64) has only a zero solution, which means P^*(W(X^*, λ^*) must be full rank. From (58), we have X_k+d_k−X^*=o(x_k−X^* X^*₂), that is

(68)

\lim_{k \to \infty} \frac{{‖ X_{k} + d_{k} - X^{*} ‖}_{2}}{{‖ X_{k} - X^{*} ‖}_{2}} = 0.

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

References

Miura

(1984) Variable geometry truss concept. Institute of Space and Astronautical Science report. 1984 January 1.

Aviles

(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.

Hanahara

, and Tada

(2008) Variable geometry Truss with SMA Wire Actuators (Basic Consideration on Kinematical and Mechanical Characteristics). 19th International Conference of Adaptive Structures and Technologies, October 6–9, 2008, Ascona Switzerland.

Rost

. (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.

Naccarato

and Peter

(1991) Inverse kinematics of variable geometry truss manipulators. Journal of Robotic Systems 8.2: 249–266.

Williams

(1994) Kinematic modeling of a double octahedral Variable Geometry Truss (VGT) as an extensible gimbal. NASA report. NASA-TM-109127, NAS 1.15:109127.

Williams

and Hexter

(1998) Maximizing kinematic motion for a 3-dof VGT module. Journal of Mechanical Design 120.2: 333–336.

Yao

and Fang

(1995) Forward displacement analysis of the decahedral variable geometry truss manipulator. Robotics and Autonomous systems 15.3:173–178.

Lee

(2000) Kinematic control experiments with trussarm, a variable-geometry-truss manipulator [PhD thesis]. Institute for Aerospace Studies: University of Toronto.

10.

Lees

and Chirikjian

GS.

(1996) A combinatorial approach to trajectory planning for binary manipulators. Robotics and Automation, IEEE International Conference on Volume: 3: p2749–2754.22 Apr 1996. Minneapolis, MN.

11.

Chernonozhkin

(2006) Binary manipulator motion planning [PhD thesis]. Available from: http://www.mayr.in.tum.de/konferenzen/Jass06/courses/5/Papers/Chernonozhkin.pdf.

12.

Lin

Chen

Chuang

(2012) Motion planning using a memetic evolution algorithm for swarm robots. Int. J. Adv. Robot. Syst. 9: 25.

13.

da Graça Marcos

Maria

Tenreiro Machado

and Azevedo-Perdicoulis

T-P

(2010). An evolutionary approach for the motion planning of redundant and hyper-redundant manipulators. Nonlinear Dynamics 60.1–2:115–129.

14.

Yahya

Moghavvemi

and Mohamed

HAF

(2011). Geometrical approach of planar hyper-redundant manipulators: Inverse kinematics, path planning and workspace. Simulation Modelling Practice and Theory 19.1: 406–422.

15.

da Graça Marcos

Maria

Tenreiro Machado

and Azevedo-Perdicoulis

T-P

(2011). A fractional approach for the motion planning of redundant and hyper-redundant manipulators. Signal Processing 91.3:562–570.

16.

Craig

(2005) Introduction to robotics: mechanics and control. Upper Saddle River, NJ: Pearson/Prentice Hall. pp. 144–146.

17.

Han

(1977) A globally convergent method for nonlinear programming. Journal of Optimization Theory and Applications 22.3: 297–309.

18.

Jian

Chen

and Huang

(2014) A superlinearly convergent SQP method without bounded-ness assumptions on any of the iterative sequences. Journal of Computational and Applied Mathematics 263:115–128.

19.

Sundaram

(1996) A first course in optimization theory. Cambridge University Press, pp.194–198.

20.

Eisner

(1973) A remark on simultaneous inclusions of the zeros of a polynomial by Gershgorin's theorem. Numerische Mathematik 21.5: 425–427.

21.

Jian

(2008) A quadratically approximate framework for constrained optimization, global and local convergence, Acta Math. Sin. (Engl. Ser.) 24.5: 771–788.

Fast Trajectory Planning for VGT Manipulator via Convex Optimization

Abstract

Keywords

1. Introduction

2. Kinematic Model of a Double Octahedral VGT Module

2.1 Description of the VGT manipulator module

3.1 Description of the problem

Table 1. Initial Parameters for VGT Parameter Value(cm) s 82 l 0 475 l 475 ls max 850 ls min 740

Footnotes

Appendix A

Appendix B

Appendix C

References

Table 1.
Initial Parameters for VGT

Parameter Value(cm)

s 82

l ₀ 475

l 475

ls _max 850

ls _min 740