1. Introduction
The apparatus of Lie groups is commonly used in the control theory of planar non-holonomics mechanisms, such as kinematic cars, robotic snakes, etc. [
1,
2,
3,
4,
5,
6,
7,
8]. The control Lie group derives a control Lie algebra as a tangent linear space of the group with operation Lie bracket. The main advantage of the control on Lie groups is that everything derived for one point of the mechanism’s configuration space can be easily generalised for all points of the configuration space thanks to the group properties [
9,
10,
11]. For computational reasons, it is preferred that the corresponding Lie algebra be nilpotent. This property generally does not apply, but the Lie algebra can be approximated, usually by an algorithm called a nilpotent approximation or Bellaïche’s algorithm [
12,
13].
This paper is motivated by [
14], where the authors study the Maxwell points of a kinematics model of vertical rolling disk using numerical optimisation methods. The Maxwell point concept is related to optimal control problems [
11,
15]. At the initial point, the local solution of optimal control problems is the optimal trajectory (referred to as a geodesic), that goes from the initial point and the corresponding control. Maxwell points are points where the geodesics intersect each other for the first time and the corresponding geodesic segments have equal length. For example, Maxwell points on the Heisenberg group lie on the line (see [
16]). This implies that Maxwell points are points where geodesics lose optimality. In optimal control problems, there can be other points at which geodesics lose their optimality, for example, conjugate points (see [
11,
15]).
The problem of existence and finding Maxwell points is an open problem [
11,
17]. There are only a few concrete situations where the problem is solvable analytically (e.g., the kinematics of a vertical rolling disc) [
16,
18]. The main goal of this paper is to design a numerical algorithm to find the Maxwell points of the dynamics of a vertical rolling disc. The rolling disk serves as a case study for the methods under development to be tested on, but it may be applicable to a much larger group of complex nonlinear dynamical systems, real-life robots, or may include considerations or models of imperfections of a real dynamical system, as demonstrated in [
19].
Using numerical approaches to solve different nonlinear tasks was proven useful in [
20,
21]. In
Section 2 we describe a vertical rolling disc, including the derivation of differential kinematics and dynamics. In particular, we derive the Lie algebra for the differential kinematics and we indicate the shape of the Lie algebra for the dynamics.
In
Section 3 and
Section 4 we study two different approximations of the Lie algebra for the dynamics of the disc. Later, in
Section 3, we use the Taylor polynomial and in
Section 4 we use Bellaïche’s algorithm for the approximation of nonlinear expressions, while both approximations give a nilpotent Lie algebra. For both approximated dynamic systems, the optimal control problem is formulated, where the goal is to get an energetically optimal trajectory between two points of the configuration space. We use the Pontryagin maximum principle to derive the ODEs whose solutions give the optimal trajectories.
In
Section 5 we show the description of the numerical algorithm developed for finding the Maxwell points of the system. The input of the algorithm is the set of ODEs given by the Pontryagin maximum principle.
4. Nilpotent Approximation Using Bellaïche’s Algorithm
In this section we would like to find 5-dimensional nilpotent algebra that approximates the algebra generated by vector fields
derived in (
6). Let us introduce some notions. Define
to be the linear subspace generated by
For define Set We define weights at by setting if where
In the construction of a nilpotent approximation of the given Lie algebra we will first proceed with the construction of privileged coordinates according to Bellaïche’s algorithm [
12].
Choose an adapted frame at p.
In our case we choose frame at .
Choose coordinates centered at p such that .
For our adapted frame we choose the coordinates
For
set
where, for
with
First notice that for coordinates of degree
the transformation
degenerates [
12]. Thus, we will focus on the evaluation of
The sum yields
Notice that
,
and furthermore
,
,
. So, the transformations
are of the form
in original coordinates
We can see that for the choice of
the transformation degenerates, and hence from now on let us introduce a substitution
Now, express the base of tangent bundle of old coordinates in terms of new coordinates
Finally, let us express original vector fields in algebraic privileged coordinates.
The second step of nilpotent approximation is the approximation of the vector fields’ elements (from a function viewpoint). First let us define a weighted degree of monomial. Given a sequence of integers we define the weighted degree of the monomial to be and the weighted degree of the monomial vector field to be
For vector field
X with a Taylor expansion
the order of
X is the least weighted degree of a monomial vector field having a nonzero coefficient in the Taylor series. Grouping together the monomial vector fields of the same weighted degree we can express each vector field as a series of monomial vector fields. A monomial vector field with the least weighted degree gives us nilpotent approximation of the Lie algebra [
13], thus
The operation Lie bracket on the algebra is shown in multiplicative
Table 3.
4.1. Optimal Control Problem
The dynamics of the mechanism
and conditions (
8) and (
9) define a different approximation of the same optimal control problem as in
Section 3. Like in
Section 3 we get that for the control the following hold:
The Hamiltonian is in the form
and the system of differential equations is in the form
The solution of the system with respect to (
8) gives the optimal curve between points
.
5. Numerical Analysis
As stated in the previous sections, obtaining the Maxwell points of the system is crucial if our goal is to solve the optimal control problem. However, finding them analytically is not always possible, and even if an analytical solution is technically reachable it may not be a practically feasible one.
In practice, a viable alternative to analytical methods is a numerical solution to the problem. Therefore, we developed a numerical apparatus to help discover the approximate location of these Maxwell points of a dynamical system, and to be able to validate any Maxwell point candidates.
The numerical approach proposed throughout this section utilises the Maxwell point definition in the form: a Maxwell point (MP) is any state of an autonomous dynamical system such that it is the end point of (at least) two different trajectories equal in both length in the state space and time, starting at any given state. Generally, there is no guarantee that Maxwell points exist in any given case. However, when presented with a specific system, we can attempt to find them using this definition, and if the attempt fails it can be presumed that the Maxwell points do not exist.
To describe the methods used in further subsections a basic nomenclature will be described here. First, we assume that we deal with an autonomous system of ODEs—the control system, which was created by expanding the original system using the PMP as described in
Section 3.1 and
Section 4.1, resulting in the explicit form (
15)
where
is the state space of the control system,
is the state space of the original system, and
is the extended space generated by the PMP with respect to a minimisation functional
described by Equation (
9).
These spaces are related as
Additionally, we denote a specific state at a given time instant as , for example, representing the initial state or the final state, if . If more than one state trajectory is being described, the respective states are marked , with i being the trajectory index. The same notation is used for vectors y, q, and h.
Given this formulation, the task of looking for a Maxwell point in the
q space corresponds to solving the boundary value problem for a system described by (
15) with the boundary values given by any
and
representing the Maxwell point. Note that we only trace the position of Maxwell points in the state space of the original system
q, not in the whole state space of the control system
y.
Further, given an initial condition
we can simulate the system numerically and produce a trajectory using numerical integration methods such as Runge–Kutta [
26]. This allows us to acquire trajectory end points. Different system trajectories can be generated using different initial conditions. These properties allow us to effectively convert the BVP solution, which is generally a very difficult task, to an IVP coupled with an optimisation algorithm (commonly referred to as the shooting method) [
27].
As stated earlier, if a point is a Maxwell point of the system, then two different trajectories from the same initial point will intersect at time T at this point. Based on this assumption we can now reformulate our problem as a shooting problem, where we have a fixed initial state of the system and we are looking for two different vectors and which generate trajectories leading to the same point .
Finally, viewing the trajectory end point position in the state space as a function of its initial condition
will allow us to formulate an optimisation task for both finding and validating the MPs. To solve these tasks we implement an interior point search algorithm. This constrained nonlinear optimisation algorithm is used to solve problems of the form
where
is the cost function,
is the equality constraint function (which is meant to match the length of multiple trajectories if necessary),
is the inequality constraint function (which is meant to penalise multiple similar trajectories), and
is the vector of parameters that are being optimised. The optimisation algorithm itself is well-described in the literature (e.g., [
28]), and we employ it using a standardised implementation in the Matlab environment [
29]. The optimisation tasks utilised in later sections follow the same formulation.
5.1. Finding Trajectories Leading to Known MPs
If the position of a Maxwell point is known we may want to verify that this point is truly the Maxwell point of the system. This may seem counter-intuitive but proves to be useful when we only assume the property and need to validate it. This method will be used in the following sections to validate our assumptions made about a subset of the state space which we had assumed the MPs to lay on.
As per our previous definition, assuming a known position of an MP, we need to find two trajectories of the same length which intersect at the point at time instant T. We can look for the trajectories one at a time, adding constraints to prevent the optimisation algorithm from converging to the same solution repeatedly.
Find the first trajectory
Assuming the MP position is invariant to the actual trajectory length in time, we can fix the initial configuration of the system to the origin (
) and choose a final time
as any positive real number (affecting only how fast the system passes through the optimal trajectory). We can now optimise the end point of the trajectory to be equal to the suspected Maxwell point
. This means we choose such
that drives the system to
in time
T. This is a straightforward convex minimisation task deduced from (
17), since
now depends only on the choice of
and no constraints are required. The cost function will take the form of a euclidean distance result in an optimisation task defined as
where
is the optimal solution. The whole situation is depicted in
Figure 2.
Find the second trajectory
Now we need to find
for a second trajectory, which is different from
and drives the system towards the same point
, as shown in
Figure 3. We can formulate our constrained minimisation problem as (
19) with constraints (
20) and (
21). The equality constraint (
20) is meant to satisfy the same functional (
9) for both trajectories.
The parameter is a tuning parameter which specifies the minimal distance between the vectors and , which are normalized so that the choice of this parameter is independent of their respective scales.
Review results
There are multiple termination conditions to the interior point algorithm. These include:
First-order optimality;
Iteration step size;
Number of iterations.
Any of these can cause the algorithm to stop, but the main concern is the case where the termination occurs due to a local minimum (first-order optimality reached). If this happens, we may need to return to step 2 and restart the algorithm, giving it a different initial guess.
To decide whether our result is valid, or if we need to restart the optimisation with a different initial guess, we can compare the value of the optimisation cost function with a numerical tolerance parameter. If the value is smaller than our chosen numerical tolerance, the result is valid and the Maxwell point has been confirmed.
5.2. Finding MPs
In the previous section we described how to verify assumed Maxwell points, now we will describe how to find them using a similar approach. We experimented with various combinations of optimisation and random shooting and found that typical approaches such as Monte Carlo or grid search are not viable in this case because it is quite difficult to “hit” the Maxwell point exactly using shooting from random initial guesses. To overcome this issue, the task needs to be formulated as a convex optimisation problem which can converge with arbitrary precision if left iterating.
The proposed process itself is very similar to the one described in the previous section. However, in this case the location of the end point where the two trajectories intersect is unknown, so we need to optimise both
and
at the same time to make the end points
and
converge to a single point. This means that the dimensionality of the problem increases twofold because the number of unknown parameters is
instead of
n. The whole process is shown in
Figure 4, and can be described as looking for two different trajectories which start and end at the same points and have the same lengths. Based on the MP definition described earlier, if the end points do converge, we can take the end point for a Maxwell point. Formally, the optimisation task is then defined as (
22), also with the constraints (
20) and (
21).
Similar to the previous section, we also need to perform an evaluation of the results through the cost function value and check whether the found minimum is truly the Maxwell point or if it is a local minimum.
After a Maxwell point is found we can restart the method with a different initial guess and repeat the process to obtain more Maxwell points. Initial guesses can be chosen randomly. It is important to note that the search process appears stochastic, and not all initial guesses are guaranteed to converge to an MP, even if they do exist. Naturally, the more initial guesses that are tried, the higher the chance of finding an MP.
When a sufficient number of Maxwell points is found, we can formulate a hypothesis about the set they form. Generally, the MPs form a subset of the full space, often a hyper-plane in linear cases. The form of the set can be analysed using standard data analysis tools such as principal component analysis or proper orthogonal decomposition (PCA) [
30]. Afterwards, our hypothesis can be verified using the algorithm from the previous section applied on a new MP predicted by the hypothesis.
7. Conclusions
In this paper, a numerical algorithm for finding and validating the Maxwell points of an affine control system was proposed in
Section 5. The algorithm was applied to two different affine control systems, which were derived by different approximations of dynamics of the vertical rolling disc, as described in
Section 6. Both of the approximations serve two purposes, that is, as a case study for the proposed numerical approach and to evaluate the effect of different approximations on the whole process of optimal control design. These numerical algorithms are the result of continuing research based on preliminary results published in [
14], which proved that the Maxwell points found using numerical optimisation comply with analytical solutions.
As can be seen from the results, there are advantages and disadvantages of both derived systems. In the nilpotent-based approximation the numerical algorithm collapses to a trivial solution, as the state space trajectories are not dependent on some of the expanded states, so the optimality of the control system is lost in points different from the Maxwells ones. The problem of finding these points is much more complicated and will be part of our next research.
In opposition to this result, the common linearisation based on Taylor approximation, while still being nilpotent, yields quite different results. In this case, the numerical algorithm is able to locate a set of Maxwell points suitable for further system analyses and optimal control design. Unfortunately, the dimension of the control Lie algebra is higher than the dimension of the configuration space.
The proposed algorithms allow us, in theory, to find or validate assumed Maxwell points on any affine control system. This applies mainly in situations when analytical solutions are not available, with the only limitations being the computation power and numerical accuracy of the optimisation task. This paper only demonstrates its function on relatively simple examples, but all of the algorithm’s steps should extend well to more complicated cases, as it does not impose any insurmountable conditions on the systems being analysed. The optimisation algorithm can reach arbitrary accuracy (within reason), limited mainly by the numerical accuracy of 64-bit double precision data type. However, increasing the accuracy slows down the computation, and obtaining even a reasonable number of points can take a very long time. The task is by definition suitable for parallelisation on multiple cores of the system to reduce the computation time if needed.
Inaccuracies can also be introduced into the algorithm via integration steps of the numerical ODE solver. This is especially so if the system is stiff. For linear systems we can overcome this issue using an analytical solution to the system of ODEs, but with nonlinear systems only choosing a specialised solver or reducing the step size can solve this problem, which again trades off the computation time.
As seen in
Section 6.1 there is a risk of false positives—either the system is ill defined and the constraints specified in (
22) are insufficient, or a local minimum occurs when converging towards the Maxwell point. Both of these instances result in false positives and can be eliminated by checking that the point we obtained is truly a Maxwell point of the system. In the future research this problem will be addressed more concretely.
While the numerical algorithm presented in this paper is not perfect and requires understanding and interaction with the user, the results are promising and form a basis for future research.