Instant Orientation Correction Strategy: Holonomic System-Based Decentralized Control for Nonholonomic Agents

This study proposes an instant orientation correction strategy to perform an orientation alignment-based formation control applicable to nonholonomic agents. Generally, an orientation alignment-based formation control cannot be applied to nonholonomic agents with orientation constraints because it is originally a protocol for holonomic agents. Therefore, we improved the orientation alignment-based formation control using an instant orientation correction strategy that modifies the local frame of the nonholonomic agents. To examine the effect of the proposed method, simulations and experiments using representative parameters were conducted for unicycle-modeled nonholonomic agents. As a result, nonholonomic agents could execute a holonomic system-based control law by applying the proposed correction strategy. The simulation confirmed the change in the convergence time and the cumulative error of the angular velocity according to the number of agents and the updated step size of the local desired position. As the updated step size increased, the cumulative error of the angular velocity tended to increase (0.3 rad/ $\text{s}\sim 0.4$ rad/s). Also, the convergence time was not dramatically affected by the update step size and the number of the agents (~400 converge step).


I. INTRODUCTION
In sub-research fields of decentralized controls, such as consensus theory and formation control, numerous control laws for holonomic systems have been reported [1], [2], [3], [4]. Nevertheless, decentralized control should be applied to nonholonomic systems having constraints in movement, such as vehicles. Notably, several studies have been reported wherein a decentralized control is initially developed for nonholonomic systems, or an algorithm for a holonomic system-based control law is extended by limiting certain conditions [5], [6], [7], [8], [9], [10], [11], [12]. However, existing studies on holonomic systems cannot be directly applied to The associate editor coordinating the review of this manuscript and approving it for publication was Qiang Li . nonholonomic systems. Therefore, the motivation of this study aims to incorporate control laws targeting holonomic systems into nonholonomic systems. When implementing holonomic system-based control laws, specially designed robots such as multicopters and omnidirectional-wheeled mobile robots are preferred. Typically, nonholonomic systems are more widely used than holonomic systems, and studies on nonholonomic systems are more practical. Although developing a certain control law by considering nonholonomic systems is more convenient, extending all control laws to various models for nonholonomic systems is time-consuming and inefficient. Therefore, this study develops a strategy that allows nonholonomic systems to use control laws based on holonomic systems. When a nonholonomic system executes a control law targeting holonomic systems, the system cannot produce the desired output owing to the constraint of the moving direction, causing a phenomenon in which an inappropriate value is fed back. In this situation, by modifying the local frame-dependent variables in the original control law, the nonholonomic system can execute the holonomic-systembased control law without the divergence of the control input.

A. RELATED WORKS
Several studies on decentralized control have been reported [1], [2], [13], [14], [15], [16]. Control laws, when local variables and the shape of formation are given, can be classified into displacement-, distance-, and angle-based controls [1]. When implementing distance-based [17], [18], [19], [20], [21] and angle-based control laws [22], [23], [24], decentralized controllers are not affected by the orientation angle of the local frame. Control laws in which the states of controllers depend on the local frame generally assume that the orientation angle of the local frame is identical to that of the global frame [25]. Moreover, the orientation angles of other agents are compared with those of neighboring agents [26], [27], or the velocity constraints of vehicles are monitored [28]. In studies considering both orientation and nonholonomic constraint simultaneously, methods that periodically change the angular velocity of agents [5] or converge the positions and the orientations of the agents to locally given formation with orientations have been employed [11].

B. CONTRIBUTIONS
In this study, we propose an instant orientation correction strategy (IOCS) satisfying nonholonomic constraints by correcting the reference frame of each agent. Because of nonholonomic constraints, numerous decentralized control approaches are not physically feasible when directly applied to nonholonomic systems. Therefore, the proposed IOCS corrects local variables in a local frame used in a holonomic system-based control law considering the difference in the orientation angles between the local frames of the nonholonomic agent and intermediate state.

II. METHODOLOGY
For a state space vector x ∈ n , the elements of the vector x are denoted by x 1 , x 2 , · · · , x n ∈ , and the Euclidean norm of x is denoted by ||x||. For the vectors x 1 , x 2 , · · · , x n ∈ d , an integrated vector is denoted by [x T 1 , · · · , x T n ] T ∈ dn×1 . For the angle element θ i , a counter-clockwise rotation matrix is denoted by R θ i ∈ d×d . For a variable x, the estimation and the time derivative of x are denoted byx, andẋ, respectively.

A. GRAPH THEORY
The basic graph theory is introduced in [29]. Interactive network can be represented using the graph G = (V, E), where V = 1, 2, · · · , n is the set of n nodes, and E ⊆ V × V is the edges connecting the nodes. Especially, a set of node js connected to a certain node i through edges are referred to as neighboring nodes N i = {j ∈ V : (i, j) ∈ E}, where (i, j) is an edge from i to j. In this study, we ignore the case of a selfloop (i, i). The direction of an edge is indicated by an arrow and denotes the state observation between nodes, that is, the node located at the tail of the arrow observes the state of that located at the head. A specific node that does not observe any nodes is represented as a leader node.
The graph G can represent the adjacency matrix, which includes node and edge information. An adjacency matrix is an n × n square matrix, and its elements are determined according to the neighboring relationships between nodes. If a graph has no weight at all edges, the elements of the adjacency matrix for the graph are determined as follows: A consensus system for a node state x i , which converges to the average of neighboring node states is expressed aṡ For all nodes, (2) can be expressed aṡ where x is a state vector [x 1 , x 2 , · · · , x n ] T , and L is a Laplacian matrix whose elements are defined as else. (4)

B. NONHOLONOMIC CONSTRAINT
Constraints are relationships between variables that a system physically obeys. For any system, the configuration space Q, which is the set of all the locations that the system can reach can be defined. This set contains generalized coordinates q 1 , q 2 , · · · , q n , which represent the independent variables that can express the system in a minimal number. If the form of a vector constraint q = [q 1 , · · · , q n ] T ∈ Q consisting of generalized coordinates is in the form of (5), a function h i is termed a holonomic constraint.
where i = 1, · · · , k < n, and k is the number of constraints. Next, (5) can be transformed into (6) by time derivative: If we define H i := ∂h i ∂q 1 , · · · , ∂h i ∂q n , (6) can be simplified by using dot product: Generally, the constraint of a system is described in the form: If ω i can be integrated by all the elements of q, that is, ω i can be expressed in the form H i , (8) is considered a holonomic constraint. Otherwise, if (8) cannot be integrated in the form (5), it is considered a nonholonomic constraint.
A holonomic system indicates that all the constraints in the system are holonomic, and a system that has at least one nonholonomic constraint is considered a nonholonomic system [30].

C. REFERENCE FRAME
Variables that require geographical criteria must be defined in a specific reference frame. We can define a global reference frame in the form g , which is the absolute basis for the measurement of variables. Moreover, we classify a specific reference frame that can be defined for each agent i in a local frame in the form i , where is the abbreviation of local. The agents are represented by a local frame, that is, the local frame corresponding to each agent has an orientation and origin according to the direction and position of the agent. In particular, we distinguish the local frames for holonomic agents, nonholonomic agents, and intermediate state in the form h , n , and m , respectively. Moreover, a common frame c is introduced to compare the variables on each local frame [31]. The common frame has a certain angle to which total agents converge to the specific computed angle determined by the graph node edges and initial angles of each agent as its orientation angle.
For variables, a left superscript, left subscript, right superscript, and right subscript are used as the characteristics of the frame, the characteristics of the agent, the agent as an observation reference, and the agent to which the variable belongs, respectively. Particularly, the explanation of the agent is included in the right subscript and separated by a comma. For example, h p i j is a relative variable p of an agent j observed in the local h of an agent i.

D. ORIENTATION ALIGNMENT-BASED FORMATION CONTROL
An OAFC is a decentralized formation control based on orientation alignment and position estimation [27].
where k θ , kp, and k p are the proper gains, θ i is the orientation angle of an agent i with respect to g , p i j is the relative position of an agent j from an agent i, cp i is the estimated position of an agent i in c , and c p i, * is the final desired position of an agent i.
An orientation alignment (9) converges the orientations of agents to a consensus angle corresponding to the topology of the graph G over time. In this case, the converged angle FIGURE 1. Reference frames and variables of agents, where h p i j is a relative position vector p of certain agent j observed from reference frame of regarding the agent i . In addition, θ represents the orientation angles of each agent. It should be noted that the common frame c is the consensus frame in which total agents converge to the specific computed value determined by the graph node edges and initial angles of each agent.
functions as a common orientation for all the agents, allowing the comparison of their relative positions. Therefore, the frame with the angle as the orientation is set as c . A position estimation (10) estimates the position of the agentp c i based on the relative positions within c . A formation control (11) moves the agent so that the estimated position cp i in c is the same as the desired position c p i, * .

E. INSTANT ORIENTATION CORRECTION STRATEGY
Due to the nonholonomic constraints, direct implementation of orientation alignment control for the holonomic systems is not applicable to the nonholonomic systems. The core concept of IOCS is to follow the virtual desired positions of agents to eventually follow the local desired positions of the agents. Fig. 2 shows the entire system including the holonomic system-based control law (HSCL), the nonholonomic control stage (NCS), and the IOC). An HSCL is an equation in which the positional variables of OAFC for nonholonomic systems are replaced with the corrected values of IOCS. g where g n θ i is the orientation angle of a nonholonomic agent i,  and W (x) is a certain function that follows connectivity between m and n generated by nonholonomic constraints. The IOCS rotates variables in m and c by R ψ i with respect to their origin. The rotation angles for correction are calculated using (17), and the calculation is based on the estimated orientation of h . The corrected control target values in (19) are used as control inputs of nonholonomic agents. Notably, a nonholonomic agent cannot perfectly reflect an intermediate state as its physical motion because of its nonholonomic constraints. Therefore, the intermediate angles and positions are pursued by nonholonomic agents, as the local target orientation angles and positions. This property is represented by the local target position in (19).
where hθ is the estimated orientation angles of virtual holonomic agents, ψ is the converging angle to the orientation angle difference between g m θ and g n θ, and n p i i, * is the local target position in n i seen from an agent i.
The major challenge arises from the difference between the orientations of n and m . The mismatch of the orientation occurs because of the disturbance, dynamic model, and controller of nonholonomic systems. If a nonholonomic agent follows (9), we must consider the function W(x) that represents the aforementioned mismatch effect. g m θ, which does not match the actual movement of nonholonomic agents, is used as the target angular velocity. Therefore, the orientation angles of nonholonomic agents diverge. Owing to an abnormality in n , the relative position to other nonholonomic agents also diverges.
The IOCS corrects the difference in the orientations between n and m . Because g m θ is used as the desired orientation angle of the nonholonomic agent, the control of the agent should be performed according to m . Therefore, for the local variables of n and m to have the same value, a correction is required to rotate the local variables of n by an angle difference between the orientation angles of n and m . The orientation angle difference is estimated in (17) and ψ. In (13) and (14), the rotation matrix R ψ i is applied to the relative position of an agent j with respect to the origin of an agent i. Through this process, ψ converges to g n θ − g m θ. Therefore, the relative positions in m are rotated by +ψ from the orientation of n , and the relationship between the relative positions of an agent j observed in n and m is expressed as For (14), the local variables in m exist on the left-hand side. Therefore, the correction is achieved by rotating the variables in c on the right-hand side by +ψ, instead. Fig. 3 shows the correction process of the IOCS through the values of the local variables in n and m .
The intermediate state calculated using (14) is input to the NCS as the local target for each agent. Because the frames to which the local target and nonholonomic agent belong are different, m is converted into n using (20).
Correction algorithms have complexities for the dimension d and number of agents n. The local variable correction of an HSCL has a complexity of O(d 2 ) because it is a rotational transformation for a dimension. The IOCS for estimating the required rotation angle ψ has a complexity of O(n 2 ). The HSCL expression has a complexity of O(n 2 d 2 ).

F. APPLICATION TO NONHOLONOMIC AGENTS
A nonholonomic agent must converge to the desired orientation while executing point-to-point motion. One such algorithm proposed in [32] generates a reference position according to the following equation: where n p i i, is a virtual desired position, as a new local desired position, of a nonholonomic agent i, However, this algorithm cannot achieve the orientation alignment for a moving target position. In this study, as the positions of agents gradually converge to a certain value over time, this corresponds to a case in which the target position moves. When the target position moves, the virtual desired position moves accordingly, and the nonholonomic agent moves toward the virtual desired position. At this time, as the virtual desired position continues to move in a direction independent of the target orientation angle, the orientation of the nonholonomic agent moving to the virtual desired position is also aligned with that of the moving vector of the virtual desired position. To address this challenge, an update step size λ was introduced. The virtual desired position is updated to a new target whenever the distance between the old virtual desired position and the updated virtual desired position is more than a tolerance λ. Otherwise, the old virtual desired position does not update to a new target.

G. UNICYCLE-MODELED AGENT CONTROL
The linear and angular velocities of the unicycle-modeled nonholonomic agent are calculated using the position, orientation, and target position as follows: where k v and k ω are positive constants. For convenience, the velocities of the nonholonomic agent were defined as (22).

III. EXPERIMENTAL SETUP
In this section, general experimental conditions and the various apparatus are explained.

A. SIMULATION: COMPARISON OF HSCL WITH AND WITHOUT IOCS
The simulations were performed for 300 iterations each, and the initial positions and angles of the agents were randomly arranged for each simulation. Moreover, the simulation conditions including the initial positions, target positions, and simulation time were set equally for the condition with and without the IOCS. The desired positions for nonholonomic agents were (320, 360), (200, 240), (320, 120), and (440, 240) pixels.

C. EXPERIMENT: AGENT CONTROL SYSTEM
As shown in Fig 5, four unicycle-modeled E-pucks were used as nonholonomic system agents (GCtronic, Swiss). The initial position of the nonholonomic agents x and y were given randomly within the range of ±3 m, respectively. Additionally, the orientation angles of the agents were given randomly within the range of ±π rad. Particularly, the orientation of the leading agent was aligned to that of g . Therefore, the consensus angle θ * at which the agents finally converge is 0 rad. The positions and orientations of nonholonomic agents were measured by the OpenCV-based object detection algorithm using a web camera that has a maximum video resolution of 4096 × 2160 pixels and a maximum frame rate of 90 fps (Logitech BRIO, Swiss) [33]. However, to prevent computational delay, object detection is performed at a resolution of 640 × 480 pixels and a frame rate of 30 fps. In addition, four distinctive graphical markers were placed on the surface of each of the agents to measure the positions and orientations of each agent. After obtaining the positions and orientation angles of the agents, the control input was transmitted via a Bluetooth protocol. Image processing and feature detection are performed using oriented FAST and rotated BRIEF (ORB) with OpenCV, and K-means clustering was used to estimate the mean position of the marker detected by the camera [34]. In addition, the direction of the object was determined by calculating the intensity centroid for rotated feature detection using a BRIEF algorithm [35]. Key points and descriptors to distinguish each marker were extracted using ORB from the marker, and the image obtained from the camera were compared with each other. Then, the best-matched section of the image was considered as the position of the agents.

D. SIMULATION: THE UPDATE STEP SIZE EFFICIENCY
A simulation was performed to analyze the convergence efficiency of each agent according to the update step size λ and the number of agents. In each simulation, the cumulative angular differences of all the agents during the simulation between different λs were recorded for eight cases, namely λ = 0.25, 0.5, 0.75, 1.0, 1.25, 1.5, 1.75, and 2.0 (pixels). Therefore, a cumulative mean angular velocity error (CMAE) was introduced to evaluate the effect of λ: where L is the number of simulation rounds, N is the number of agents, n θ i|j (λ = 0.25) is the angle record of nonholonomic agents when λ is zero, n θ i|j (λ = 0.25) is the angle record of nonholonomic agents for eight cases in which λ is not 0.25, and t j is the converging time of the agents for each round. Additionally, the CMAE is calculated for four cases according to the number of agents, namely 3, 4, 5, and 6.  To compare the difference according to λs, all the other conditions such as initial positions or initial angles other than λ are the same. The simulation is performed for 1000 rounds (L = 1000), and both the initial positions and angles of the agents are randomly arranged for each simulation. In addition, the simulation time step δt was set at 0.02 s with a constant velocity model. Furthermore, the simulation is performed until all the agents are placed in the desired positions. The desired positions are arranged at equal intervals so that the positions forms a circle centered at (320,240) pixels with a radius of 120 pixels, that is, (320, 360), (200, 240), (320, 120), and (440, 240) pixels for four agents. The desired positions, target orientations, and interaction graphs for each number of agents are shown Fig 6.

IV. RESULTS
In this section, the experiment results described in section III are presented. Notably, the IOCS parameters are empirically chosen to control each agent (Table 1). Fig. 7 and 8 show the visualized the formation and position errors for each nonholonomic agents under holonomic system-based control without proposed IOCS approach.

V. DISCUSSION
In this section, the considerations of the experiment and the simulation results in section IV are discussed.

A. INFLUENCE OF IOCS ON HSCL
The effect of the IOCS can be confirmed through simulations for nonholonomic agents. During the iterations, nonholonomic agents without the IOCS do not make formation, and intermediate positions do not converge to estimate the positions ( Fig. 7 and 8). Conversely, as expected, nonholonomic agents with the IOCS are arranged to the desired positions after 300 iterations ( Fig. 9 and 10). Furthermore, the effect of IOCS can be confirmed even in the case where the formation FIGURE 20. Cumulative mean angular velocity errors (CMAE) for each number of agents according to each of the λ for round L = 1000. The boxed area represents 50% of the quantile of the cumulative error of angular velocity and red dots denote outlier samples beyond the box area. It can be known that as the λ grows, the cumulative error of angular velocity gets bigger. Note that the error was computed based on the simulation result when λ was 0.25.

FIGURE 21.
Required converge steps (0.02 s per step) of all agents for each number of agents according to each of the λ for round L = 1000. The boxed area represents 50% of the quantile of the cumulative error of angular velocity and red dots denote outlier samples beyond the box area. It can be known that as the λ grows, the converge time gets smaller.
dynamically changes (Fig. 11, and 12). Therefore, introducing a frame correction strategy such as the proposed IOCS when implementing holonomic control laws for agents that have nonholonomic constraints is effective.

B. POSITION AND ANGULAR FOLLOW-ABILITY OF IOCS METHOD
As expected, by using the IOCS strategy, the holonomic control method (OAFC method) is applied to nonholonomic agents in the experiment (Fig. 14). As shown in the result, to apply the IOCS method, the reference position frame should be computed in advance (Fig. 15). In the agent control experiment, both the absolute and angular errors between the desired positions and the angles of the agents converged to zero after 15 s (Fig. 16 and Fig. 19). However, by implementing the IOCS method to more high-performance designed robot agents, the converging time may be reduced with higher gains (Table 1). Therefore, the results show that by using the IOCS, other orientation alignment-based control strategies for holonomic systems can be applied to nonholonomic systems, such as a distributed consensus controller approach [26].

C. RELATIONSHIP BETWEEN λ AND CMAE
When applying the IOCS, the angle difference ψ is critical during the instant correction process because it is fundamental to follow the consensus frame (Fig. 18). Therefore, during the estimation of the absolute error between the orientation angle and nonholonomic frame, a bumping epoch, which can be interpreted as an inconstant angular velocity during agent control, was observed (Fig. 19). As expected, the bumping samples originated from the parameter λ (23). Particularly, regardless of the number of agents, the results of the CMAE analysis have shown that as λ increases, the cumulative mean angular velocity increases (Fig. 20). This is because λ is essentially an updated frequency of the intermediate frame m . Therefore, when applying the IOCS, a suitable value for λ should be set in advance for the precise control of multiple agents to prevent unstable angular outputs.

D. CONVERGE TIME TO TARGET POSITIONS
Contrarily, λ was relevant to the converging time to the desired points (Fig. 21). Notably, λ only contributes to the stability of angular control because the agents immediately change their intermediate frames when the distance of λ increases. Therefore, as λ decreases, the agents can precisely be controlled in angular and positional aspects. Conversely, when λ is unnecessarily small, local frames can be over-updated to degrade the performance of the convergence. In addition, depending on the number of agents, the sensitivity of converging time against λ was shown to be different. For instance, the mean coverage step iteration of agents when the number of agents was three was smaller than that of four. Therefore, following the usage of the IOCS, λ should be carefully chosen not to degrade the performance of the agent system considering the number of agents.

E. LIMITATIONS AND PRACTICAL FEASIBILITY
During the experiment of the agent control system, the OpenCV algorithm with the top-view camera was used to estimate the positions and orientations of each of the agents. However, due to the precision of the camera and the object detection algorithm, the error of positions and orientation angle remained for about 15 pixels, and ±0.3 radian (Fig. 16,  and 16). On the other hand, because the uncertainty of object detection is not considered in the simulation results, it shows a relatively high alignment accuracy (Fig. 10). Therefore, when applying IOCS in an actual environment, it is important to accurately estimate their each location.
In addition, careful consideration should be given to the situation where implementing IOCS aligns various formations. Due to the non-holonomic constraints, orientation alignment is not guaranteed if the desired position is beyond the domain of dimensions. Therefore, desired position and target should be located in the domain of physically feasible boundaries to align the agents effectively. Furthermore, the agents are able to be located closely until each agent collides and is physically not disturbing. Therefore, an appropriate collision avoidance algorithm should be carefully adopted not to degrade the performance of the system.

VI. CONCLUSION
In this study, a correction method, the IOCS, for nonholonomic dynamics was proposed to perform a control law targeting holonomic agents using nonholonomic systems. The IOCS corrected local variables in a local frame used in a holonomic system-based control law by using the difference in the orientation angle between the local frames of nonholonomic agents and intermediate states. By using the IOCS, the divergence of variables due to the difference between the orientations of local frames was successfully removed and the result was verified by experiment and simulations with unicycle-modeled agents. Therefore, nonholonomic agents could execute a holonomic system-based control law by applying the proposed correction strategy.