Tessellation and Linear-Quadratic Feedback Control
Authors:
Hans Flodin Gustav Friberg Fredrik Wahlberg
hansfl@kth.se gfriberg@kth.se fwahl@kth.se Supervisors:
Xiaoming Hu Johan Thunberg
hu@kth.se johan.thunberg@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
When a rigid body moves in 3-dimensional space, it is of interest to find a trajectory such that it avoids obstacles. With this report, we create an algorithm that finds such a trajectory for a 6-DOF rigid body. For this trajectory, both the rotation and translation of 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 where the first part comprises translational path planning in the 3-dimensional Euclidean space and the rotational path planning. The second part comprises the design of control laws in 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 our augmented Voronoi tessellation method. We then create a graph containing all feasible paths along the faces of the convex polytopes containing the central points of the virtual spheres. A simple global graph-search algorithm is used to find the shortest path between the nodes of the polytopes, which is then further improved by approximately 3%. Where random obstacles are uniformly distributed in a confined space. During the work process, we discovered that our translational path planning method easily could be generalized to be used in n-dimensional space.
In the second part, the control law for the rigid body such that it follows the rotation and translation trajectory is designed to minimize the cost, which is done by creating a Linear-Quadratic feedback loop for a linear system. The translational control is designed in 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 control laws for the translational and rotational systems. The rotational system is a nonlinear system 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 feasible trajectory as long as a feasible path exists.
1 Introduction 1
2 Rigid Body Transformation and Motion 3
2.1 Transformation . . . 3
2.2 Kinematics . . . 7
2.3 Dynamics . . . 7
3 Path Planning 8 3.1 Translational Path Planning . . . 8
3.1.1 Voronoi Tessellation . . . 8
3.1.2 Virtual Spheres . . . 11
3.1.3 Graph Searching . . . 13
3.1.4 Further Path Improvement . . . 14
3.2 Rotational Path Planning . . . 19
4 Control Design 22 4.1 State Space Model . . . 22
4.1.1 Translational System . . . 23
4.1.2 Rotational System . . . 23
4.2 Reachability . . . 26
4.3 Observability . . . 26
4.4 Linear Quadratic Controller on an Infinite Time Interval . . . 27
4.4.1 Algebraic Riccati Equation . . . 28
4.4.2 Change of Reference Point . . . 29
4.5 Linear Quadratic Controller on a Finite Time Interval . . . 30
4.5.1 Matrix Differential Riccati Equation . . . 31
4.5.2 Trajectory . . . 32
4.6 Stability . . . 32
5 Simulations 34 5.1 Simulation of Graph Creation . . . 34
5.2 Path Improvement Simulation . . . 35
5.3 Control Law Simulation . . . 37
5.3.1 Translation . . . 37
5.3.2 Rotation . . . 39
7 Discussion 47
Bibliography 48
A Proofs 49
A.1 Path Planning . . . 49
B Nomenclature 54
Introduction
In this report we provide a means of finding a path avoiding obstacles through space and design control laws for a rigid body to follow this path. This is applicable in many different areas, e.g., satellites, UAVs1or 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 is assumed to be global, i.e., that the constellation of the space environment is fixed and fully known before initial path planning. We have then separated the path planning and control design of the rigid body and developed an algorithm for finding a trajectory avoiding obstacles.
If we have a set of obstacles O = {Ok : k ∈ K}, where K is a set of m indices, the purpose of the path planning part of our algorithm is to find the shortest translational path, with orientation, between a start position and an end position in 3-dimensional space, which avoids the obstaclesO 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 to minimize the input energy and deviation from the reference state. This can be associated with 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 obtain convex 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 section 3.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 way of further optimizing this path, as long as we had obstacles that could be approximately
1Unmanned Aerial Vehicle
considered as points, is described in section 3.1.4. A method of finding a rotational path for 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-Quadratic controller. Two Linear-Quadratic controllers are designed in chapter 4, one minimizing the 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 and the control design part of our algorithm to evaluate results that were difficult to predict from theory.
The two final chapters consist of a conclusion and a discussion of the apprehended results, followed by an appendix with all self-compiled proofs and notations used in the report.
Rigid Body Transformation and Motion
In this chapter we show how our rigid body is represented. The position of the rigid body 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 frame is rigidly attached to the center of mass of the rigid body. To distinguish these frames further, we use uppercase letters, XY Z, for the cartesian coordinate axes in the world frameFW and lowercase letters, xyz for the body frame FB. When the transformation between these two frames is determined, the kinematic equations and dynamic equations are defined for the rigid body.
2.1 Transformation
The translation of the body frame FB relative the world frame FW is represented by a translation vector ~T according to figure 2.1.
Figure 2.1: The translation vector ~T representing the position of the body frame origin relative the world frame origin.
An explicative method of representing a specific rotational orientation of a 3-dimensional object relative a reference frame, in our case the world frameFW, is by using the Euler angles ψ, θ, φ. Let the world frameFW and the body frameFB have a coinciding origin, then the angle ψ is the angle between the coordinate axis X and the line of intersection between 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 the intersection 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 the xy-plane.
To transform vectors represented in the world frame FW to the rotated body frame FB a rotation matrix R need to be specified. The rotation matrix R then maps vectors in the world frameFW to the body frameFB, i.e.,
R : ~bW → ~bB, (2.1)
where b is a vector and the subindices W and B refer to the world frameFW and body frameFB. 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 determinants are +1, i.e.,
SO(n) =
A∈ Rn×n: det(A) = +1
. (2.2)
SO(n) is called the special orthogonal group.
In this report, we use the following rotation matrices:
Definition 2.1.2. The rotation matricesRψ,Rθ andRφ are given by
Rψ=
cos ψ − sin ψ 0 sin ψ cos ψ 0
0 0 1
, (2.3)
Rθ=
1 0 0
0 cos θ − sin θ 0 sin θ cos θ
, (2.4)
Rφ=
cos φ − sin φ 0 sin φ cos φ 0
0 0 1
. (2.5)
Since SO(n) is a group, it is closed under its binary operation, i.e., multiplicity, which implies 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, first assume the world frameFW and body frameFB axes and origin coinciding. The rotation sequence can then be demonstrated in the following way:
1. First, we rotate the body frameFB an angle φ around the Z-axis. This is done by the rotation matrixRφ 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 frameFB around the X-axis an angle θ. This is done by the rotation matrix Rθand is illustrated in figure 2.4.
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 done by 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 rotation sequence 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 detR = +1.
ThenR−1 is a map from body frameFB to world frameFW.
It can also be noted that the rotation sequence from our choice of Euler angles has at least one singularity, e.g., θ = nπ, n∈ Z.
2.2 Kinematics
The kinematics of a rigid body consists of a translational and rotational motion. The rotational kinematics of a rigid body using Euler angles results in a set of nonlinear differential 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 = dtd(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 torque around the principal axes of the body frameFB.
Path Planning
One issue of a trajectory planning problem, is to find a feasible path for it through the area of interest. We chose to address the problem globally, i.e. that the constellation of the space environment was fixed and fully known before initial path planning. This is partly because of the difficulties with simulating local knowledge and partly a way of simplifying the problem.
Initially, we describe our method of finding a translational path. During the work process, 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 theory in numerical calculating environments like M AT LAB 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 same time avoid a number of obstacles, some kind of structure is needed. We decided to use Voronoi tessellation because of its simplicity and compatibility with numerical computer environments, like for example M AT LAB R. As we shall later see, the diagram of a Voronoi tessellation is also created such that the minimum distance to every obstacle is maximized. An example of a Voronoi tessellation over sets containing finitely many unique 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, named after 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-space proven to be a metric space we will define the set of each Voronoi cell.
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 are closer 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 Euclidean space.
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, called the generators or the sites, the Voronoi diagram induced by this tuple is the tuple(Rk)k∈K of nonempty subsets Rk ⊆ X, such that for all k ∈ K,
Rk = dom(Pk, [
j∈K,j6=k
Pj) ={x ∈ X | d(x, Pk)≤ d(x, Pj),∀j ∈ K : j 6= k}. (3.2)
In other words, eachRk, called the k:th Voronoi cell, is the set of allx∈ X whose distance toPk is not greater than their distance to any of the otherPj, 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 over P results in m n-dimensional sets, i.e., the Voronoi cells, where the k:th point xk∈ Pk is located 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 tessellation over a set containing finitely many unique points, it can be proven that the sets making
up the Voronoi cells for all obstacles as a matter of fact are convex polytopes. This is a well known fact which usually is not proved, but we provide a proof in this report. For that, 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.
Proof. See appendix A.1.
Definition 3.1.6. [14] Leta = (a1, a2, . . . , an)∈ Rn\{0} and b ∈ R. An n-dimensional hyperplane H = Ha,b⊂ Rn is defined as
H =
x∈ Rn
n
X
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
X
k=1
akxk+ b≤ 0
. (3.8)
Definition 3.1.7. Ann-dimensional convex polytope is an intersection of n-dimensional half-spaces.
Proposition 3.1.8. Every cell of a Voronoi tessellation of a finite number of points in Euclidean n-space is an n-dimensional convex polytope.
Proof. See appendix A.1.
Corollary 3.1.9. Each face of a convex polytope of a Voronoi tessellation over finitely many unique points in Euclidean n-space is an(n− 1)-dimensional convex polytope.
Proof. See appendix A.1.
3.1.2 Virtual Spheres
As conditioned in proposition 3.1.8, we only obtain convex polytopes when performing a tessellation over a set of finitely many unique points. Then what happens if the obstacles are not just point obstacles?
We wanted to be able to use the properties of having convex polytopes generated by the Voronoi tessellation, independent of the geometry of the obstacles. We therefore developed a method to approximate each obstacle as a point by minimizing a virtual sphere around each obstacle. This was also a way to escape the difficulties of handling sets in numerical calculating environments.
This is done by solving a convex nonlinear optimization problem which minimizes an n-dimensional sphere around each obstacleOk, whereOk is a subspace represented by a set of finitely many unique points corresponding to the extreme points of the obstacle’s physical shell. Furthermore,
Ok∩ Oj=∅, ∀k, j ∈ K : k 6= j, (3.9) i.e., all obstaclesOk are disjoint. For this, some optimization definitions are needed.
Definition 3.1.10. [13] An optimization problem is a problem where one want to find the minimum value of an objective function with respect to some variable under some constraints. This is written as
minimize
x 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 and x is a variable of a finite dimension. The constraint functions can also be denoted with a feasible setF, i.e.,
minimize
x f (x)
subject to x∈ F (3.11)
wheref (x) is an objective function, F is the feasible region and x is a variable of a finite dimension.
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 problem like (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)
we had an optimization problem as in (3.23), with the variable x ∈ Rn+1, objective function
f (x) = xn+1 (3.14)
and constraint functions
(gj(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 sphere and xn+1represents the squared radius of the sphere.
Since the objective function f (x) = xn+1is convex and the constraint functions
gj(x) =
pk,j− (x1, x2, . . . , xn)
T
pk,j− (x1, x2, . . . , xn)
− xn+1
=
n
X
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 obstacle with a virtual sphere, which can be seen in figure 3.2. It should be noted that these virtual spheres can intersect with the polytope faces.
Figure 3.2: Virtual spheres in red surrounding obstacles represented by blue points in R3.
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 are guaranteed a link into our structure of convex polytope faces. We then need a method of effectively 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 feasible partial paths between the nodes of the polytopes. We denote these partial paths as legs.
Definition 3.1.14. [5] A graph is a pairG = (N , L), consisting of a finite set N and a setL 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 in the tessellation, where N is the set of all nodes, and L is the set of all legs between the nodes of the polytopes.
For a path to be feasible, it must avoid all obstacles. Therefore, it is inconvenient if the path passes through a polytope, and it is therefore required to travel along the faces of the polytopes.
A set P of all faces of the polytopes is then created by gathering all intersections of the convex polytopes created by the tessellation, i.e.,
P =Pk
k∈ K , (3.17)
where
Pk = [
j∈K:j6=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 all nodes in the same face, a graph of all face legs is created. At this moment, all these legs are examined so that no part intersects with any virtual sphere making them infeasible, and giving them weights corresponding to their lengths. Here, one can also demand further properties of the legs, e.g., maximum angular changes. With all potentially infeasible legs removed, the graph
GF = (N , LF), (3.19)
consisting of all nodesN 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’s Algorithmbecause of its implementation in MATLAB R. This is a graph search algorithm named 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-negative weighted graphs only. Since our weights are the lengths of the leg, i.e., the distances between 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.
Figure 3.3: Linearly interpolated path obtained from graph search, avoiding virtual spheres 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 surrounded by 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 the faces over their edges, an equally short or shorter path could be found. This is denoted as the edge-path.
By finding all nodes lying in the same polytope face, the neighbor nodes of the nodes in the path, i.e., path nodes, can be found by examining the angles between all vectors between 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 that the vector from the path node to these nodes maximizes
αij=
arccos h−−−→
N0Ni,−−−→
N0Nji k−−−→
N0Nikk−−−→
N0Njk
!
, i, j∈ J. (3.20)
−−−→N0Ni is the vector between the path nodeN0and the i:th nodeNi in the polytope face, αij is the angle between the vectors to the i:th and j:th node, illustrated in figure 3.4 and i, j ∈ J where J is a set of elements representing indices of the nodes in the polytope face.
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 edges between the path node and the neighbors are represented by vectors and are called feasible edges. Furthermore, the fact that every path node often relates to more than one face results in a variation within the number of feasible edges. Figure 3.5 shows a 2-dimensional projection of a 3-dimensional structure. The node-path is in black and the green vectors are the feasible edges pointing at the neighbors.
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 in different faces. Since the edge-path is not allowed to move inside the polytopes it is necessary to determine which feasible edges are located in the same face. By comparing all neighbor nodes to the k:th and (k + 1):th path node, it is possible to determine if the leg between the neighbor nodes correspond to a leg in LF ∈ GF from (3.19). If so, the path nodes and neighbor nodes are located in the same polytope face and are feasible for 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 2 5 4 7 9
, 4 6 9 11
,
11 20
6 19 14 16 14 17
,
20 23 31 27 16 25
, . . . (3.21)
This illustration shows the path nodes k − 1, k, k + 1, k + 2 and their corresponding feasible neighbor pairs as rows in respective matrix. We want to connect the feasible neighbor pairs into a set of neighbor combinations. This is done with an iterative method examining the feasible neighbor pairs of each path node. We start by creating a feasible neighbor 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 iterated as follows until the z:th path node.
(i )If the (k + 1):th path node share this neighbor, it is added to the feasible neighbor combination 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 node is 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 feasible neighbor combinations. With feasible neighbor pairs as in (3.21), two examples of feasible neighbor combinations can be:
[. . . , 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 results in 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 intersection between the edge-path and a combination of feasible edges. This is solved with a nonlinear optimization 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 each edge, vk, in the combination of feasible edges. If ak = 0, the edge-path on that specific edge, vk, is intersected with the node-path. On the other hand, if ak = 1, the edge-path on that specific edge is intersected with the neighbor. Furthermore, ak ∈ [0, 1], k = 1, 2, . . . , z and we get the following nonlinear non-convex optimization:
minimize
x
z−1
X
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-path by finding the minimizing values on ak ∈ [0, 1], where the edge-path crosses the edges between the k:th and (k + 1):th path node’s shared neighbors. The objective function is a sum of the distances between all edges. Since there are z nodes in the node path, there are z−1 distances in the sum. By solving this optimization problem for each combination of feasible edges, the shortest one is found, which is illustrated in figure 3.7 and 3.8.
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 the obstacles that could be approximately considered as points, i.e., that the distance between the obstacles was much greater than the size of the obstacles. If not, the obstacles might generate virtual spheres intersecting with polytope faces. As seen in section 3.1.3, this was avoided for the node-path. Unfortunately, infeasible legs were introduced again in order to find the feasible neighbor pairs when creating the edge-path, illustrated in figure 3.9.
Neighbor node
Path node Polytope face
Infeasible edge−path Neighbor
node
Neighbor node virtual
sphere
Path
node 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, the rotation 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 translational path 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 figure 3.10.
Figure 3.10: Translation path avoiding virtual spheres, with normal plane disc visible in green.
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 figure 3.11.
φ φ
Figure 3.11: Normal plane of the translation path with virtual spheres intersecting the disc representing the maximal extension of the rigid body with infeasible angles in dark green.
In turn, this represents obstacles for the angle φ in 2-dimensional rotation space, which can be seen illustratively in figure 3.12 and 3.13, where φ∈ [0, 2π).
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 orientation for the rigid body is found.
Figure 3.13: Illustative picture of rotation path avoiding obstacles (infeasible angles) in rotation space for φ.
Since we have not explicitly seen the geometry of the infeasible angles in rotation space, it should be noted that figure 3.12 and 3.13 only display an illustrative view of the rotation 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 infeasible for the rotation is removed. The rotational path planning is then repeated.
Control Design
Translational motion is controlled in the coordinates of the world frame FW and rota- tional motion in body frameFB. Then the translational and rotational control problem can 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. Two types 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 be the Linear-Quadratic controller for an infinite time interval. The second one is also a Linear-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 is minimized:
J(u) = Z t1
t0
(x(t)TQx(t) + u(t)TRu(t))dt + x(t1)TSx(t1), [8] (4.1) where Q, S≥ 02and R > 03 are symmetric. We call these linear feedback controllers LQ 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), where i = 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) are called the state-, input- and output vectors.
Definition 4.1.1. [8] The linear state space decription has the form (˙x(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 are independent of timet, i.e. they are constant matrices, we call it a time invariant linear system.
2Meaning Q,S are positive semidefinite matrices.
3Meaning R is a positive definite matrix.
Definition 4.1.2. [6] The nonlinear state space description has the form (˙x(t) = f (x(t), u(t))
y(t) = h(x(t)) , (4.3)
wheref 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.1 for 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 Newtons law of motion ~F = m~a, where we assume a constant mass. The state space description for the translational motion is described in the world frameFW, where the subindex T is used 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 world frame FW. The output yT(t) of the system can be chosen as, for example, the position of 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 with yT(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 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
and BT =
0 0 0
0 0 0
0 0 0
1
m 0 0
0 m1 0 0 0 m1
(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 in the body frame FB, where Ixx, Iyy, Izz and m are contant in the description of the
rigid body. Since the translational control is done in world frameFW the rotational and translational control can be separated.
Let xR(t) denote the rotational system state vector, with the systems states as its entries. We choose six different states, namely ψ, θ, φ, ωx, ωy, ωz, i.e., xR(t)∈ R6×1. To achieve reorientations of the rigid body, the torques Mx, My, Mzabout the body frameFB principal 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 form
˙xR(t) = f (xR(t), uR(t)), i.e.,
˙xR(t) =
(ωxsin φ + ωycos φ)/ sin θ ωxcos φ− ωysin φ ωz− (ωxsin φ + ωycos φ)/ 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 function h(xR(t)) from the system defined in definition 4.1.2 is a map h : R6×1 → R6×1. This implies 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)) = I6with definition 4.1.2 we can conclude that we have a nonlinear rotational state space model. Note that the Euler angles ψ, θ, φ are needed for transformations between the world frameFW and the body frameFB.
Linearization
For a linearized state space model we need to find an equilibrium that the nonlinear system can be linearized about. This equilibrium can also be refered to as a stationary point. The linearized system is then a good approxiamtion if the deviations from the equilibrium 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 equlibrium point (x0, u0), then the system 4.3 can be written as
˙z = Az + Bv + g(z, v) (4.9)
wherez = x− x0,v = u− u0 andg(z, v)/(|z| + |v|) → 0 when |z| + |v| → 0. The matrices A and B have elementsaij andbij given by
aij = ∂fi
∂xj
, bij = ∂fi
∂uj
(4.10)
wherefi 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,0
is a constant input value and xR,0is a constant vector. From (4.8) we see that a feasible equilibrium is
xR,0=
ψ0
θ0
φ0
0 0 0
, uR,0=
0 0 0
, (4.11)
with the restriction θ0 6= nπ, ∀n ∈ Z, i.e., avoiding the Euler angle singularity discussed in section 2.1. This equilibrium corresponds to a configuration where the body 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 vector v(t) = uR(t)− uR,0. We also introduce q(t) as the output for the linearized rotational state 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(φsin(θ00)) cos(φsin(θ00)) 0 0 0 0 cos(φ0) −sin(φ0) 0 0 0 0 −sin(φtan(θ00)) −cos(φtan(θ00)) 1
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
,
BR=
0 0 0
0 0 0
0 0 0
1
Ixx 0 0
0 Iyy1 0 0 0 Izz1
.
(4.13)
Note that CR = I6 and DR = 06,3 when q(t) = z(t) using the same arguments from section 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)
4.2 Reachability
To design a feedback control law for the translational and linearized rotational state space systems 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 corresponding toΓ is reachable.
Since the systems (4.5) and (4.14) are linear and time invariant systems we can use lemma 4.2.2 to determine if Γ has full rank. Both AT as in (4.6) and AR as in (4.13) are nilpotent4 matrices. This implies that Aj = 06,6, ∀j ≥ 2. For the translational system the reachability matrix ΓT is given by
ΓT =
03,3 I3 03,12 1
m· I3 03,3 03,12
. (4.16)
ΓT has 6 rows and 6 linearly independent row vectors, thus it has full rank[11], and hence the system is reachable. Analyzing the linearized rotational system (4.14) results in ΓR given by
ΓR=
0 0 0 Isin φ0
xxsin θ0
cos φ0
Iyysin θ0 0 01,12
0 0 0 cos φI 0
xx −sin φIyy0 0 01,12
0 0 0 −Ixxsin φtan θ00 −Iyycos φtan θ00
1 Izz 01,12 1
Ixx 0 0 0 0 0 01,12
0 I1
yy 0 0 0 0 01,12
0 0 I1zz 0 0 0 01,12
. (4.17)
Let ˜ΓRbe the first six columns of ΓR, then det ˜ΓR= (sin2φ0+ cos2φ0)/(Ixx2
Iyy2
Izz2
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 linearized rotational 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 state x(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
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
Ω =
C CA
... CAn−1
. (4.19)
Lemma 4.3.2. [8] Let n be the dimension of the state space. The pair (C, A) is said to be 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 I3
03,3 I3
027,3 027,3
. (4.21)
ΩT has six linearly independent columns, which implies that ΩT has rank 6[11] and thereby the translational system is observable. The observability matrix ΩR for the linearlized rotational system has the form
ΩR=
1 0 0 0 0 0
0 1 0 0 0 0
0 0 1 0 0 0
0 0 0 1 0 0
0 0 0 0 1 0
0 0 0 0 0 1
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 027,1 027,1 027,1 027,1 027,1 027,1
. (4.22)
Using elementary row operations on (4.22), ΩRcan be written with 6 linearly independent column vectors, which implies that rank ΩR= 6[11] and the linearized rotational system is observable.
4.4 Linear Quadratic Controller on an Infinite Time Interval
With infinte time inteval we mean [t0,∞). From the objective function (4.1) which is minimized when deriving a LQ controller, we see that (4.1) is minimized on an infinite time interval.
To determine if we have unnecessary states in our realization we need to determine if the system is minimal, i.e., if the system is a minimal realization. By unnecessary states
we mean states that cannot be reached and observed. This means that a natural state candidate to keep is the part that is in the reachable subspace and not in the unobservable subspace[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 are reachable 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 to solve the Algebraic Riccati Equations.
Definition 4.4.2. [8] The Algebraic Riccati Equations (ARE) is given by
0 = ATP + P A− P BR−1BTP + Q, (4.23) whereQ≥ 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 real symmetric positive definite solution.
To illustrate two example solutions to ARE, one for the rotational system (4.5) and one for the linearized rotational system (4.14), we choose
Q =
1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
, R =
1 0 0 0 1 0 0 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)
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−1BTP x, (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−1BTTPTxT(t) =
−xW(t)− ˙xW(t)√ 2
−yW(t)− ˙yW(t)√ 2
−zW(t)− ˙zW(t)√ 2
, (4.29)
v(t) =−R−1BRT
PRz(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 at time t0and converges to state x1as t1→ ∞. To get the system to visit a set of reference states, a time dependant path can be constructed, i.e., a trajectory. The reference state can then be changed at some time tk, where t0 ≤ tk <∞. Unfortunaetly if we change reference 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 energy to decrease the kinetic energy of the rigid body.
Instead of changing reference state at a certain time, we chose to change reference state when some state variables are within a tolerance distance. Then, for the translational problem we can construct tolerance spheres around the reference positions. The rigid body can then preserve some of the kinetic energy when changing reference state. This idea is illustrated in figure 4.2.
Figure 4.2: When the position of the rigid body is within a tolerance sphere, the reference positionis changed.
The radius of the tolerance spheres can be chosen in several ways. For the illustration in figure 4.2, the tolerance radius of the spheres was chosen to be 10% of the preceding leg distance in the path. This technique can also be used for the rotational problem. We then 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 that the rotational system is linearized around the reference state, i.e., a linearized model is needed for at least each reference state, if we intend to have good behavior of the system dynamics around reference state. In case of large state differences between initial state and 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 the objective 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].
4.5.1 Matrix Differential Riccati Equation
Definition 4.5.1. [8] The Matrix Differential Riccati Equation (MRE) is given by (P˙ =−ATP− P A + P BR−1BTP− Q
P (t1) = S , (4.32)
whereQ, 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 as (X = AX˙ − BR−1BTY ; X(t1) = I
Y =˙ −QX − ATY ; Y (t1) = S , (4.33) whereQ, 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 the translational 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,6
and S given by
ST =
1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
, (4.34)
the resulting PT(t) matrix is:
PT(t) =C ·
1 0 0 2− t 0 0
0 1 0 0 2− t 0
0 0 1 0 0 2− t
2− t 0 0 (t−2)1 2 0 0
0 2− t 0 0 (t−2)1 2 0
0 0 2− t 0 0 (t−2)1 2
,
where C = 3m2
(3 m2− t3+ 6 t2− 12 t + 8).
(4.35)
The matrix PT(t) given in (4.35) is used in chapter 5 for the translational trajectory following simulation.
For the rotational system (4.14), the solution matrix PR(t) to MRE becomes even more extensive5. 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 use SR given by
SR=
1 0 0 0 0 0
0 1 0 0 0 0
0 0 1 0 0 0
0 0 0 1 0 0
0 0 0 0 1 0
0 0 0 0 0 1
. (4.36)
The physical difference between ST and SRis that at the reference state x1at time t1, the translational controller only intend to minimize the squared physical position distance error at time t1 together with the input energy, with no mathematical restrictions on the velocity. The rotational controller intend to minimize both the squared angle error and the squared angular velocity error at time t1together with input energy.
The feedback signal u(t) is constructed according to figure 4.1 and (4.28) with the matrix P (t) that solves MRE.
4.5.2 Trajectory
To change from one state x0at time t0towards another state x1at time t1, i.e., following a time dependant path, is called a trajectory. The LQ finite controller uses a time variant feedback, which can be a good choice when following a trajectory.
4.6 Stability
Theorem 4.6.1. [8] Let
˙x = Ax; x(0) = x0, (4.37)
whereA∈ Rn×n is constant.
(1) The system (4.37) is asymptotically stable iff the real parts of all eigenvalues of A are negative, i.e., the eigenvalues are all located in the open left half plane.
(2) The system (4.37) is unstable if A has at least one eigenvalue in the open right half plane.
With the A matrix in theorem 4.6.1 as AT in (4.6) and AR in (4.13), analyzing the system poles, we can see that both systems have all poles in the origin. Using theorem 4.6.1, we can conclude that the system is unstable. When introducing the LQ infinite regulator from (4.29) and (4.30) to each corresponding system, the closed loop system can be written on the form
˙x(t) = (A− BR−1BTP )x. (4.38)
5Meaning it contains more variables and constants.
Lemma 4.6.2. [8] If the system is a minimal realization and if P is a real symmetric positive 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 loop system is stable.
When analysing the system using a LQ finite feedback regulator, the system does not converge to the reference state as t → ∞, and thus does not display stability characteristics[6].
