Optimal Path Planning on the Three-Input Six-Dimensional Brockett’s Canonical System

This paper addresses an optimal path planning problem on the three-input six-dimensional Brockett’s canonical system. This class of systems can be applied to control the position and orientation of a rigid body in SE, such as spacecraft, aerial vehicles, or underwater vehicles, using only three inputs. Since the number of inputs is less than the total number of degrees of freedom, it raises non-trivial technical issues in finding the actual time sequence of control inputs. Here we show that the shortest paths connecting two points are parametrized as helix paths by introducing the input quadratic norm as a Riemannian metric. In addition, we present a quasi-analytical procedure to determine the optimal helix path for any given target point. The characteristic feature of our method is that the optimal paths are parametrized as an explicit function on the state space, which enables the solution paths to be derived without multidimensional iterations. The approach was validated by numerical computations in two aspects: matching for arbitrary target points and covering known optimal paths as special cases. The result that the shortest paths are represented by helices may also help as guidance for solving more general problems numerically, for example, as an initial solution or as a measure on the state space.


I. INTRODUCTION
A. BACKGROUND This paper addresses the path planning problem of a class of nonlinear control systems. This problem is motivated by nonholonomic mechanical systems, that is, mechanical systems associated with nonintegrable constraints. A typical class of nonholonomic constraints is given by the following 1st-order differential equation: where q ∈ R n denotes the state vector (e.g., generalized coordinates in the context of classical mechanics) and n denotes its degrees of freedom. There are m constraints c 1 (q), . . . , c m (q) ∈ R n , which restrict the generalized velocityq to be perpendicular to them. If (1) is not integrable, The associate editor coordinating the review of this manuscript and approving it for publication was Ton Duc Do . that is, if there is no constraint h(q) = const. that replaces the velocity constraints (1), the system is called nonholonomic. In a nonholonomic system, the possibility of changing its full configuration to an arbitrary one remains, in spite of the instantaneous restriction on its velocity. A wellknown example is the differential-drive vehicle shown in Fig. 1. It is possible to change its position and orientation (three degrees of freedom in total) from any initial state to another by appropriately choosing the velocity of the two wheels. This property has attracted significant interest in the nonlinear control theory. Considering the admissible velocity as the control input, the non-integrability of the mechanical constraints is interpreted as the controllability of nonlinear systems. Since the number of admissible velocities is less than n (in the case of a differential-drive vehicle, lateral sliding motion is not allowed), it raises non-trivial technical issues in finding the actual time sequence of control inputs. These controllable underactuated systems have several advantages and possible applications, as discussed in [1]. However, their control is much more difficult than that of fully actuated systems. Therefore, for the underactuated systems with nonholonomic constraints, it is of the primary importance to discuss the path planning problem.
Many mechanical systems, such that the nonholonomic constraints (1) correspond to the generalized velocities, are represented as the symmetric affine systems of the forṁ where q denotes the state, u i denotes the input. The vector field g i (q) indicates the direction of state change driven by the corresponding input. This paper focuses on a subclass of these symmetric affine systems in which nonlinear controllability is ensured by the first-order Lie brackets, called ''first-order systems'' [2]. We introduce existing path planning methods that include first-order systems as their target systems and classify them in two aspects: rate of decay and representations of the target systems. Although computational optimization methods such as the A* algorithm (e.g., [3]) and model predictive control (MPC) (e.g., [4]) can be used for general purposes, it is difficult to strictly satisfy the termination boundary conditions, especially for nonholonomic systems. There are many path planning methods based on Lyapunov's stability theory, in which the time evolution of state quantities decays exponentially (or its extension), for example, [5], [6], [7], [8], [9], and [10]. However, we focused on finite-time steering, which is a stronger result than exponential stabilization. If finite-time steering is achieved, the system is exponentially stable; however, the converse is not always true. We note that finite-time steering is not always superior to exponential stabilization as a controller. Several finite-time steering methods have been proposed in [2], [11], and [12]. Murray and Sastry [2] presented a step-by-step steering method on the first-order canonical systems, introduced in [13]. Owing to the difficulty of matching multiple state variables to boundary conditions by the optimal paths, they divided the state steering into steps based on controllability structure. Leonard and Krish-FIGURE 2. A schematic sequence of steering a differential-drive vehicle by the method presented in [2].
Step 1: Steer x-coordinate to zero by forwarding motion.
Step 3: Steer y -coordinate to zero by combining forward/backward motion and rotation. Each state quantity is steered in a separate step. The motion is not aimed at its optimality.
naprasad [11] expanded the target system to matrix Lie groups, and provided a steering method similar to that in [2]. Matrix Lie groups can represent Brockett's canonical system without any defects and include the matrix representations of SO(m) and SE(m), which are important for application use. The behavior of these step-by-step steering for a differential-drive vehicle is shown schematically in Fig. 2.
In the words of [11], step-by-step steering is a ''constructive controllability'' method and is not the result of aiming to gain a good path but only to steer in finite time. Although there are various measures of good paths, the authors feel it natural to minimize the path length by introducing the inputs as a Riemannian metric.
This input Riemannian metric minimization problem was tackled early on by Brockett [13], [14] and Baillieul [15]. The formulation of these optimization problems is summarized in [16]. Jurdjevic [17], [18] also dealt with this problem in a mathematical manner. In [13], Brockett introduced a local canonical system for the first-order controllable systems. In addition to introducing the canonical system, Brockett parametrizes the optimal inputs by solving an optimization problem. This result plays an important role in this study.
Several methods to obtain specific optimal paths have been presented using the above formulation or numerical computations. A numerical computation method is reported in [19]. Using the existing result that the optimal paths on SO(3) are parametrized as elliptic functions, Spindler [20] used the shooting method to match the boundary conditions. Henninger and Biggs [21] extended the target system to include SO groups and provided results similar to those of [20]. They also presented an analytical method for threedimensional systems. The authors of [22] and [23] focused on a specific system on SE(3). Both authors discussed optimal orientation problems at unit speeds intended for airplanes, and helix paths were derived as the solution paths.

C. THE FOCUS OF THE PAPER
In summary, for the case of more than three-dimensional systems, either step-by-step steering, a ''constructive controllability'' method, or multidimensional numerical optimization is required. Analytical procedures for finding the shortest path have only been achieved for specific systems, such as two input three-dimensional systems. Hence, we address the optimal path planning on a three-input sixdimensional system, represented by a canonical form, which we call Brockett's canonical systems [13], and which is called ''generalized Heisenberg system'' in [24].
The setting of the target system is limited but significant. Three-dimensional inputs can steer six-dimensional states maximally by the first-order Lie brackets, and six dimensions are identical to the degrees of freedom of SE(3). Precisely, it corresponds to controlling the position and orientation of a rigid body in the three-dimensional space by three-dimensional inputs. Our focus on the canonical system will contribute as the first step toward solving more general problems because finite-time steering was first solved for the canonical systems and then extended to larger classes.
Our purpose is to present a parametric representation of the optimal paths and a path-determining method for any given target point. To this end, in Section III, we show that the set of good paths, meaning that the path length is the local minimum, can be parametrized as helix paths. In Section IV, the existence of helix paths that satisfy the boundary conditions for almost all target points and a determination method are presented. In Section V, numerical examples show that paths can be matched for almost all target points with reasonable accuracy. We also show that the helix paths introduced in Section III contain known shortest paths for several specific target points. Finally, in Section VI, we summarize the contents of this paper.
The position of this paper relative to existing studies can be summarized as follows. References [2] and [11] were the direct motivations of this study. They presented finite-time steering methods for a wide class of systems, but did not aim to gain good paths. In contrast, the path planning method in this study provides good paths in the sense that the path length measured by the input Riemannian metric is the local minimum. While there are numerical methods to plan the paths, this study not only presents a path planning method but also parametrizes the optimal paths by helix.

II. PRELIMINARIES
This section describes mathematical notations. These techniques are described in [25].
We use the operator · to represent scalar or matrix products to distinguish between multiplication and arguments. When representing the dot product of vectors, we use the transpose operator (·) T . The cross product of the two vectors v, Since the cross product by v is a linear operator, by defining we can represent the cross product as For a vector ω ∈ R 3 , |ω| = 1, the following equation holds: where I denotes the unit matrix. The following equation holds for the dot product and cross product of the vectors u, v, w ∈ R 3 : We use the matrix representation of SO(3) defined as With the definition of the normalization operator for the nonzero vectors we can define map C from two ordered nonzero vectors to where v, w ∈ R 3 , v × w ̸ = 0. Maps by R ∈ SO(3) hold the following relations for the given v, w ∈ R 3 :

III. PARAMETRIZATION OF THE OPTIMAL PATHS
This section establishes a set of candidates for good paths. For this purpose, we define an input Riemannian metric minimization problem between two points on the three-input six-dimensional Brockett's canonical system. We then derive a parametric path representation that satisfies the shortest stationary condition. The logical structure of this section is shown in Fig. 3. First, we define the minimization problem as follow: Problem 1: Primal optimization problem  [13]. We supplement Theorem 1 by Corollary 1 and prove that Theorem 2 is held independently from the above two theorems. By applying the three theorems in sequence to Problem 1, the search space of Problem 1 can be restricted to the form of Problem 2 without losing the optimal paths. This means that the search space of the optimization problem is reduced from infinite-dimensional functions x(t ) to 6-dimensional parameters.
Where x, y ∈ R 3 are state vectors, and let x and y be called base coordinates and fiber coordinates, respectively. Let a pair of {x f , y f } denotes a target point. Let u(·) : [0, 1] → R 3 denotes control input and be piecewise continuous. In this formulation, we introduce the quadratic form u T · u as a Riemannian metric, not the input two-norm √ u T · u, to simplify the proof of Theorem 1. Whichever Riemannian metric is introduced, the solution for this optimization problem is identical [13].
For this problem, the following theorem holds: Theorem 1 [13]: For the solutions of Problem 1 x * (τ ), there exist constant vectors λ c , v ∈ R 3 , such that the following equation holds: where γ λ : [0, 1] → R 3 is a parametric curve defined by Proof: This theorem was discussed for general dimensional case in [13]. Here we provide a proof for the case of 3-dimensional input, which is the system under consideration in this paper.
The Lagrangian function of the evaluation function (17) and the constraint equation (14) is expressed as with the Lagrangian multiplier λ : [0, 1] → R 3 and without u. Solving the Euler-Lagrange equation for y yields =λ.
Likewise, solving for x with substitutingλ = 0 yields This differential equation can be solved by using the matrix exponential function aṡ □ From here, we begin the original formulations. First, we normalize λ c and impose its norm on the time.
Due to the equivalence of γ λ and γ ω , integrating γ ω on the time yields another parametric curve that is equivalent to the solution curve of Problem 1 x * (τ ). We can observe that exp( ωt) · v represents the rotated vector of a constant vector v about an axis ω for an angle t. This geometric observation leads us to express (27) as the following time-integrated form.
Theorem 2: For the constant vectors ω, v ∈ R 3 , satisfying Proof: First, we proceed by cases to prove that there exist ∈ SO(3), h ∈ R, r ≥ 0 such that is held.
a: CASE 1: ωv ̸ = 0 The Rodrigues' rotation formula expand exp( ωτ ) · v as Here we used (6) to derive (32). We can re-arrange (33) as We can verify that ω − ω 2 v ωv is an element of SO(3) by the definition (8) as VOLUME 11, 2023 The coefficients of the trigonometric functions | ω 2 v| and | ωv| are positive scalar values, and these are equal. Therefore, (30) holds by letting For the vector v, there exists a vector w ∈ R 3 such that v × w ̸ = 0. Then exp( ωτ ) · v can be expressed as Therefore, (30) holds by letting For this case, exp( ωτ ) · v is expressed as Therefore, (30) holds by letting □ To summarize, the infinite-dimensional optimization problem defined as Problem 1 was reduced to the following boundary value problem with the evaluation function.
Problem 2: Boundary value problem with evaluation function We note that Theorem 1 provides only the necessary conditions for the optimal paths to be satisfied. Hence the existence and uniqueness of the admissible solutions of Problem 2 are not guaranteed. In fact, there exist multiple admissible solutions if |y f | is relatively large. Although it is interesting to find the path that minimizes the evaluation function J among the admissible solutions, we do not aim to solve this problem but use the helix trajectory (49) as a set of candidates for good paths. Conversely to the uniqueness, the existence of the helix paths that steer the system to almost all target points will be proved constructively in the next section.

IV. DETERMINATION METHOD OF THE OPTIMAL HELIX PATHS
This section provides a method to obtain an admissible solution of Problem 2, that is, a helix path that satisfies the boundary conditions. For simplicity, we assume that the target points satisfy x f T · y f ̸ = 0, x f × y f ̸ = 0.

A. REPRESENTATION OF THE HELIX ORIENTATION
First, we solve about the orientation by considering its symmetries on SO(3) and obtain its explicit representation as a function of {r, h, t f } and {x f , y f }. By substituting the optimal base trajectory (49) into the system constraints (46), the behavior of the fiber coordinates y(t) is expressed as where Since ∈ SO(3) is a rotation matrix, which preserves the distance and angle, a set of necessary conditions for {r, h, t f } to satisfy the boundary condition (48) is obtained as Conversely, if (57) is satisfied, the boundary conditions can be matched by determining as This can be verified by the equations (49) and (55). Therefore, finding the admissible solutions of Problem 2 is reduced to the following three parameter problem:

B. DETERMINATION OF THE HELIX SHAPE
Next, we solve Problem 3 with respect to r, h, t f , for the given conditions x f , y f . Let θ be the angle between them, which satisfies (64) In order to simplify the expression, we prepare the following functions of t: From the conditions (63), we obtain We begin to express r and h as functions of t f . Equations (71), (72) can be summarized as The right-hand side is invertible since ad − bc = ef < 0 for any t > 0, then we obtain (75) The first row is a quadratic equation with respect to r 2 , i.e., is obtained. Because the last term (b|y f | 2 )/(ef ) < 0 for any t > 0, there exists a unique positive real solution with respect to r 2 . Moreover, r must be positive according to its definition. Therefore, r has a unique solution for t f as By substituting this into (73), we have Finally, we solve t f by |x f |, |y f |, and the angle θ. Substituting (78) into the second row of (75) yields (79) Substituting (77) to eliminate r from (79), we obtain · ef Solving (80) as a quadratic equation with respect to ef |y f | 2 /|x f | 4 , we have a function that gives its root as the t f to be solved as where In order to find a t f for any given x f and y f , we expect the range of the functions (β θ + γ θ )/α and (β θ − γ θ )/α to cover [0, ∞) by a domain of t. This is proven as follows, with the aid of the intermediate value theorem. Fig. 4 shows an overview of the functions (β θ ± γ θ )/α. Where T d is the constant time defined by t θ is the parametric time defined by Authorized licensed use limited to the terms of the applicable license agreement with IEEE. Restrictions apply.  whose range is in (0, 2π). Then, we can see that the following relationships hold: In addition, the function (β θ + γ θ )/α is continuous with respect to t in [t θ , 2π), and (β θ − γ θ )/α is continuous in [t θ , T d ]. Therefore, by the intermediate value theorem, for almost all pairs of {x f , y f }, there exists at least one t ∈ (0, T d ] that satisfies (81) is zero. This root is the t f that we want to obtain. Such t f can be obtained numerically, for example, by applying the bisection method to (81). Once t f is determined, r, h and can be obtained by just substituting t f into (77), (78) and (58) sequentially, as shown in Fig. 5. In summary, we have shown that there exist helix paths reaching almost all target points and also show the method to find such a path with the smallest phase angle.

Remark 1 (Excepted Cases: x f
T · y f = 0, x f × y f = 0): Although we have provided a path determination method for almost all target points, the above procedure cannot be used to find a solution path for the regions of measure zero such that the parallel or orthogonal conditions are satisfied. For such cases, the solution path will be obtained by appropriate case methods. For the orthogonal case x f T ·y f = 0, we should degenerate the system into the 2-inputs 3-states system. For the parallel case x f ×y f = 0, there exist numerous optimal due to rotational symmetry, and one of them can be chosen as an optimal path's parameter.

V. NUMERICAL EXPERIMENTS
In this section, we show some numerical examples. The procedure shown in Fig. 4 was implemented using the programming language Julia and was executed on a computer equipped with a Ryzen5 3600 CPU. Since the procedure does not require multidimensional iterations and the parameters other than t f are written in closed-form, the computation time to obtain the helix parameters is less than 30 µs per path using this setup. The paths are obtained directly by substituting the helix parameters {r, h, t f , } into (49) and (55). In the figures, the given target points x f , y f are plotted as dots, and the numerically obtained paths x(t), y(t) are plotted as lines. Note that the state dimension of the whole system is six, but we divide it into the base coordinates x and the fiber coordinates y in order to overlay them as a trajectory on a three-dimensional space. We first discuss generic examples, then focus on four extreme cases.

A. GENERIC OPTIMAL PATHS AND THEIR ACCURACY
First, we mention that the numerical solutions are obtained for a set of comprehensive target points with reasonable errors. Symmetry on SO(3) is not covered. In addition, base coordinates x is also normalized. Based on these considerations, we set the target values in the range of We cut the mesh at 0.1 intervals, and excluded the case |y f | = 0.0 or θ = 0.0. The termination error µ was evaluated by 88624 VOLUME 11, 2023 Authorized licensed use limited to the terms of the applicable license agreement with IEEE. Restrictions apply.   where x p , y p denote the termination point which calculated by the set of numerically obtained parameters {r, h, t f , }. As a result of the above settings, numerical solutions were obtained for all cases, and the termination error µ was less than 0.02. Fig. 6 shows several sampled paths which steer six-dimensional states x(t), y(t) optimally. Introducing the helix path representation on x enabled us to obtain the optimal paths on the six-dimensional underactuated system quasi-analytically.
B. EXTREME CASE 1: A TYPICAL OPTIMAL HELIX PATH The remaining three cases correspond to those in which the global optimal paths are already known in previous studies. We show that the paths in this study encompass the global optimal paths on these three cases. The two-input threestate Brockett's canonical system can be represented as a special case of the three-input six-state Brockett's canonical system; where the case of x T · y = 0, x, y ∈ R 3 are satisfied. Therefore, we show the three cases where x is embedded in the horizontal plane and y is embedded in the vertical axis.   . This path is also consistent with the known optimal paths. We note that there are two ways to represent a straight-like path by the helix paths (49), one is {rt = |x f |, h ≃ 0, t f ≃ 0} and the other one is {ht = |x f |, r ≃ 0}, and the numerical solutions vary significantly depending on the angle θ.

VI. CONCLUSION
In this paper, the authors have presented a helix path representation as the optimal paths on the three-input six-dimensional Brockett's canonical system. Then a procedure to determine the helix path that reaches any given target point was presented. The key results of paper are as follows: • The parametric representation of the optimal helix paths was obtained as an explicit function x(t) = · ξ r,h (t).
• The helix parameters r, h, t f and have distinct geometric interpretations (representing the radius, pitch, phase angle, and orientation, respectively). Future work will include the following topics. As indicated by Brockett's necessary condition [26], the method of this paper contains discontinuities around the excluded end conditions (59). The other is that the paths only satisfy the stationary conditions; hence the sufficiency of the optimality is not guaranteed. These analyses will be interest in the future.
It is also required to develop for applicate the results of this paper to real engineering systems. One important direction is to extend the target systems from a Brockett's canonical system to Lie group representations or more general forms. Achieving this would allow the results of this paper to be applied to control the position and orientation of underwater vehicles [11] without approximation. Another approach is to improve the nilpotent approximation [27], [28] as an engineering technique. This will allow us to apply the methods of this paper to aircraft landing [4], [22] and spacecraft docking [21]. In addition, the construction and verification of a feedback controller is also required.