Trajectory Planning for a Rigid Body Based on Voronoi...

Trajectory Planning for a Rigid Body Based on Voronoi

Tessellation and Linear-Quadratic Feedback Control

Authors:Hans FlodinGustav FribergFredrik Wahlberg

[email protected]@kth.se

[email protected]

Supervisors:Xiaoming HuJohan Thunberg

[email protected]@math.kth.se

SA104X, Bachelor Thesis in Mathematics,Division of Optimization and Systems Theory,

Department of Mathematics,Royal Institute of Technology, Stockholm, Sweden

May 21, 2012

Abstract

When a rigid body moves in 3-dimensional space, it is of interest to find a trajectorysuch that it avoids obstacles. With this report, we create an algorithm that finds such atrajectory for a 6-DOF rigid body. For this trajectory, both the rotation and translationof the rigid body are included.

This is a trajectory planning problem in the space of Euclidean transformations SE(3).Because of the complexity of this problem, we divide it into two consecutive parts wherethe first part comprises translational path planning in the 3-dimensional Euclidean spaceand the rotational path planning. The second part comprises the design of control lawsin order to follow the designed trajectory.

In the first part we create virtual spheres surrounding each obstacle in order to ob-tain approximate central points for each obstacle, which are then used as input for ouraugmented Voronoi tessellation method. We then create a graph containing all feasiblepaths along the faces of the convex polytopes containing the central points of the virtualspheres. A simple global graph-search algorithm is used to find the shortest path betweenthe nodes of the polytopes, which is then further improved by approximately 3%. Whererandom obstacles are uniformly distributed in a confined space. During the work process,we discovered that our translational path planning method easily could be generalized tobe used in n-dimensional space.

In the second part, the control law for the rigid body such that it follows the rotationand translation trajectory is designed to minimize the cost, which is done by creating aLinear-Quadratic feedback loop for a linear system. The translational control is designedin an inertial reference frame and the rotational control is designed in a body frame,rigidly attached to the rigid body’s center of mass. This allows us to separate the controllaws for the translational and rotational systems. The rotational system is a nonlinearsystem and has been linearized to be able to use the Linear-Quadratic feedback controller.

All this resulted in a well performing algorithm that would find and track a feasibletrajectory as long as a feasible path exists.

Contents

1 Introduction 1

2 Rigid Body Transformation and Motion 32.1 Transformation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.2 Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72.3 Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3 Path Planning 83.1 Translational Path Planning . . . . . . . . . . . . . . . . . . . . . . . . . . 8

3.1.1 Voronoi Tessellation . . . . . . . . . . . . . . . . . . . . . . . . . . 83.1.2 Virtual Spheres . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113.1.3 Graph Searching . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133.1.4 Further Path Improvement . . . . . . . . . . . . . . . . . . . . . . 14

3.2 Rotational Path Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

4 Control Design 224.1 State Space Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

4.1.1 Translational System . . . . . . . . . . . . . . . . . . . . . . . . . . 234.1.2 Rotational System . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

4.2 Reachability . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 264.3 Observability . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 264.4 Linear Quadratic Controller on an Infinite Time Interval . . . . . . . . . . 27

4.4.1 Algebraic Riccati Equation . . . . . . . . . . . . . . . . . . . . . . 284.4.2 Change of Reference Point . . . . . . . . . . . . . . . . . . . . . . . 29

4.5 Linear Quadratic Controller on a Finite Time Interval . . . . . . . . . . . 304.5.1 Matrix Differential Riccati Equation . . . . . . . . . . . . . . . . . 314.5.2 Trajectory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

4.6 Stability . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

5 Simulations 345.1 Simulation of Graph Creation . . . . . . . . . . . . . . . . . . . . . . . . . 345.2 Path Improvement Simulation . . . . . . . . . . . . . . . . . . . . . . . . . 355.3 Control Law Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

5.3.1 Translation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 375.3.2 Rotation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

6 Conclusions 45

7 Discussion 47

Bibliography 48

A Proofs 49A.1 Path Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49

B Nomenclature 54

Chapter 1

Introduction

In this report we provide a means of finding a path avoiding obstacles through spaceand design control laws for a rigid body to follow this path. This is applicable in manydifferent areas, e.g., satellites, UAVs1 or robotic units.

By controlling the translational motion in an inertial world frame, we separate the ro-tational and translational dynamics. Furthermore, the knowledge of the environment isassumed to be global, i.e., that the constellation of the space environment is fixed andfully known before initial path planning. We have then separated the path planningand control design of the rigid body and developed an algorithm for finding a trajectoryavoiding obstacles.

If we have a set of obstacles O = Ok : k ∈ K, where K is a set of m indices, thepurpose of the path planning part of our algorithm is to find the shortest translationalpath, with orientation, between a start position and an end position in 3-dimensionalspace, which avoids the obstacles O and maximizes the minimum distance to the neigh-boring obstacles and thereby minimizing the interference with obstacles.

The aim for the control law design is to construct a controller for both the transla-tional and rotational motion, tracking the trajectory. As a further objective we want tominimize the input energy and deviation from the reference state. This can be associatedwith a cost, resulting in a problem of minimizing this cost.

This report will proceed as follows:Since the kinematics, dynamics and transformations of the rigid body are needed for

the control design, this is reviewed in chapter 2.To successfully create a path that avoids obstacles, some kind of structure is needed.

We have chosen to use Voronoi tessellation, described in section 3.1.1. In order to obtainconvex polytopes from this tessellation, the obstacles have to be approximated as points.This is done by minimizing virtual spheres around the obstacles as described in section3.1.2. After compiling a graph of feasible paths through the tessellation, Dijsktra’s Al-gorithm is used to find the shortest path between two points, as shown in 3.1.3. A wayof further optimizing this path, as long as we had obstacles that could be approximately

1Unmanned Aerial Vehicle

1

CHAPTER 1. INTRODUCTION

considered as points, is described in section 3.1.4. A method of finding a rotational pathfor the rigid body along the found translational path is presented in section 3.2.

To design the desired controllers, we have chosen to use the theory of Linear-Quadraticcontroller. Two Linear-Quadratic controllers are designed in chapter 4, one minimizingthe cost on an infinite time interval and one minimizing the cost on a finite time interval.

In chapter 5 we simulate example conditions from both the path planning part andthe control design part of our algorithm to evaluate results that were difficult to predictfrom theory.

The two final chapters consist of a conclusion and a discussion of the apprehendedresults, followed by an appendix with all self-compiled proofs and notations used in thereport.

2

Chapter 2

Rigid Body Transformationand Motion

In this chapter we show how our rigid body is represented. The position of the rigidbody is expressed in a world frame FW which we assume is a cartesian inertial frame.To represent the orientation a cartesian body frame FB is introduced. This body frameis rigidly attached to the center of mass of the rigid body. To distinguish these framesfurther, we use uppercase letters, XY Z, for the cartesian coordinate axes in the worldframe FW and lowercase letters, xyz for the body frame FB . When the transformationbetween these two frames is determined, the kinematic equations and dynamic equationsare defined for the rigid body.

2.1 Transformation

The translation of the body frame FB relative the world frame FW is represented by atranslation vector ~T according to figure 2.1.

Figure 2.1: The translation vector ~T representing the position of the body frame originrelative the world frame origin.

3

CHAPTER 2. RIGID BODY TRANSFORMATION AND MOTION

An explicative method of representing a specific rotational orientation of a 3-dimensionalobject relative a reference frame, in our case the world frame FW , is by using the Eulerangles ψ, θ, φ. Let the world frame FW and the body frame FB have a coinciding origin,then the angle ψ is the angle between the coordinate axis X and the line of intersectionbetween the XY plane and the xy plane, the angle θ is the angle between the coordi-nate axes Z and z and the angle φ is the angle between the x-axis and the line of theintersection between the XY plane and the xy plane[2]. This can be seen in figure 2.2.

Figure 2.2: Euler angles, where the green line is the intersection of the XY -plane and thexy-plane.

To transform vectors represented in the world frame FW to the rotated body frameFB a rotation matrix R need to be specified. The rotation matrix R then maps vectorsin the world frame FW to the body frame FB , i.e.,

R : ~bW → ~bB , (2.1)

where b is a vector and the subindices W and B refer to the world frame FW and bodyframe FB . The rotation matrix R ∈ SO(3), with SO(3) defined as in definition 2.1.1.

Definition 2.1.1. [3] SO(n) is the group of all matrices in Rn×n whose determinantsare +1, i.e.,

SO(n) =

A ∈ Rn×n : det(A) = +1

. (2.2)

SO(n) is called the special orthogonal group.

4


In this report, we use the following rotation matrices:

Definition 2.1.2. The rotation matrices Rψ,Rθ and Rφ are given by

Rψ =

cosψ − sinψ 0sinψ cosψ 0

0 0 1

, (2.3)

Rθ =

1 0 00 cos θ − sin θ0 sin θ cos θ

, (2.4)

Rφ =

cosφ − sinφ 0sinφ cosφ 0

0 0 1

. (2.5)

Since SO(n) is a group, it is closed under its binary operation, i.e., multiplicity, whichimplies that any sequence of rotations in SO(n) is also a rotation, e.g.,

R = RψRθRφ ∈ SO(3), (2.6)

where Rψ,Rθ,Rφ are as in definition 2.1.2. To illustrate this rotation sequence, firstassume the world frame FW and body frame FB axes and origin coinciding. The rotationsequence can then be demonstrated in the following way:

1. First, we rotate the body frame FB an angle φ around the Z-axis. This is done bythe rotation matrix Rφ and is illustrated in figure 2.3.

Figure 2.3: The body frame rotated an angle φ around the Z-axis.

2. Secondly, we rotate the body frame FB around the X-axis an angle θ. This is doneby the rotation matrix Rθ and is illustrated in figure 2.4.

5


Figure 2.4: The body frame rotated an angle θ around the X-axis.

3. Finally, the body frame FB is rotated an angle ψ around the Z-axis This is doneby the rotation matrix Rψ and is illustrated in figure 2.5.

Figure 2.5: The body frame is rotated an angle ψ around the Z axis.

With definition 2.1.2 and (2.6), we have a rotation matrix R for the whole rotationsequence given by

R =

cosψ cosφ− sinψ cos θ sinφ − cosψ sinφ− sinψ cos θ cosφ sinψ sin θsinψ cosφ+ cosψ cos θ sinφ cosψ cos θ cosφ− sinψ sinφ − cosψ sin θ

sin θ sinφ sin θ cosφ cos θ

.

(2.7)From definition 2.1.1 we see that all rotational matrices are invertible since det R = +1.Then R−1 is a map from body frame FB to world frame FW .

6


It can also be noted that the rotation sequence from our choice of Euler angles has atleast one singularity, e.g., θ = nπ, n ∈ Z.

2.2 Kinematics

The kinematics of a rigid body consists of a translational and rotational motion. Therotational kinematics of a rigid body using Euler angles results in a set of nonlineardifferential equations, namely the Euler Kinematic Equations[2]

ωx = ψ sin θ sinφ+ θ cosφ

ωy = ψ sin θ cosφ− θ sinφ

ωz = ψ cos θ + φ

, (2.8)

where, ωx, ωy, ωz are the angular velocities around the x, y, z axes.

2.3 Dynamics

The translational dynamics of the rigid body follow Newton’s Second Law ~p = ddt (m~v) and

the rotational dynamics of the rigid body is described by the Euler Dynamic Equations[2] Ixxωx + (Izz − Iyy)ωyωz = Mx

Iyyωy + (Ixx − Izz)ωzωx = My

Izzωz + (Iyy − Ixx)ωxωy = Mz

, (2.9)

where Ixx, Iyy, Izz are the moments of inertia and Mx,My,Mz are the applied torquearound the principal axes of the body frame FB .

7

Chapter 3

Path Planning

One issue of a trajectory planning problem, is to find a feasible path for it through thearea of interest. We chose to address the problem globally, i.e. that the constellationof the space environment was fixed and fully known before initial path planning. Thisis partly because of the difficulties with simulating local knowledge and partly a way ofsimplifying the problem.

Initially, we describe our method of finding a translational path. During the workprocess, it was discovered that this method easily could be generalized to n dimensions,which is why it is also described in n dimensions. Due to difficulties in handling set theoryin numerical calculating environments like MATLAB R©, some changes had to be done,e.g., approximating the obstacles, resulting with a suboptimal path.

Finally, the method of finding a rotational path for the rigid body along the transla-tional path is described.

3.1 Translational Path Planning

3.1.1 Voronoi Tessellation

To quickly determine the shortest path through an n-dimensional space, and at the sametime avoid a number of obstacles, some kind of structure is needed. We decided to useVoronoi tessellation because of its simplicity and compatibility with numerical computerenvironments, like for example MATLAB R©. As we shall later see, the diagram of aVoronoi tessellation is also created such that the minimum distance to every obstacleis maximized. An example of a Voronoi tessellation over sets containing finitely manyunique points as obstacles in R2 and R3 can be seen in figure 3.1.

The Voronoi tessellation is a special kind of decomposition of a given space, namedafter the Ukrainian mathematician Georgy Voronoi who studied and defined the n-dimensional case[15]. In our case, this is a decomposition of a Euclidean n-space, de-termined by the distance to some specified objects in space.

Following are some definitions needed to define the subset of each Voronoi cell, i.e.,the space which is closest to each obstacle. We have chosen to work in Euclidean space,which is defined and proved to be metric in appendix A.1. With the Euclidean n-spaceproven to be a metric space we will define the set of each Voronoi cell.

8

CHAPTER 3. PATH PLANNING

Figure 3.1: Voronoi tessellation in R2 and R3.

Definition 3.1.1. [10] Let (X,d) be a metric space. Given two nonempty sets P,A ⊆ X,the dominance region, dom(P,A), of P with respect to A is the set of all x ∈ X which arecloser to P than to A, i.e.,

dom(P,A) = x ∈ X | d(x, P ) ≤ d(x,A), (3.1)

where d(x,A) = inf d(x, a) : a ∈ A, e.g., the shortest Euclidean distance in Euclideanspace.

Definition 3.1.2. [10] Let (X,d) be a metric space and let K be a set of at least 2 elements(indices), possibly infinite. Given a tuple (Pk)k∈K of nonempty subsets Pk ⊆ X, calledthe generators or the sites, the Voronoi diagram induced by this tuple is the tuple (Rk)k∈Kof nonempty subsets Rk ⊆ X, such that for all k ∈ K,

Rk = dom(Pk,⋃

j∈K,j 6=kPj) = x ∈ X | d(x, Pk) ≤ d(x, Pj),∀j ∈ K : j 6= k. (3.2)

In other words, each Rk, called the k:th Voronoi cell, is the set of all x ∈ X whose distanceto Pk is not greater than their distance to any of the other Pj , j ∈ K, j 6= k.

Now, let P be a set containing m disjoint and confined subsets of Rn, i.e.,

P = Pk ⊂ Rn|Pk ∩ Pj = ∅,∀j ∈ K : j 6= k, (3.3)

where K is a set of m elements (indices) and m is finite. Then, a Voronoi tessellation overP results in m n-dimensional sets, i.e., the Voronoi cells, where the k:th point xk ∈ Pk islocated within the k:th cell.

Let Xk denote the space within the k:th Voronoi cell. We then have that

Xk = dom(Pk, Pj), ∀j ∈ K : j 6= k. (3.4)

By exclusively working in Euclidean space and only performing the Voronoi tessellationover a set containing finitely many unique points, it can be proven that the sets making

9


up the Voronoi cells for all obstacles as a matter of fact are convex polytopes. This is awell known fact which usually is not proved, but we provide a proof in this report. Forthat, we also need the following definitions of convex sets and n-dimensional hyperplanes.

Definition 3.1.3. [7] A set C ⊆ Rn is convex if it has the following property:

(1− t)x+ ty ∈ C ∀x, y ∈ C, t ∈ [0, 1]. (3.5)

Lemma 3.1.4. An intersection of two convex sets is convex.

Proof. See appendix A.1.

Lemma 3.1.5. A set S with linear constraints, i.e.,

S =

x ∈ Rn

∣∣∣∣Ax ≤ b, A ∈ Rm×n, b ∈ Rm, (3.6)

is a convex set.


Definition 3.1.6. [14] Let a = (a1, a2, . . . , an) ∈ Rn\0 and b ∈ R. An n-dimensionalhyperplane H = Ha,b ⊂ Rn is defined as

H =

x ∈ Rn

∣∣∣∣ n∑k=1

akxk + b = 0

. (3.7)

The vector a is orthogonal to H and is called the normal vector of H.A half-space S is defined as

S =

x ∈ Rn

∣∣∣∣ n∑k=1

akxk + b ≤ 0

. (3.8)

Definition 3.1.7. An n-dimensional convex polytope is an intersection of n-dimensionalhalf-spaces.

Proposition 3.1.8. Every cell of a Voronoi tessellation of a finite number of points inEuclidean n-space is an n-dimensional convex polytope.


Corollary 3.1.9. Each face of a convex polytope of a Voronoi tessellation over finitelymany unique points in Euclidean n-space is an (n− 1)-dimensional convex polytope.


10


3.1.2 Virtual Spheres

As conditioned in proposition 3.1.8, we only obtain convex polytopes when performing atessellation over a set of finitely many unique points. Then what happens if the obstaclesare not just point obstacles?

We wanted to be able to use the properties of having convex polytopes generatedby the Voronoi tessellation, independent of the geometry of the obstacles. We thereforedeveloped a method to approximate each obstacle as a point by minimizing a virtualsphere around each obstacle. This was also a way to escape the difficulties of handlingsets in numerical calculating environments.

This is done by solving a convex nonlinear optimization problem which minimizes ann-dimensional sphere around each obstacle Ok, where Ok is a subspace represented by aset of finitely many unique points corresponding to the extreme points of the obstacle’sphysical shell. Furthermore,

Ok ∩ Oj = ∅, ∀k, j ∈ K : k 6= j, (3.9)

i.e., all obstacles Ok are disjoint. For this, some optimization definitions are needed.

Definition 3.1.10. [13] An optimization problem is a problem where one want to findthe minimum value of an objective function with respect to some variable under someconstraints. This is written as

minimizex

f(x)

subject to gk(x) ≤ 0, k = 1, 2, . . . ,m,(3.10)

where f(x) is an objective function, gk(x), k = 1, 2, . . . ,m, are constraint functions andx is a variable of a finite dimension. The constraint functions can also be denoted with afeasible set F , i.e.,

minimizex

f(x)

subject to x ∈ F(3.11)

where f(x) is an objective function, F is the feasible region and x is a variable of a finitedimension.

Definition 3.1.11. [13] A convex function is a function f : X → R such that

f((1− t)x+ ty

)≤ (1− t)f(x) + tf(y), ∀x, y ∈ X. (3.12)

Definition 3.1.12. [13] A convex optimization problem is an optimization problem like(3.23), where the objective function and all constraint functions are convex, i.e., an opti-mization of a convex objective function over a convex set.

Definition 3.1.13. [13] A nonlinear optimization problem is an optimization problemlike (3.23), such that at least one of f(x) and gk(x), k = 1, 2, . . . ,m, are nonlinear.

In our case, with Ok being a set represented by finitely many, sk, unique points, i.e.,

Ok =

pk,j ∈ Rn, j = 1, 2, . . . , sk

, (3.13)

11


we had an optimization problem as in (3.23), with the variable x ∈ Rn+1, objectivefunction

f(x) = xn+1 (3.14)

and constraint functionsgj(x) =

(pk,j − (x1, x2, . . . , xn)

)T (pk,j − (x1, x2, . . . , xn)

)− xn+1,

j = 1, 2, . . . , sk(3.15)

where (x1, x2, . . . , xn) is a vector representing the coordinate of the origin of the sphereand xn+1 represents the squared radius of the sphere.

Since the objective function f(x) = xn+1 is convex and the constraint functions

gj(x) =

(pk,j − (x1, x2, . . . , xn)

)T(pk,j − (x1, x2, . . . , xn)

)− xn+1

=

n∑i=1

(pik,j − xi)2 − xn+1, j = 1, 2, . . . , sk

(3.16)

are convex and nonlinear, this is a convex nonlinear optimization problem.

By solving this optimization problem, the location and radius of the sphere surround-ing obstacle Ok is found. With this method we get an approximation of each obstaclewith a virtual sphere, which can be seen in figure 3.2. It should be noted that thesevirtual spheres can intersect with the polytope faces.

Figure 3.2: Virtual spheres in red surrounding obstacles represented by blue points in R3.

12


3.1.3 Graph Searching

With section 3.1.1 and 3.1.2, we now have the knowledge to divide a space into a tessel-lation according to some obstacles by approximate them with help of the virtual spheres.By also introducing the start and end points in the Voronoi tessellation, these points areguaranteed a link into our structure of convex polytope faces. We then need a method ofeffectively finding the shortest feasible path between two points in this tessellation.

From this well defined tessellation, we have set up a graph consisting of all feasiblepartial paths between the nodes of the polytopes. We denote these partial paths as legs.

Definition 3.1.14. [5] A graph is a pair G = (N ,L), consisting of a finite set N and aset L of 2-element subsets of N , where N is the set of all nodes (or vertices, or points)of G and L the set of legs (or edges, or lines) between the nodes of G.

Let G = (N ,L) denote the graph of all paths between all nodes of the polytopes inthe tessellation, where N is the set of all nodes, and L is the set of all legs between thenodes of the polytopes.

For a path to be feasible, it must avoid all obstacles. Therefore, it is inconvenient ifthe path passes through a polytope, and it is therefore required to travel along the facesof the polytopes.

A set P of all faces of the polytopes is then created by gathering all intersections ofthe convex polytopes created by the tessellation, i.e.,

P =Pk∣∣k ∈ K, (3.17)

wherePk =

⋃j∈K:j 6=k

Xk ∩Xj (3.18)

is the set of all faces of the k:th polytope, and Xk, Xj are as in (3.4) with (Pk, Pj) =(Ok,Oj). In our case, these faces are represented by their corner nodes. By connecting allnodes in the same face, a graph of all face legs is created. At this moment, all these legs areexamined so that no part intersects with any virtual sphere making them infeasible, andgiving them weights corresponding to their lengths. Here, one can also demand furtherproperties of the legs, e.g., maximum angular changes. With all potentially infeasible legsremoved, the graph

GF = (N ,LF ), (3.19)

consisting of all nodes N and all feasible legs LF , is completed.

Dijkstra’s Algorithm

To find the shortest path between two nodes in this graph, we have chosen to use Dijkstra’sAlgorithm because of its implementation in MATLAB R©. This is a graph search algorithmnamed after the Dutch computer scientist Edsger W. Dijkstra, that, from a given node,finds the shortest path to all other nodes[4]. This algorithm is applicable to non-negativeweighted graphs only. Since our weights are the lengths of the leg, i.e., the distancesbetween the nodes, this is not of our concern.

By using this algorithm between all nodes of both the start and end points’ polytopes,we obtain the shortest path through the graph, which can be seen in figure 3.3.

13


Figure 3.3: Linearly interpolated path obtained from graph search, avoiding virtualspheres in R3.

Since there is always some open polytopes in the tessellation stretching towards infin-ity, the method described will always find a path as long as it is not completely surroundedby virtual spheres, i.e., if a feasible path exists.

3.1.4 Further Path Improvement

A drawback with the path resulting from the previous section is that it will only go be-tween nodes of the polytopes’ faces. This is denoted as the node-path. By crossing thefaces over their edges, an equally short or shorter path could be found. This is denotedas the edge-path.

By finding all nodes lying in the same polytope face, the neighbor nodes of the nodesin the path, i.e., path nodes, can be found by examining the angles between all vectorsbetween the nodes, illustrated in figure 3.4. From corrollary 3.1.9 we know that the poly-tope faces are convex, and therefore the two neighbor nodes are the two nodes such thatthe vector from the path node to these nodes maximizes

αij =

∣∣∣∣∣ arccos

(〈−−−→N0Ni,

−−−→N0Nj〉‖−−−→N0Ni‖‖

−−−→N0Nj‖

)∣∣∣∣∣, i, j ∈ J. (3.20)

14


−−−→N0Ni is the vector between the path node N0 and the i:th node Ni in the polytope face,αij is the angle between the vectors to the i:th and j:th node, illustrated in figure 3.4and i, j ∈ J where J is a set of elements representing indices of the nodes in the polytopeface.

ijα

Figure 3.4: The pair with the biggest angle defines the neighbors.

This means each face along the node-path contributes with two neighbors. The edgesbetween the path node and the neighbors are represented by vectors and are called feasibleedges. Furthermore, the fact that every path node often relates to more than one faceresults in a variation within the number of feasible edges. Figure 3.5 shows a 2-dimensionalprojection of a 3-dimensional structure. The node-path is in black and the green vectorsare the feasible edges pointing at the neighbors.

15


Figure 3.5: Node-path in black and all feasible edges in green

In 3-dimensional space, the feasible edges can end up on polytope faces located indifferent faces. Since the edge-path is not allowed to move inside the polytopes it isnecessary to determine which feasible edges are located in the same face. By comparingall neighbor nodes to the k:th and (k+ 1):th path node, it is possible to determine if theleg between the neighbor nodes correspond to a leg in LF ∈ GF from (3.19). If so, thepath nodes and neighbor nodes are located in the same polytope face and are feasiblefor a potential edge-path. We call this pair of neighbor nodes a feasible neighbor pair.An illustration of feasible neighbor pairs in an arbitrary node-path sequence is shown in(3.21),

. . . , k − 1, k, k + 1, k + 2, . . .

. . . ,

1 25 47 9

,

[4 69 11

],

11 206 1914 1614 17

,

20 2331 2716 25

, . . .(3.21)

This illustration shows the path nodes k − 1, k, k + 1, k + 2 and their correspondingfeasible neighbor pairs as rows in respective matrix. We want to connect the feasibleneighbor pairs into a set of neighbor combinations. This is done with an iterative methodexamining the feasible neighbor pairs of each path node. We start by creating a feasibleneighbor combination by examining if a neighbor should be added to this combination.This method starts with examining the neighbors of the first path node and is iteratedas follows until the z:th path node.

(i)If the (k + 1):th path node share this neighbor, it is added to the feasible neighborcombination and the neighbors which it is paired with are examined.

(i)If the (k + 1):th path node does not share this neighbor, the (k + 1):th path nodeis added to the feasible neighbor combination and all feasible neighbor pairs of the(k + 1):th node are examined.

By finding all these feasible neighbor combinations, we can create the set of feasibleneighbor combinations. With feasible neighbor pairs as in (3.21), two examples of feasibleneighbor combinations can be:

16


[. . . , 7, 9, 11, 20, 23, . . . ][. . . , 5, k, 11, 20, 23 . . . ]

(3.22)

These two combinations represents a subset of the set of feasible neighbor combinations.Furthermore, a combination of feasible edges is denoted as the edges between the node-path and a corresponding neighbor combination, illustrated in figure 3.6. This resultsin equally many combinations of neighbors as combinations of feasible edges. However,to minimize the distance of an edge-path we want to calculate the optimal intersectionbetween the edge-path and a combination of feasible edges. This is solved with a nonlinearoptimization problem.

Figure 3.6: Node-path in black and a combination of feasible edges in green.

Minimization of the Edge-to-Edge Path Distance

This distance minimization problem requiers a variable, denoted ak, which is set on eachedge, vk, in the combination of feasible edges. If ak = 0, the edge-path on that specificedge, vk, is intersected with the node-path. On the other hand, if ak = 1, the edge-path onthat specific edge is intersected with the neighbor. Furthermore, ak ∈ [0, 1], k = 1, 2, . . . , zand we get the following nonlinear non-convex optimization:

minimizex

z−1∑k=1

∥∥(pk+1 + vk+1ak+1)− (pk + vkak)∥∥

subject to ak ∈ [0, 1], k = 1, 2, . . . , z,

(3.23)

The aim of this optimization problem is to minimize the total distance of the edge-pathby finding the minimizing values on ak ∈ [0, 1], where the edge-path crosses the edgesbetween the k:th and (k + 1):th path node’s shared neighbors. The objective function isa sum of the distances between all edges. Since there are z nodes in the node path, thereare z−1 distances in the sum. By solving this optimization problem for each combinationof feasible edges, the shortest one is found, which is illustrated in figure 3.7 and 3.8.

17


Figure 3.7: 2-dimensional representation of an optimized edge-path.

Figure 3.8: 3-dimensional optimized edge-path.

To be able to create a feasible edge-path, we discovered that it requiered that theobstacles that could be approximately considered as points, i.e., that the distance betweenthe obstacles was much greater than the size of the obstacles. If not, the obstacles mightgenerate virtual spheres intersecting with polytope faces. As seen in section 3.1.3, thiswas avoided for the node-path. Unfortunately, infeasible legs were introduced again inorder to find the feasible neighbor pairs when creating the edge-path, illustrated in figure3.9.

18


Neighbornode

Pathnode

Polytope faceInfeasibleedge−path

Neighbornode

Neighbornode

virtualsphere

Pathnode Node−path

Figure 3.9: Illustration an edge-path intersecting a virtual sphere, making it infeasible.

3.2 Rotational Path Planning

After finding a translational path avoiding all obstacles, we have to find a feasible ori-entation path for our rigid body. Assuming the rigid body has a physical extension, therotation path must avoid certain angles φ, such that it does not penetrate any obstacles.For simplicity and greater safety margins, we use our approximation of the obstacles, i.e.,the virtual spheres when finding a feasible rotational path. Furthermore, the translationalpath is followed by the center of mass of the rigid body.

Then, in every point of the translational path, a normal plane disc can be produced,such that its radius is equal to the maximal extension of the rigid body, as seen in figure3.10.

19


Figure 3.10: Translation path avoiding virtual spheres, with normal plane disc visible ingreen.

By checking if the normal plane disc intersects with any of the virtual spheres, in-feasible angles for the rotation are found, as seen highlighted with dark green in figure3.11.

φ φ

Figure 3.11: Normal plane of the translation path with virtual spheres intersecting thedisc representing the maximal extension of the rigid body with infeasible angles in darkgreen.

In turn, this represents obstacles for the angle φ in 2-dimensional rotation space, whichcan be seen illustratively in figure 3.12 and 3.13, where φ ∈ [0, 2π).

20


Figure 3.12: Illustative picture of obstacles (infeasible angles) in rotation space for φ.

By finding a rotational path for φ through the rotation space, a feasible orientationfor the rigid body is found.

Figure 3.13: Illustative picture of rotation path avoiding obstacles (infeasible angles) inrotation space for φ.

Since we have not explicitly seen the geometry of the infeasible angles in rotationspace, it should be noted that figure 3.12 and 3.13 only display an illustrative view of therotation space and its obstacles.

If a feasible path not can be found, i.e., if there at some time exist no feasible angles, then,the translational path planning graph is altered, such that the leg which was infeasiblefor the rotation is removed. The rotational path planning is then repeated.

21

Chapter 4

Control Design

Translational motion is controlled in the coordinates of the world frame FW and rota-tional motion in body frame FB . Then the translational and rotational control problemcan be separated. Since the rotational kinematics (2.8) and dynamics (2.9) are non-linear, we linearize the system around an equilibrium to achieve a linear system. Twotypes of controllers can then be designed for both translational and rotational control,which originates from the theory of optimal controllers[8]. The first one designed will bethe Linear-Quadratic controller for an infinite time interval. The second one is also aLinear-Quadratic controller but with a finite time interval. With time interval, we mean[t0, t1) for which the control law is valid, i.e., the time interval the following functional isminimized:

J(u) =

∫ t1

t0

(x(t)TQx(t) + u(t)TRu(t))dt+ x(t1)TSx(t1), [8] (4.1)

where Q,S ≥ 02 and R > 03 are symmetric. We call these linear feedback controllersLQ infinite and LQ finite respectively.

4.1 State Space Model

The state space description uses state variables xi(t) stored in a state vector x(t), wherei = 1, 2, 3, . . . , n and n is the number of state variables. Given a rational transfer function,there is at least one state space representation[8]. The column vectors x(t), u(t), y(t) arecalled the state-, input- and output vectors.

Definition 4.1.1. [8] The linear state space decription has the formx(t) = A(t)x(t) +B(t)u(t)

y(t) = C(t)x(t) +D(t)u(t), (4.2)

where A(t) ∈ Rn×n, B(t) ∈ Rn×m, C(t) ∈ Rp×n and D(t) ∈ Rp×m. If the matrices areindependent of time t, i.e. they are constant matrices, we call it a time invariant linearsystem.

2Meaning Q,S are positive semidefinite matrices.3Meaning R is a positive definite matrix.

22

CHAPTER 4. CONTROL DESIGN

Definition 4.1.2. [6] The nonlinear state space description has the formx(t) = f(x(t), u(t))

y(t) = h(x(t)), (4.3)

where f and h are function vectors of the same dimension as x(t) and y(t) respectively.

The intention is to find a time invariant linear state space form as in definition 4.1.1for both the translational and rotational dynamics.

4.1.1 Translational System

The input to the translational motion is a force acting on the object according to Newtonslaw of motion ~F = m~a, where we assume a constant mass. The state space descriptionfor the translational motion is described in the world frame FW , where the subindex T isused to denote the translational state space system. If we choose the state vector xT (t)and uT (t) as

xT (t) =

xW (t)yW (t)zW (t)xW (t)yW (t)zW (t)

, uT (t) =

FX,W (t)FY,W (t)FZ,W (t)

, (4.4)

where Fi,W (t) is the applied force in the directions i = X,Y, Z expressed in the worldframe FW . The output yT (t) of the system can be chosen as, for example, the positionof the rigid body or all entries in the state vector xT (t), where the position is needed

for the translation vector ~T . From definition 4.1.1 the complete state space system withyT (t) = xT (t) is

xT (t) = ATxT (t) +BTuT (t)

yT (t) = CTxT (t) +DTuT (t), (4.5)

where the matrices AT , BT are given by

AT =

0 0 0 1 0 00 0 0 0 1 00 0 0 0 0 10 0 0 0 0 00 0 0 0 0 00 0 0 0 0 0

and BT =

0 0 00 0 00 0 01m 0 00 1

m 00 0 1

m

(4.6)

and yT (t) = xT (t), the matrix CT = I6 and the matrix DT = 06,3.

4.1.2 Rotational System

To derive a time invariant rotational state space description, the control law is done inthe body frame FB , where Ixx, Iyy, Izz and m are contant in the description of the

23


rigid body. Since the translational control is done in world frame FW the rotational andtranslational control can be separated.

Let xR(t) denote the rotational system state vector, with the systems states as itsentries. We choose six different states, namely ψ, θ, φ, ωx, ωy, ωz, i.e., xR(t) ∈ R6×1. Toachieve reorientations of the rigid body, the torques Mx,My,Mz about the body frame FBprincipal axes x, y, z, defined in section 2.3, are applied, which implies that uR(t) ∈ R3×1.The output response yR(t) after some input uR(t) could be all entries in xR(t), i.e.,yR(t) ∈ R6×1. Explicitly for our problem we define xR(t), uR(t) and yR(t) as

xR(t) =

ψθφωxωyωz

, uR(t) =

Mx

My

Mz

, yR(t) =

ψθφωxωyωz

. (4.7)

If we take the time derivative of xR(t) and compare it with (2.8) and (2.9), we realize,after some algebraic manipulations, that the equations can be rewritten on the formxR(t) = f(xR(t), uR(t)), i.e.,

xR(t) =

(ωx sinφ+ ωy cosφ)/ sin θ

ωx cosφ− ωy sinφωz − (ωx sinφ+ ωy cosφ)/ tan θ

(Mx − (Izz − Iyy)ωyωz)/Ixx(My − (Ixx − Izz)ωzωx)/Iyy(Mz − (Iyy − Ixx)ωxωy)/Izz

(4.8)

As the chosen output yR(t) contain all variables of xR(t), we can see that a functionh(xR(t)) from the system defined in definition 4.1.2 is a map h : R6×1 → R6×1. Thisimplies that h(xR(t)) is a matrix where h(xR(t)) ∈ R6×6 is defined by h(xR(t)) = I6.Comparing (4.8) and h(xR(t)) = I6 with definition 4.1.2 we can conclude that we have anonlinear rotational state space model. Note that the Euler angles ψ, θ, φ are needed fortransformations between the world frame FW and the body frame FB .

Linearization

For a linearized state space model we need to find an equilibrium that the nonlinearsystem can be linearized about. This equilibrium can also be refered to as a stationarypoint. The linearized system is then a good approxiamtion if the deviations from theequilibrium is sufficiently small. Let the equilibrium be denoted (xR,0, uR,0)[6].

Lemma 4.1.3. [6] If the function f is differentiable in a neighbourhood of the equlibriumpoint (x0, u0), then the system 4.3 can be written as

z = Az +Bv + g(z, v) (4.9)

where z = x−x0, v = u−u0 and g(z, v)/(|z|+ |v|)→ 0 when |z|+ |v| → 0. The matricesA and B have elements aij and bij given by

aij =∂fi∂xj

, bij =∂fi∂uj

(4.10)

24


where fi is the i:th row of f and the derivatives are computed at (x0, u0).

To find an equilibrium (xR,0, uR,0), f(xR,0, uR,0) = 0[6] must be satisfied, where uR,0is a constant input value and xR,0 is a constant vector. From (4.8) we see that a feasibleequilibrium is

xR,0 =

ψ0

θ0φ0000

, uR,0 =

000

, (4.11)

with the restriction θ0 6= nπ, ∀n ∈ Z, i.e., avoiding the Euler angle singularitydiscussed in section 2.1. This equilibrium corresponds to a configuration where thebody frame FB has neither angular rotation velocities nor angular accelerations rela-tive the world frame FW . To linearize the nonlinear dynamical system given by (4.3),we use lemma 4.1.3 and get the new state vector z(t) = xR(t) − xR,0 and input vectorv(t) = uR(t) − uR,0. We also introduce q(t) as the output for the linearized rotationalstate space model. Using the equilibrium given in (4.11) we have

z =

ψ

θ

φωxωyωz

, z =

ψ − ψ0

θ − θ0φ− φ0ωxωyωz

, v =

Mx

My

Mz

. (4.12)

From lemma 4.1.3 we also recieve the matrices AR and BR as

AR =

0 0 0 sin(φ0)sin(θ0)

cos(φ0)sin(θ0)

0

0 0 0 cos(φ0) −sin(φ0) 0

0 0 0 −sin(φ0)tan(θ0)

−cos(φ0)tan(θ0)

1

0 0 0 0 0 00 0 0 0 0 00 0 0 0 0 0

,

BR =

0 0 00 0 00 0 01Ixx

0 0

0 1Iyy

0

0 0 1Izz

.

(4.13)

Note that CR = I6 and DR = 06,3 when q(t) = z(t) using the same arguments fromsection 4.1.1. The linearized state space model for the rotational dynamics is then

z(t) = ARz(t) +BRv(t)q(t) = CRz(t) +DRv(t)

. (4.14)

25


4.2 Reachability

To design a feedback control law for the translational and linearized rotational state spacesystems we have to determine if the systems (4.5) and (4.14) are reachable[8].

Definition 4.2.1. [8] Let (A,B) be a pair of matrices, where A is an n × n −matrix.The reachability matrix Γ is then defined as

Γ =(B AB A2B · · · An−1B

). (4.15)

Lemma 4.2.2. [8] If the reachability matrix has full rank, then the system correspondingto Γ is reachable.

Since the systems (4.5) and (4.14) are linear and time invariant systems we can uselemma 4.2.2 to determine if Γ has full rank. Both AT as in (4.6) and AR as in (4.13) arenilpotent4 matrices. This implies that Aj = 06,6, ∀j ≥ 2. For the translational systemthe reachability matrix ΓT is given by

ΓT =

(03,3 I3 03,121m · I3 03,3 03,12

). (4.16)

ΓT has 6 rows and 6 linearly independent row vectors, thus it has full rank[11], andhence the system is reachable. Analyzing the linearized rotational system (4.14) resultsin ΓR given by

ΓR =

0 0 0 sinφ0

Ixx sin θ0

cosφ0

Iyy sin θ00 01,12

0 0 0 cosφ0

Ixx− sinφ0

Iyy0 01,12

0 0 0 − sinφ0

Ixx tan θ0− cosφ0

Iyy tan θ01Izz

01,121Ixx

0 0 0 0 0 01,120 1

Iyy0 0 0 0 01,12

0 0 1Izz

0 0 0 01,12

. (4.17)

Let ΓR be the first six columns of ΓR, then

det ΓR = (sin2 φ0 + cos2 φ0)/(Ixx2Iyy

2Izz2 sin θ0) 6= 0. (4.18)

This holds for all φ0 and is well posed for Ixx, Iyy, Izz 6= 0 and θ0 6= nπ, n ∈ Z. Then

ΓR has full row rank, which implies that ΓR has full row rank[11], hence the linearizedrotational state space system (4.14) is reachable.

4.3 Observability

In this section the observability of the system is determined, i.e., if the complete statex(t) of a linear system can be constructed from a given input u(t) and output y(t)[8].

4The lower triangular part and diagonal elements are all 0

26


Definition 4.3.1. [8] Let (C,A) be a pair of matrices, where A is an n × n −matrix.The observability matrix Ω is then defined as

Ω =

CCA

...CAn−1

. (4.19)

Lemma 4.3.2. [8] Let n be the dimension of the state space. The pair (C,A) is said tobe completely observable if Ω has full rank, i.e.,

rank Ω = n. (4.20)

For the translational system (4.5) the observability matrix ΩT is on the form

ΩT =

I3 03,3

03,3 I303,3 I3027,3 027,3

. (4.21)

ΩT has six linearly independent columns, which implies that ΩT has rank 6[11] andthereby the translational system is observable. The observability matrix ΩR for thelinearlized rotational system has the form

ΩR =

1 0 0 0 0 00 1 0 0 0 00 0 1 0 0 00 0 0 1 0 00 0 0 0 1 00 0 0 0 0 10 0 0 sinφ0/ sin θ0 cosφ0/ sin θ0 00 0 0 cosφ0 − sinφ0 00 0 0 − sinφ0/ tan θ0 − cosφ0/ tan θ0 1

027,1 027,1 027,1 027,1 027,1 027,1

. (4.22)

Using elementary row operations on (4.22), ΩR can be written with 6 linearly independentcolumn vectors, which implies that rank ΩR = 6[11] and the linearized rotational systemis observable.

4.4 Linear Quadratic Controller on an Infinite TimeInterval

With infinte time inteval we mean [t0,∞). From the objective function (4.1) which isminimized when deriving a LQ controller, we see that (4.1) is minimized on an infinitetime interval.

To determine if we have unnecessary states in our realization we need to determine ifthe system is minimal, i.e., if the system is a minimal realization. By unnecessary states

27


we mean states that cannot be reached and observed. This means that a natural statecandidate to keep is the part that is in the reachable subspace and not in the unobservablesubspace[8].

Lemma 4.4.1. [8] A minimal realization is reachable and observable.

The converse of lemma 4.4.1 also holds[8]. From section 4.2 and 4.3, both systems arereachable and observable, which implies that (4.5) and (4.14) systems are minimal.

4.4.1 Algebraic Riccati Equation

When minimizing the objective function (4.1) when t1 → ∞ the resulting problem is tosolve the Algebraic Riccati Equations.

Definition 4.4.2. [8] The Algebraic Riccati Equations (ARE) is given by

0 = ATP + PA− PBR−1BTP +Q, (4.23)

where Q ≥ 0 and R > 0 are as in (4.1).

Lemma 4.4.3. Suppose that the system is a minimal realization. Then ARE has a realsymmetric positive definite solution.

To illustrate two example solutions to ARE, one for the rotational system (4.5) andone for the linearized rotational system (4.14), we choose

Q =

1 0 0 0 0 00 1 0 0 0 00 0 1 0 0 00 0 0 0 0 00 0 0 0 0 00 0 0 0 0 0

, R =

1 0 00 1 00 0 1

, (4.24)

m = Ixx = Iyy = Izz = 1

(ψ0, θ0, φ0, ωx, ωy, ωz) = (0, π/2, 0, 0, 0, 0). (4.25)

The corresponding P matrices are then

PT =

√2 0 0 1 0 0

0√

2 0 0 1 0

0 0√

2 0 0 1

1 0 0√

2 0 0

0 1 0 0√

2 0

0 0 1 0 0√

2

, (4.26)

PR =

√2 0 0 0 1 0

0√

2 0 1 0 0

0 0√

2 0 0 1

0 1 0√

2 0 0

1 0 0 0√

2 0

0 0 1 0 0√

2

. (4.27)

28


System

−R−1BTP

u(t) x(t)

y(t)

Figure 4.1: A block diagram representation of the system with a feedback loop.

From figure 4.1 the linear feedback control signal is

u(t) = −R−1BTPx, (4.28)

where P is the matrix PT or PR, and x is the state vector for the respective problems.In explicit form we have

uT (t) = −R−1BT TPTxT (t) =

−xW (t)− xW (t)√

2

−yW (t)− yW (t)√

2

−zW (t)− zW (t)√

2

, (4.29)

v(t) = −R−1BRTPRz(t) =

−(θ(t)− θ0)− ωx(t)√

2

−(ψ(t)− ψ0)− ωy(t)√

2

−(φ(t)− φ0)− ωz(t)√

2

. (4.30)

4.4.2 Change of Reference Point

The concept of the LQ infinite regulator is that the system starts at rest in state x0 attime t0 and converges to state x1 as t1 →∞. To get the system to visit a set of referencestates, a time dependant path can be constructed, i.e., a trajectory. The reference statecan then be changed at some time tk, where t0 ≤ tk < ∞. Unfortunaetly if we changereference state with respect to a certain time, we either penetrate the constructed poly-topes from chapter 3 more than necessary, or we use more than necessary input energyto decrease the kinetic energy of the rigid body.

Instead of changing reference state at a certain time, we chose to change reference statewhen some state variables are within a tolerance distance. Then, for the translationalproblem we can construct tolerance spheres around the reference positions. The rigidbody can then preserve some of the kinetic energy when changing reference state. Thisidea is illustrated in figure 4.2.

29


Figure 4.2: When the position of the rigid body is within a tolerance sphere, the referencepositionis changed.

The radius of the tolerance spheres can be chosen in several ways. For the illustrationin figure 4.2, the tolerance radius of the spheres was chosen to be 10% of the precedingleg distance in the path. This technique can also be used for the rotational problem. Wethen change reference Euler angles when the angles ψ, θ, φ are within some angle interval,i.e., ψref − ξ ≤ ψ ≤ ψref + ξ

θref − ζ ≤ θ ≤ θref + ζφref − χ ≤ φ ≤ φref + χ

, (4.31)

where ξ, ζ, χ are some tolerance angles for the respective Euler angles. Also note thatthe rotational system is linearized around the reference state, i.e., a linearized model isneeded for at least each reference state, if we intend to have good behavior of the systemdynamics around reference state. In case of large state differences between initial stateand reference state, we might consider using more than one linearization along this path.

4.5 Linear Quadratic Controller on a Finite Time In-terval

We now let [t0, t1] be a finite time interval as in (4.1). This means that we minimize theobjective function (4.1) when t0 ≤ t ≤ t1, and do not consider any outcome for t > t1.This results in solving the Matrix Differential Riccati equation[8].

30


4.5.1 Matrix Differential Riccati Equation

Definition 4.5.1. [8] The Matrix Differential Riccati Equation (MRE) is given byP = −ATP − PA+ PBR−1BTP −QP (t1) = S

, (4.32)

where Q,S are poitive semidefinite matrices and R is positive definite.

Here, (4.32) is a nonlinear matrix differential equation. This can be solved by expand-ing the dimension of the differential equation system, resulting in an adjoint system.

Definition 4.5.2. [8] The adjoint system to MRE can be written asX = AX −BR−1BTY ; X(t1) = I

Y = −QX −ATY ; Y (t1) = S, (4.33)

where Q,R and S are the matrices in (4.1). Then P = Y X−1.

When solving the linear differential equation system in definition 4.5.2 for either thetranslational or rotational controller, X,Y, P ∈ R6×6, the solution matrix P (t) to (4.32)becomes extensive. When solving (4.32) for the translational system with m = 1, Q = 06,6and S given by

ST =

1 0 0 0 0 00 1 0 0 0 00 0 1 0 0 00 0 0 0 0 00 0 0 0 0 00 0 0 0 0 0

, (4.34)

the resulting PT (t) matrix is:

PT (t) = C ·

1 0 0 2− t 0 00 1 0 0 2− t 00 0 1 0 0 2− t

2− t 0 0 (t−2)21 0 0

0 2− t 0 0 (t−2)21 0

0 0 2− t 0 0 (t−2)21

,

where C =3m2

(3m2 − t3 + 6 t2 − 12 t+ 8).

(4.35)

The matrix PT (t) given in (4.35) is used in chapter 5 for the translational trajectoryfollowing simulation.

31


For the rotational system (4.14), the solution matrix PR(t) to MRE becomes even moreextensive5. We use the same technical arguments with the linearization around the ref-erence state as we applied in the end of section 4.4.2. For the rotational problem we useSR given by

SR =

1 0 0 0 0 00 1 0 0 0 00 0 1 0 0 00 0 0 1 0 00 0 0 0 1 00 0 0 0 0 1

. (4.36)

The physical difference between ST and SR is that at the reference state x1 at time t1,the translational controller only intend to minimize the squared physical position distanceerror at time t1 together with the input energy, with no mathematical restrictions on thevelocity. The rotational controller intend to minimize both the squared angle error andthe squared angular velocity error at time t1 together with input energy.

The feedback signal u(t) is constructed according to figure 4.1 and (4.28) with thematrix P (t) that solves MRE.

4.5.2 Trajectory

To change from one state x0 at time t0 towards another state x1 at time t1, i.e., followinga time dependant path, is called a trajectory. The LQ finite controller uses a time variantfeedback, which can be a good choice when following a trajectory.

4.6 Stability

Theorem 4.6.1. [8] Letx = Ax; x(0) = x0, (4.37)

where A ∈ Rn×n is constant.

(1) The system (4.37) is asymptotically stable iff the real parts of alleigenvalues of A are negative, i.e., the eigenvalues are all located in theopen left half plane.

(2) The system (4.37) is unstable if A has at least one eigenvalue inthe open right half plane.

With the A matrix in theorem 4.6.1 as AT in (4.6) and AR in (4.13), analyzing thesystem poles, we can see that both systems have all poles in the origin. Using theorem4.6.1, we can conclude that the system is unstable. When introducing the LQ infiniteregulator from (4.29) and (4.30) to each corresponding system, the closed loop systemcan be written on the form

x(t) = (A−BR−1BTP )x. (4.38)

5Meaning it contains more variables and constants.

32


Lemma 4.6.2. [8] If the system is a minimal realization and if P is a real symmetricpositive definite solution to ARE, then

A−BR−1BTP (4.39)

is a stable matrix.

From lemma 4.6.2, implementing the LQ infinite to minimal systems, the closed loopsystem is stable.

When analysing the system using a LQ finite feedback regulator, the system doesnot converge to the reference state as t → ∞, and thus does not display stabilitycharacteristics[6].

33

Chapter 5

Simulations

To illustrate some results from the theory given in the previous chapters, we intend tosimulate some conditions in this section. In the first part of this chapter we simulate themethod given in chapter 3 with obstacles uniformly distributed in the simulated space.In the second part we simulate the control laws designed in chapter 4, with main focus onthe rotational motion due the applicability of the linearized feedback loop of a nonlinearsystem.

5.1 Simulation of Graph Creation

The creation of a graph from the Voronoi tessellation is done in three steps. First a setis created, with indices declaring which polytopes are connected to each node. Secondly,all feasible legs to the graph are found and weighed correctly. Finally, this is remadeto a sparse matrix in order to be able to use a built in graph searching algorithm inMATLAB R© using Dijkstra’s Algorithm. We simulted the graph creating method byexecuting the algorithm several times with different amounts of obstacles and timed thedifferent parts of the method to be able to analyze the complexity.

0 5 10 15 20 25 30 35 400

5

10

15

20

25

30

35

40

45

Number of obstacles

Tim

e [s]

First step values

First step mean value curve

Second step values

Second step mean value curve

Third step valures

Third step mean value curve

Figure 5.1: Computional time comparison between steps in graph creation.

34

CHAPTER 5. SIMULATIONS

As seen in figure 5.1, the first part (green) and third part (magenta) does not impact onthe complexity since the second part of the method has a considerably higher complexity.We therefore focused more on the complexity of the second part of the graph creatingmethod.

0 5 10 15 20 25 30 35 400

5

10

15

20

25

30

35

40

45

Number of obstacles

Tim

e [s]

Measured time values

Mean value curve

Fitted n3−curve

Figure 5.2: Computional time of the middle step of the graph creation compared to an3-curve.

5.2 Path Improvement Simulation

To collect data the path improvment method was executed 100 times with a specificnumber of random obstacles in a 3-dimensional space. The chosen numbers of obstacleswere 25, 37, 50, 63 and 75. When examining the results of this simulation, we couldsee that the number of nodes in the node-path did not depend much on the number ofobstacles. This is because some environments with many obstacles sometimes generatesa short node-path, in respect to the number of nodes, and vice versa. We therefore variedthe number of obstacles in order to obtain a bigger variation in the number nodes in thenode-path. This is illustrated in figure 5.3.

35


4 5 6 7 8 9 100

1

2

3

4

5

Number of nodes in path

Tim

e [s]

Values at different path lenghts

Mean value curve

Figure 5.3: Computional time to generate all combinations of feasible sets of neighbors.

For N measured values ti and an estimated expected value t given by the mean value,the standard deviation can be calculated with

Sz =

√√√√ 1

N

N∑i=1

(ti − t)2 [12], (5.1)

where z is the number of nodes in the node-path. With the values seen in figure 5.3, thefollowing standard deviations Stz of the computional time were obtained:

St5 = 0.309

St6 = 0.401

St7 = 0.441

St8 = 0.647

St9 = 0.944

St10 = 0.609

(5.2)

The mean value Sttot of these standard deviations was then

Stot =S5 + S6 + S7 + S8 + S9 + S10

5= 0.558. (5.3)

Furthermore, the mean value of the percentual improvement of the path length was2.811 % and the estimated standard deviation of this improvement was 2.5 percent points.

The calculation time of the optimization when calculating the edge-path was not con-sidered since a built in toolbox in MATLAB R© was used.

36


5.3 Control Law Simulation

For the simulation purpose a initial state and a referens state is needed. Let x0 correspondto the initial state at time t0 = 0, and x1 the referens state at time t1. The LQ infintetime interval controller referense state x1 corresponds to the reference state when t1 →∞,thus for simulation and purpose the end time for the simulation will be t = 10 on thetime axis. The same end time for the time axis will be used for all simulations, includingthe LQ finite time interval controller, thus resulting in a consistent illustration. For thetranslational and rotational simulations we set the mass and inertia of the object to 1,i.e., as we did in (4.25)

5.3.1 Translation

In the first case we simulate the object starting at rest in the origin of the world frame,with the reference position (X,Y, Z) = (1, 2, 3), then

x0 =

000000

, x1 =

123000

. (5.4)

The simulation result of the translational system (4.5) with the LQ infinity feedbackcontroller (4.29) and the initial and referense state given by (5.4) is given in figure 5.4.

37


0 1 2 3 4 5 6 7 8 9 10−1

0

1

2

3

4System states

Time

Positio

n a

nd v

elo

city

x

y

zv

x

vy

vz

0 1 2 3 4 5 6 7 8 9 10−1

0

1

2

3

4Applied force

Time

Input fo

rce

F

x

Fy

Fz

Figure 5.4: A simulation of the translational system with a LQ infinite feedback controllerwith x0 and x1 given by (5.4).

Note that the three states in x1 that equals 0 do not influens the control law since STis choosen as (4.34) for the LQ finite feedback controller. The result for a translationalsimulation using the LQ finite feedback controller (4.35) is seen in figure 5.5.

38


0 1 2 3 4 5 6 7 8 9 100

0.5

1

1.5

2

2.5

3System states

Time

Positio

n o

ch v

elo

city

0 1 2 3 4 5 6 7 8 9 100

0.02

0.04

0.06

0.08

0.1Applied force

Time

Input fo

rce

x

y

zv

x

vy

vz

Fx

Fy

Fz

Figure 5.5: A simulation of the translational system with the LQ finite feedback controller(4.35) using x0 and x1 given by (5.4).

5.3.2 Rotation

For the rotational simulation we have a fixed reference state x1 at time t1 = 10. Theinitial condition will vary to demonstrate the behavior of the nonlinear system whendeviating from the equilibrium x1 with different deviations of some states. Let x0,S be astate close to the equilibrium x1, and x0,L be a state with larger angle deviations fromthe equilibrium x1. These initial states and reference state is given by

x0,S =

−0.5

20.3000

, x0,L =

−311000

, x1 =

0π/20000

. (5.5)

39


The results for a simulation using x0,S as an initial state can be seen in figure 5.6for the LQ infinite controller and in figure 5.7 the same conditions using the LQ finitecontroller.

0 1 2 3 4 5 6 7 8 9 10−0.5

0

0.5

1

1.5

2System states

Time

Angle

s a

nd a

ngula

r velo

city

psi

theta

phiomega

x

omegay

omegaz

0 1 2 3 4 5 6 7 8 9 10−0.5

0

0.5Applied torque

Time

Input to

rque

M

x

My

Mz

Figure 5.6: A simulation of the rotational system with a LQ infinite feedback controllerwith x0,S and x1 given by (5.5).

40


0 1 2 3 4 5 6 7 8 9 10−0.5

0

0.5

1

1.5

2System states

Time

Angle

and a

ngula

r velo

city

psi

theta

phiomega

x

omegay

omegaz

0 1 2 3 4 5 6 7 8 9 10−0.03

−0.02

−0.01

0

0.01

0.02

0.03Applied torque

Time

Input to

rque

M

x

My

Mz

Figure 5.7: A simulation of the rotational system with a LQ finite feedback controllerwith x0,S and x1 given by (5.5).

When analyzing small deviations from the equilibrium x1 where the feedback controlleris linearized, one can see that the closed loop system behaves well. If we now use x0,L in(5.5) as the initial condition, the simulations can be seen in figure 5.8 for the LQ infinitecontroller and in figure 5.9 for the LQ finite controller.

41


0 1 2 3 4 5 6 7 8 9 10−3

−2

−1

0

1

2System states

Time

Angle

s a

nd a

ngula

r velo

city

psi

theta

phiomega

x

omegay

omegaz

0 1 2 3 4 5 6 7 8 9 10−1

0

1

2

3Applied torque

Time

Input to

rque

M

x

My

Mz

Figure 5.8: A simulation of the rotational system with a LQ infinite feedback controllerwith x0,L and x1 given by (5.5).

42


0 1 2 3 4 5 6 7 8 9 10−3

−2

−1

0

1

2System states

Time

Angle

and a

ngula

r velo

city

psi

theta

phiomega

x

omegay

omegaz

0 1 2 3 4 5 6 7 8 9 10−0.15

−0.1

−0.05

0

0.05

0.1

0.15

0.2Applied torque

Time

Input to

rque

M

x

My

Mz

Figure 5.9: A simulation of the rotational system with a LQ finite feedback controllerwith x0,L and x1 given by (5.5).

If even larger angle deviations for the initial state from the reference point is consideredthe system can achieve unstable characteristics. This can be seen in figure 5.10 where

x0,U =

−20.322.3000

, x1 =

0π/20000

. (5.6)

43


0 1 2 3 4 5 6 7 8 9 10−300

−200

−100

0

100

200System states

Time

Angle

s a

nd a

ngula

r velo

city

psi

theta

phiomega

x

omegay

omegaz

0 1 2 3 4 5 6 7 8 9 10−20

0

20

40

60

80Applied torque

Time

Input to

rque

M

x

My

Mz

Figure 5.10: An unstable simulation of the rotational system with a LQ infinite feedbackcontroller with x0,U and x1 given by (5.6).

44

Chapter 6

Conclusions

In a given space with obstacles, if a feasible path existed, our algorithm would always beable to find a path connecting the start point with the end point. Control laws were thenimplemented to make sure that an object followed this trajectory.

During the process of creating our algorithm, we laid an emphasis on making thealgorithm successfull, resulting in a higher complexity than optimal. However, we foundit more important to actually find a path than to optimize the process.

The method used in section 3.1.2 is not even considered in an analysis of complexitysince it only depends on the tool which solves the given optimization problem. Since wehave used a built-in tool in MATLAB R©, this is not of interest to us. The same applies tothe built-in Voronoi tool in MATLAB R©, used when creating the Voronoi tessellation.

However, the method of creating a graph of feasible paths from this tessellation is ofinterest, since it is entirely self-produced. As seen in figure 5.2, this method seems tohave an experimental O(n3)-complexity.

As presented in section 3.1.4, the generated edge-path is based on the shortest node-path. This path improvement always results in an equally short or shorter path. Allfeasible node-paths are not evaluated, this can result in that an edge-path, based on alonger node-path, possibly, but unlikely, is shorter than the edge-path corresponding tothe shortest node-path.

To obtain an optimal edge-path for all node-paths, the path improvement has to bedone on each node-path possible in the given graph. This is possible, but will not be con-sidered in this report due to a considerable higher complexity. Therefore, the edge-pathin this report is determined as suboptimal.

Unfortunately, the introduction of the virtual spheres resulted in unexpected compli-cations with the path improvement. Since the Voronoi tessellation is based on the centerpoints of virtual spheres, an intersection between a sphere and the corresponding bound-ary of a polytope is possible if the sphere is large enough. To avoid this, these infeasiblelegs, intersecting the spheres, were eliminated from the graph, such that the node-pathwould always be feasible.

However, this was not accounted for in the path improvement. This is because in-feasible legs were introduced again when finding all feasible pairs of neighbors, risking in

45

CHAPTER 6. CONCLUSIONS

an edge-path intersecting a virtual sphere. Therefore, the path improvement only workswith obstacles approximate to points, i.e., obstacles whose size is much smaller than thedistance between the obstacles.

The complexity of the algorithm used for generating the edge-path is evaluated in chapter5. The worst case scenario, that is, a graph consisting of n nodes located in the samepolytope face forming a convex surface, where each path-node allows all the other n− 1nodes as its neighbours. Then, if the node-path consists of all n nodes in the graph, theresult will be (n − 1)n feasible neighbor combinations. If we assume the time neededto calculate the optimal solution for all feasible sets are equal, the complexity would beO((n− 1)n).

From figure 5.3, the complexity does not seem to accelerate according to O((n− 1)n)with a finite number of uniformly scattered obstacles in a finite space. In our simula-tions the number of nodes in the whole graph were approximately a few hundred and thenumber of neighbors corresponding to each node was then approximately 5, making thecomplexity decreasing to approximately O(5n).

The results plotted in figure 5.3 seem to be of even lower character, most likely due tolimitations in the comparison between the feasible pairs of neighbors. The fact that theyhave to fulfill the condition of being located in the same face results in a lower complexitythan O(5n).

The complexity was mostly a problem that occured in the graph searching part of thealgorithm development. However, when concentrating on the other parts such as the rigidbody and the control design, other problems had to be dealt with.

With the rotation matrix R, derived as in chapter 2, the translation dynamics representedin the world frame can be transformed to a body frame representation, and quantitiesrepresented in body frame can be transformed to world frame, for example, if velocitiesare measured in the body frame, they can be transformed to world frame coordinates andused for the translational control law. The inverse transformation is also possible andcould be used for the control signal, transforming it from the world frame to the bodyframe.

The Linear-Quadratic regulator satisfied the aim of the control design stated in the prob-lem formulation. When controlling the rotational dynamics, the results are similar tothe linear translational dynamics when deviations from the equilibrium state, where therotational system is linearized about, are small enough. When the deviation from theequilibrium is large, the system shows a fluctuating behavior. For certain deviations fromequilibrium, the system could show an unstable behavior.

During the control design it was assumed that all states could be measured. In manyapplications this is not true. E.g., the Euler angles could be hard to measure if the rigidbody is accelerating. Thus these states need to be estimated. This can be achieved byimplementing an observer estimating these unmeasurable states. If an optimal observeris desired, a Kalman filter could be implemented.

46

Chapter 7

Discussion

In this report we decided to use MATLAB R© as our primary tool when performing cal-culations. The difficulties in handling sets caused many problems, mainly when devel-oping methods for finding a path, both translational and rotational. Another result ofMATLAB R©’s limitations were high complexities in the algorithm. Some of this complex-ity could possibly be avoided with more time and greater programming knowledge.

In the path improvement, a combinatorial problem had to be solved among the feasi-ble pairs of neighbours, resulting in a complexity that highly depended on the length ofthe node-path. The consequence of this was an exponential growth. We also discoveredthe problem concerning the path improvement which declared this method useful only ifwe had obstacles that could be approximately considered as points.

The path improvement always resulted in an equally short or shorter edge-path. Dur-ing the measurements the maximum improvement was around 18 percent. One edge-paththat equaled the node-path was also represented, i.e., there was no improvement in thatcase. However, the mean value of the improvement during the whole measurement was2.811 percent, which is rather low.

The decision concerning the use of the Euler angles resulted in problems with the singu-larity in SO(3) for the Euler angle sequence of rotations as well in unnecessarily manynonlinear equations. To avoid these problems another angle representation, e.g., quater-nions could have been chosen.

47

Bibliography

[1] Anton, H. Elementary Linear Algebra with Applications. 9th ed. New York, USA:Wiley; 2005.

[2] Apazidis, N. Rigid Body and Analytical Mechanics. 2nd ed. Stockholm, Sweden: RoyalInstitute of Technology; 2010.

[3] Bump, D. Lie Groups. New York, USA: Springer; 2004.

[4] Chang, SK. Data Structures and Algorithms. Singapore, Singapore: World ScientificPublishing; 2003.

[5] Diestel, R. Graph Theory. 3rd ed. Berlin, Germany: Springer; 2006

[6] Glad, T, Ljung L. Control Theory. London, Great Britain: Taylor & Francis; 2000.

[7] Gruber, PM. Convex and Discrete Geometry. Dordrecht, Germany: Springer; 2007.

[8] Hu, X, Lindquist, A, Sand, J. An Introduction to Mathematical Systems Theory.Stockholm, Sweden: Royal Institute of Technology; 2010.

[9] Jones, A, Morris, SA, Pearson, KR. Abstract Algebra and Famous Impossibilities.2nd ed. New York, USA: Springer; 1991.

[10] Reem, D. An Algorithm for Computing Voronoi Diagrams of General Generatorsin General Normed Spaces. Paper presented at: ISVD ’09. The Sixth InternationalSymposium on Voronoi Diagrams; 23-26 June 2009; Copenhagen, Denmark.

[11] Roman, S. Advanced Linear Algebra. 2nd ed. New York, USA: Springer; 2005.

[12] Rade, L, Westergren R. Mathematics Handbook for Science and Engineering. 5thed. Lund, Sweden: Studentlitteratur; 2004.

[13] Sasane, A, Svanberg, K. Optimization. Stockholm, Sweden: Royal Institute of Tech-nology; 2012.

[14] Schobel, A. Locating Lines and Hyperplanes. Dordrecht, Germany: Springer; 1999.

[15] Tan, CJK. Transactions on Computational Science IX. Berlin, Germany: Springer;2010.

[16] Willard, S. General Topology. New York, USA: Courier Dover Publications; 2004.

[17] Zakon, E. Mathematical Analysis I. Indiana, USA: The Trillia Group; 2004.

48

Appendix A

Proofs

A.1 Path Planning

The Euclidean n-space is a metric space

Definition A.1.1. [17] The Euclidean n-space En is the set of all possible ordered n-tuples of real numbers, i.e., the Cartesian product

E1 × E1 × ...× E1 (n times), (A.1)

where E1 is the set of real numbers, commonly denoted as R. The inner product, alsoknown as the dot product, between x, y ∈ En is

〈x, y〉 =

n∑k=1

xkyk, (A.2)

the norm, also known as the length, of x ∈ En is

‖x‖ =√〈x, x〉, (A.3)

and the distance between x, y ∈ En is

d(x, y) = ‖x− y‖. (A.4)

Furthermore, our following definitions demand us using a metric space, which is de-fined as follows:

Definition A.1.2. [16] A metric space is an ordered pair (S,d) consisting of a set M anda function d : S × S → R on S, satisfying, for all x, y, z ∈ S:

1) d(x, y) ≥ 0 (non-negative),

2) d(x, y) = 0 iff x = y (identity of indiscernibles),

3) d(x, y) = d(y, x) (symmetry),

4) d(x, z) ≤ d(x, y) + d(y, z) (triangle inequality).

(A.5)

The function d is called the metric on S and is also know as the distance function.

49

APPENDIX A. PROOFS

We therefore need to prove that the Euclidean n-space is a metric space.

Lemma A.1.3. The Euclidean n-space, with the ordered pair (Rn, d), with Rn and d asabove, is a metric space.

Proof. Since all x, y∈ Rn we have that xk, yk ∈ R ∀k ∈ K, which gives us that (xk−yk)2 ≥0 ∀k ∈ K. Therefore √√√√ n∑

k=1

(xk − yk)2 ≥ 0 (A.6)

and d(x, y) ≥ 0, i.e., the metric fulfills non-negativity.Furthermore,

d(x, y) = 0 iff

n∑k=1

(xk − yk)2 = 0

⇒ d(x, y) = 0 iff (xk − yk)2 = 0 ∀k⇒ d(x, y) = 0 iff xk = yk ∀k⇒ d(x, y) = 0 iff x = y,

(A.7)

i.e., the identity of indiscernibles is fulfilled.Since

√(xk − yk)2 =

√(yk − xk)2, then ‖x − y‖ is symmetric, and therefore d(x, y)

is symmetric, i.e., d(x, y) = d(y, x).Additionally, since d(x, y) + d(y, z) = ‖x− y‖+ ‖y − z‖, we have

(d(x, y) + d(y, z))2

=(‖x− y‖+ ‖y − z‖)2

=‖x− y‖2 + 2‖x− y‖‖y − z‖+ ‖y − z‖2.(A.8)

With the Cauchy-Swartz inequality[1], we get

‖x− y‖2 + 2‖x− y‖‖y − z‖+ ‖y − z‖2

≥‖x− y‖2 + 2〈x− y, y − z〉+ ‖y − z‖2=〈x− y, x− y〉+ 2〈x− y, y − z〉+ 〈y − z, y − z〉

=

⟨(x− y) + (y − z), (x− y) + (y − z)

⟩=‖x− y + y − z‖2

=‖x− z‖2 = (d(x, z))2.

(A.9)

Since d(x, y), d(x, z), d(y, z) ≥ 0 we have

d(x, z) ≤ d(x, y) + d(y, z), (A.10)

i.e., the triangle inequality in (A.5) is fulfilled. We can therefore conclude that theEuclidean n-space is a metric space.

50

APPENDIX A. PROOFS

Proof of Lemma 3.1.4

Proof. Let the sets A,B ⊂ Rn be convex and let x, y ∈ A ∩ B. Since x, y ∈ A ∩ B, thenx, y ∈ A and x, y ∈ B, and therefore,

(1− t)x+ ty ∈ A and (1− t)x+ ty ∈ B ∀t ∈ [0, 1]. (A.11)

Then,(1− t)x+ ty ∈ A ∩B ∀t ∈ [0, 1], (A.12)

and therefore the intersection A ∩B is convex.

Proof of Lemma 3.6

Proof. Let A =[Ai,j

]m×n and let b = (b1, b2, . . . , bm)T . Then Ax ≤ b is equivalent to

m⋂i=1

( n∑j=1

(Ai,jxj − bj) ≤ 0

), (A.13)

i.e., if Si = x ∈ Rn :∑nj=1(Ai,jxj − bj) ≤ 0 is convex, then, by lemma 3.1.4, S is

convex.Let x, y ∈ Si. We then need to prove that (1− t)x+ ty ∈ Si ∀t ∈ [0, 1].

n∑j=1

(Ai,j

[(1− t)xj + tyj

]− bj

)

=

n∑j=1

(Ai,j

[(1− t)xj + tyj

]−[(1− t)bj + tbj

])

=

n∑j=1

((1− t)

[Ai,jxj − bj

]+ t

[Ai,jyj − bj

])

=(1− t)n∑j=1

(Ai,jxj − bj

)+ t

n∑j=1

(Ai,jyj − bj

)(A.14)

Since t ∈ [0, 1], then (1 − t), t ≥ 0, and since x, y ∈ Si, both sums are non-positive. Wetherefore have that

n∑j=1

(Ai,j

[(1− t)xj + tyj

]− bj

)≤ 0, (A.15)

i.e., (1− t)x+ ty ∈ Si, which means that Si and therefore also S is convex.

Proof of Proposition 3.1.8

Proof. The distance between two arbitrary points y, z∈ Rn is given by the metric

d(y, z) =

√√√√ n∑k=1

(yk − zk)2. (A.16)

51

APPENDIX A. PROOFS

The set of all equidistant points between y and z is then given by

Deq = x ∈ Rn : d(y, x) = d(z, x). (A.17)

Furthermore, d(y, x) = d(z, x) is equivalent to√√√√ n∑k=1

(yk − xk)2 =

√√√√ n∑k=1

(zk − xk)2

⇐⇒n∑k=1

(y2k − 2ykxk + x2k) =

n∑k=1

(z2k − 2zkxk + x2k)

⇐⇒n∑k=1

(y2k − z2k) =

n∑k=1

2(yk − zk)xk

⇐⇒n∑k=1

2(yk − zk)xk −n∑k=1

(y2k − z2k) = 0.

(A.18)

The set Deq of all equidistant points can therefore be written as

Deq =

x ∈ Rn :

n∑k=1


(y2k − z2k) = 0

, (A.19)

which is the same form as in (3.1.6), i.e., Deq is a hyperplane with the normal vector

a = (a1, a2, . . . , an) : ak = 2(yk − zk) (A.20)

and b-value

b = −n∑k=1

(y2k − z2k) (A.21)

Let the dominance region of y in respect to z be denoted as Xy,z. Then, from (3.1) and(A.19), we get an expression for Xy,z given by

Xy,z =

x ∈ Rn :

n∑k=1


(y2k − z2k) ≥ 0

⇐⇒ Xy,z =

x ∈ Rn : −

n∑k=1

2(yk − zk)xk ≤ −n∑k=1

(y2k − z2k)

(A.22)

It is easy to see that this is a special case of (3.6) with m = 1, i.e., these are linearconstraints. Then, by lemma 3.1.5, Xy,z is convex. Furthermore, Xy,z is the half-spacewhich is closer to y than z.

Let P = pk : k ∈ K ⊂ Rn be a set of m points, where pk = (pk,1, pk,2, . . . , pk,n) ∈Rn, K is a set of m elements (indices) and m is finite. Then Xk,j is the half-space whichis closer to pk than to pj given by

Xk,j =

x ∈ Rn : −

n∑i=1

2(pk,i − pj,i)xk ≤ −n∑i=1

(p2k,i − p2j,i), k 6= j. (A.23)

52

APPENDIX A. PROOFS

The intersectionXk =

⋂j∈K,j 6=k

Xk,j (A.24)

then is the space which is closer to pk than to any other pj , j ∈ K, j 6= k, i.e., thek:th Voronoi cell. Since (A.24) is an intersection of half-spaces, all half-spaces are convexdue to their linear conditions and this is true ∀k ∈ K, then we can conclude that everyVoronoi cell is a convex polytope.

Proof of Corollary 3.1.9

Proof. Let the set Xk be a polytope in the Voronoi tessellation as in (A.24). The inter-section Sk,j = Xk ∩Xj , j ∈ K, k 6= j between two convex polytopes Xk, Xj ⊂ Rn is theneither the empty set ∅, or a hyperplane shared between both polytopes, depending on ifthe two polytopes are neighbors or if they are not. Since Xk is convex ∀k ∈ K, then theintersection Sk,j is convex ∀k, j ∈ K, k 6= j.

We then have that, if Sk,j 6= ∅, then Sk,j is the shared hypersurface of two neighboringconvex polytopes Xk, Xj , i.e., one of the hypersurfaces of both Xk, Xj .

Since all Xk are sets with linear conditions, then any intersection Sk,j also consists oflinear conditions, and is therefore also a polytope. Since Sk,j is convex ∀k ∈ K, k 6= j, theneach hypersurface of a convex polytope Xk is an (n−1)-dimensional convex polytope.

53

Appendix B

Nomenclature

aA,A(t)bB,B(t)C,C(t)dD,D(t)En

f(x)Fx, Fy, FzFFBFWgk(x)GGFH,Ha,b

Ixx, Iyy, IzzJJ(u)KLLFmMx,My,Mz

nNOOkpkpk,jP

variable used to minimize edge-pathmatrices used in state space descriptionconstant value used in definition of hyperplane and half-spacematrices used in state space descriptionmatrices used in state space descriptiondistance function used in metric spacesmatrices used in state space descriptionEuclidean n-spaceoptimization objective functionapplied force to the rigid body in world framefeasible region in optimizationbody frame with x, y, z axesworld frame with X,Y, Z axesoptimization constraint functiongraphgraph of feasible legshyperplanesmoments of inertia around x, y, z axesset of elements representing indices of nodes in a polytope faceobjective functional to be minimized for the LQ controllerset of indices of obstacleslegs in graphfeasible legs in graphmass of the rigid body or number of obstaclesapplied torques around x, y, z axesdimensionnodesset of obstaclesk:th obstaclek:th path nodethe j:th point representing obstacle ksubset of a metric space

54

APPENDIX B. NOMENCLATURE

PPkq(t)QRRRskS~Tu(t)v(t)~vkxx(t)XXk

y(t)Yz(t)ZαijΓζ, ξ, χψ, θ, φωx, ωy, ωzΩ

set of all polytope facesfaces of the k:th polytopeoutput of a linearized nonlinear systemLQ weight matrixLQ weight matrixrotation matrixthe set of real numbersthe number of points represening the k:th obstacleLQ weight matrixtranslation vectorstate space input signalinput of a linearized nonlinear systemvector between k:th path node and its neighborcoordinate in spacestate vectoradjoint system submatrixset of the k:th Voronoi cellstate space outputadjoint system submatrixstate vector of a linearized nonlinear systemthe set of all integersangle between neighbor nodesreachability matrixEuler angle tolerancesEuler anglesangular velocities around x, y, z axesobservability matrix

55

Trajectory Planning for a Rigid Body Based on Voronoi...

Documents

Transcript of Trajectory Planning for a Rigid Body Based on Voronoi...