There are two main contributions in this work. First, we introduce obstacle avoidance based on local environmental information that fits our original NMPC formulation. We also propose point cloud processing that can significantly reduce the computational burden for both two-dimensional and three-dimensional point clouds. Second, we conduct an HIL simulation to investigate the feasibility of implementing NMPC-based flock navigation on Raspberry Pi 4 in a fully distributed manner.

Section 2 briefly outlines the NMPC-based flock navigation with modified flocking rules, serving as a self-contained context for readers. Section 3 discusses the main results of this work, consisting of point cloud data processing, obstacle-avoidance constraints, and NMPC formulation featuring a local obstacle-avoidance strategy. Section 4 presents the evaluation of the algorithm through realistic simulation scenarios, as well as an HIL simulation conducted on embedded platforms.

A system consisting of N agents in an n p -dimensional space, categorized into leaders and followers, is considered. Leaders are given trajectories, while followers only react to their immediate surroundings and lack knowledge of any predefined destination. At time step t , let N i t be the index set of neighbors, including itself (Agent i ), and let N ̄ i t be the index set of neighbors without itself. Let x i t ∈ R n be the state vector of Agent i and let y i t be the vector that contains Agent i ’s position p i t ∈ R n p and velocity v i t ∈ R n p in the global frame defined as y i t = [ p i t ⊤ , v i t ⊤ ] ⊤ . The variable sequences defined along the prediction horizon T are bold-faced. For instance, the sequence of the control input u i t ∈ R n u , computed at time t , is u i t ≔ u i t | t …   u i t + T − 1 | t . Similarly, the sequence of the predicted state of Agent i is x i t . The nonlinear discrete-time state equation for each agent f i : R n × R n u → R n can be described by Equation 1: x i t + k + 1 | t = f i x i t + k | t , u i t + k | t , k = 0 , … , T − 1 . (1)

The goal is for followers to find optimal actions u i t | t for navigating through obstructed environments while minimizing deviation from the proposed flocking rules, thus ensuring fleet connectivity.

To achieve our main objective, we introduced a quadratic minimum-effort cost function given by J u i t = ‖ u i t ‖ R i 2 + ∑ k = 0 T − 1 γ k ‖ y i t + k + 1 | t − y ̄ i t + k + 1 | t ‖ Q i t 2 , (2) where R i ∈ R n u × n u , and Q i t ∈ R 2 n p × 2 n p are positive semi-definite diagonal weight matrices corresponding to the first and second term. Let y j | i t be the output of Agent j ∈ N ̄ i t , detected by Agent i at time instant t , and let y ̄ i t + k | t = [ p ̄ i t + k | t ⊤ , v ̄ i t + k | t ⊤ ] ⊤ be the weighted average of y j | i t + k | t , where p ̄ i t + k | t = ∑ j ∈ N i t + k | t w p , j | i t + k | t p j | i t + k | t , v ̄ i t + k | t = ∑ j ∈ N i t + k | t w v , j | i t + k | t v j | i t + k | t , (3) for k = 0 , … , T − 1 . The weights w p , j | i t + k | t and w v , j | i t + k | t will be discussed in the modified flocking rules section (Section 2.4). The errors of the predicted states can be accumulated during the prediction. Hence, a discount factor γ ∈ 0,1 is introduced by prioritizing the near-future prediction.

This section will discuss the subjective neighbor weights and the weighted matrix Q i t . The effect of each rule was explained in the prior work (see Nag and Yamamoto, 2024).

In point cloud processing, we utilize directional filtering and down-sampling, which can significantly reduce the number of data points while preserving vital features of the point cloud raw data. Because we do not consider the detected neighboring agents to be obstacles, the neighbor exclusion will be discussed in this section. The details for each process will be described in the following subsections.

We denote a set of processed point cloud’s index perceived by Agent i at time t that will be fed into the NMPC solver as O i t . The symbol O raw , i t refers to the index set of raw point cloud data from Agent i ’s sensor before any processing. Furthermore, we define O f , i t and O s , i t as the index sets of the processed point cloud after directional filtering and down-sampling, respectively.

Because most of the sensors perceive the environment in their body frame, the point data are analyzed in the agent’s body frame, where they are defined as follows: the x-axis points forward in the direction of its primary movement or orientation, representing its heading. The z-axis points upward, perpendicular to the plane of movement or operation. The y-axis completes the right-hand coordinate system, pointing to the robot’s left side, orthogonal to the x-axis (forward) and the z-axis (up). Consequently, the robot’s orientation can be briefly defined using the Euler angle ( ϕ , θ , ψ ), where ϕ i t denotes roll (rotation about the x-axis), θ i t signifies pitch (rotation about the y-axis), and ψ i t represents yaw (rotation about the z-axis) of the i t h agent at time t . Furthermore, vectors observed from the agents’ body frame are denoted with the subscript b .

To ensure obstacle-free trajectories along the finite prediction horizon for each agent, we impose an obstacle-avoidance constraint. Because obstacles are defined as processed point clouds in n p -dimensional space, let p m | i t ∈ R n p be an m t h obstacle’s position sensed by the i t h agent, where m ∈ O i t , and let r s ∈ R + be a safety distance away from an obstacle. The obstacle can be expressed as h p i t , p m | i t = r s 2 − ‖ p i t − p m | i t ‖ 2 . (19)

The obstacle-avoidance condition is that the trajectory of Agent i along the NMPC’s prediction horizon must be completely outside the sphere or circle defined by Equation 19. Thus, the obstacle-avoidance constraint can be defined as h p i t + k + 1 | t , p m | i t ≤ 0 , k = 0 , … , T − 1 . (20)

From the objective function (Equation 7) and the constraints Equations 4, 5, and 20 described earlier, the optimization problem can be formulated as Equation 21: Minimize u i t   J ̃ u i t (21) subject to Equation 22: x i t + k + 1 | t = f i x i t + k | t , u i t + k | t u i t + k | t ∈ U i x i t + k + 1 | t ∈ X i h p i t + k + 1 | t , p m | i t ≤ 0 ,   m ∈ O i t for  k = 0 , … , T − 1 d j | i t + k + 1 | t ≥ d i , sep ,   j ∈ N ̄ i t + k + 1 | t   for  k = 0 , … , T sep − 1 . (22)

This problem will be fed into the OpEn (see Sopasakis et al., 2020) framework to generate Rust code that solves the constrained optimization problem using PANOC (see Stella et al., 2017) with augmented Lagrangian and penalty methods.

In this work, the proposed optimal control problem is demonstrated through a close-to-real-world simulation in Gazebo (see Koenig and Howard, 2004), which incorporates physical quantities and three-dimensional dynamics, such as friction and inertia. This simulation environment is well integrated with the ROS. The agent model in our study is the unicycle ground vehicle model, chosen for its representation of most mobile robots and its inherent nonlinear properties as a nonlinear input-affine system.

The continuous-time state equation of the unicycle ground vehicle model is given by Equation 23: p ̇ i , x t p ̇ i , y t ψ ̇ i t = cos ψ i t 0 sin ψ i t 0 0 1 v i t ψ ̇ i t , (23) where v i ( t ) and ψ ̇ i ( t ) are the system inputs. The discrete-time state update equations can be derived through a forward Euler discretization. Additionally, the extra states, which are translational velocities with respect to the global frame, are added so that the alignment with its neighbors will be explicitly determined. Hence, the system’s state becomes x i k = p i , x k , p i , y k , ψ i k , v i , x k , v i , y k ⊤ ∈ R 5 , and the system’s input is u i k = v i k , ψ ̇ i k ⊤ ∈ R 2 . The discrete-time state equation can be expressed as p i , x k + 1 p i , y k + 1 ψ i k + 1 v i , x k + 1 v i , y k + 1 = p i , x k p i , y k ψ i k 0 0 + Δ t ⁡ cos ⁡ ψ i k 0 Δ t ⁡ sin ⁡ ψ i k 0 0 Δ t cos ⁡ ψ i k 0 sin ⁡ ψ i k 0 v i k ψ ̇ i k , (24) where Δ t is the sampling time in seconds. This discretized state equation will be used in NMPC to update the agent’s trajectory along the finite horizon period.

We utilized the Husky UGV from Clearpath Robotics (2015) as agents in the 3D Gazebo simulation. Figure 3 displays the simulated Husky UGVs and environment in our Gazebo 3D simulation. The robots are equipped with a UST10 simulated 2D LiDAR at their body center. The LiDAR has a default range of 5 m, a 360° angular range starting from the robot heading, and rotates counterclockwise, with 720 sampling points per round in the robot’s body frame polar coordinate. The output information from the LiDAR contains a time stamp, frames, sensor configurations, and ranges of detected obstacles, packed in the laser scanner message type in the ROS convention, which are two-dimensional point clouds. These raw data will be fed into the point cloud processor to produce the processed point data and then provided to the NMPC solver. The demonstration of the point cloud data is shown through RViz (ROS visualization) in Figure 4. The video of the 3D simulation with visualized point cloud data can be accessed via the following link: https://youtu.be/APg52Rw725M.

Note that the state Equation 24 represents the kinematic model of a unicycle mobile robot, which omits physical characteristics such as the relationship between motor voltage, current, torque, and speed. In other words, this model serves as a simplified representation of the proposed NMPC, where the controller generates high-level commands in the form of translational and angular velocities. In a real-world implementation, these commands would typically be passed to a low-level controller, such as a proportional–integral–derivative (PID) controller, which regulates motor actuation by adjusting electrical inputs accordingly. In this study, the high-level commands are sent to a virtual low-level system that converts the unicycle model’s control inputs into individual wheel speeds and torques. In the simulation, the wheels’ speeds are sensed and torques are regulated using well-tuned low-level PID controllers. A complete model incorporating other dynamic characteristics could be considered for more accurate state prediction in an NMPC scheme. However, such a model requires system identification techniques and is not within our scope.

In the simulation, we consider the system with three agents, consisting of one leader and two followers. The leader will accept the arbitrary input command and predict its translational position and velocity trajectory within the horizon period T based on the instantaneous inputs. The predicted trajectory will be shared with the followers within the detection range, which is assumed to be the same as the LiDAR default range. The followers also share their predicted trajectories from the NMPC optimizer with one another. The schematic diagram of follower Agent i is shown in Figure 5, where Agent j is the neighbor within Agent i ’s detection range j ∈ N ̄ i t + k | t .

The MPC horizon for all agents is set to T = 10 steps. The separation horizons are set as T sep = 5 steps, and the prediction of collision avoidance beyond T sep is softly constrained with a penalty parameter of ρ sep = 20 . The discount factor γ is set to 0.8. For the alignment weights of agents positioned behind, as stated in Equation 10, a value of β i = 0.5 is adopted. In the trade-off rule described in Equation 12, a default static coefficient of q i , st = 0.5 is utilized alongside a dynamic weight of c i = 10 . An upper limit of π ̄ = 3 is set to be equal to the total number of agents (including the leader) for the hierarchy levels according to Equation 8. The sampling time for state equation discretization Δ t is 0.1 s. Because we employ a two-dimensional laser scanner as a sensor in our simulation, the down-sampling processing is utilized based on Equation 15, where the down-sampling factor f s is set to 4.

We employed rectangular sets for the inputs and state constraints. The input boundaries for translational velocity are [ − 0.1 , 1.0 ] m/s and [ − 8,8 ] rad/s for rotational velocity. The state constraint can be interpreted as a square workspace in a two-dimensional plane originating at (0,0), in which the robots can move regardless of obstacles. We define the spatial limit as p x = p y = ± 10 meters. For obstacle-avoidance constraint, the inequality in Equation 20 is reformulated to min { 0 , h ( p i t + k + 1 | t , p m | i t ) } = 0 for k = 0 , … , T − 1 and m ∈ O i t . The obstacle-avoidance constraint is implemented through OpEn’s penalty method, while the state and separation (for k < T sep ) constraints are handled using the augmented Lagrangian method. The solver parameters are configured as follows: the solution tolerance ϵ and δ are set to 1 × 1 0 − 5 and 1 × 1 0 − 4 , respectively. The initial penalty λ 0 is set to 1 × 1 0 − 2 . The penalty update coefficient ρ , which is different from the separation soft constraint coefficient, is selected to be 5.

We tested the proposed optimal control formulation and algorithm by letting the leader agent track a reference trajectory in an obstructed environment created by Mukherjee (2015) and evaluating the followers’ trajectories resulting from the NMPC problem formulated in Section 3.3. The environment contains various obstacles, such as barriers, fire hydrants, dumpsters, and construction cones. The reference trajectory can be obtained by first specifying waypoints. Then, create points between the two immediate waypoints, where the distances between points are constant except for the remaining segments. Suppose that T p is the total number of points, and P ̂ = { p ̂ 0 , … , p ̂ T p } is a set of points from the given process. The reference trajectory for the leader agent ref P = { p 0 , … , p T p } is defined as   ref P = arg min p k , u k ∑ k = 0 T p − 1 q p ‖ p k − p ̂ k ‖ 2 + q u ‖ u k ‖ 2 + q T ‖ p T p − p ̂ T p ‖ 2 , k = 0 , … , T p , (25) subjected to the state Equation 24, where q p ∈ R , q T ∈ R , and q u ∈ R are weighted constants. The optimized trajectory, a so-called admissible path for unicycle mobile robots, is depicted in Figure 6. The leader agent will track this reference trajectory in Equation 25 employing a simple reactive controller, and the followers will preserve the fleet connectivity using the proposed NMPC scheme. Let us say the agent index for the leader is 0. Its control law is defined below. v 0 k = K v ‖ p ̂ k − p 0 k ‖ 2 ψ ̇ 0 k = K ψ ⋅ wrap ψ ̂ 0 k − ψ 0 k , k = 0 , … , T p , (26) where K v ∈ R and K ψ ∈ R are the proportional gains for each control channel. ψ ̂ k is defined by Equation 27: ψ ̂ 0 k = arctan p ̂ k − p 0 k y p ̂ k − p 0 k x . (27) A function wrap : R → R is a function that wraps the angle to be within the range of [ 0 , π ) , defined by Equation 28: wrap θ = θ − 2 π ⋅ θ 2 π . (28)

The initial poses of the robots are: Agent 0 at (5.86, −5.13) with 0 radians (the same pose as the initial waypoint in Figure 6), Agent 1 at (6.85, −6.25) with 1.5 radians, and Agent 2 at (7.87, −7.35) with 0 radians, where Agent 0 is the leader, and the others are followers. The resulting trajectories are shown in Figure 7, and the reference trajectory is illustrated as a broken black path. The control inputs throughout the experimentation are displayed in Figure 8 with the control inputs’ boundary shown as broken red lines. The simulation video can be accessed through the following link https://youtu.be/u8TcpLy7NKI. In the simulation, the leader is given a control input (Equation 26) to track the optimized path, and the two followers can maintain fleet connectivity and separation while avoiding obstacles using local information. In the demo, each follower recognized up to two neighbors if they were in the sensor’s range, regardless of the point cloud processing, which only simplifies the obstacle-avoidance constraint. The algorithm is scalable, but we only showed three robots for visualization purposes.

A comparison experiment is conducted to benchmark the proposed NMPC algorithm. For this purpose, the vector field histogram (VFH) algorithm, tailored for flock navigation, is employed. VFH is a simple reactive obstacle-avoidance strategy well-suited for a two-dimensional LiDAR sensor, where obstacle information is stored in polar coordinate form. The algorithm converts obstacle data into a one-dimensional histogram and selects a sector that is both obstacle-free and closest to the target, allowing the robot to navigate accordingly. See Liang et al. (2023) for more details on this algorithm. The simulation is performed in a similar manner, where the leader agent tracks the reference path (Equation 25), and the followers execute the tailored VFH algorithm to maintain fleet connectivity. The fleet connectivity is quantified by the deviation from the average position of all agents, that is, the deviation from the centroid. The plot comparing the deviation from the centroid for both the proposed method and the VFH algorithm is shown in Figure 9. The results indicate that the proposed algorithm navigates the robot fleet through an obstructed environment with better connectivity preservation.

In the previous subsection, the optimal control problem was solved successfully in a distributed manner on a station computer. However, the embedded processor might not have enough computational power to solve such a problem smoothly. Thus, in this section, we examine the feasibility of implementing the proposed NMPC on Raspberry Pi by connecting it to a station computer and performing a hardware-in-the-loop (HIL) simulation. In this setting, we use two Raspberry Pis to represent the two followers, Agents 1 and 2. The board model we use is Raspberry Pi 4 B, with a 1.5 GHz 64-bit quad-core ARM Cortex-A72 processor (ARM architecture), 1500 MHz clock speed, and 4 GB of RAM. The HIL simulation is conducted through a master–slave communication scheme, where the station computer is assigned to be a master in an ROS fashion. We connect the two Raspberry Pis to the same network as the simulation computer and create the communication bridge. Then, ROS topics published by either the master or the slaves can be subscribed to by every machine within this network. The NMPC solvers operate independently on each target board while exchanging information. The time required for the optimizer to attain the solution is crucial. The longer it takes, the more the performance degrades. To ensure that the solution for each optimizer is available within the discretization sampling period, set at 100 ms, we set the solve time cut-off to 95 ms, meaning that the solver will stop the operation once the solving time reaches the specified cut-off, and the lowest cost value in the last iteration will be the result. The leader is given a reference trajectory to track, akin to the simulation in the preceding section, utilizing the robots’ initial poses from the previous section. The schematic diagram of the experimentation is depicted in Figure 10.

The resulting trajectories of the HIL simulation are shown in Figure 11, and the time-series data of control inputs are depicted in Figure 12. Figure 13 displays the solving time in milliseconds (ms). The average and peak solving times are 17.19 ms and 95 ms for Agent 1 and 24.6 ms and 95 ms for Agent 2, respectively. The results show that the solver occasionally reached the cut-off time limit, particularly for Agent 2. However, this does not indicate a failure in controlling the robots. Rather, it means that the optimizer did not fully converge to the optimal solution within the allotted time. Nevertheless, the suboptimal solutions returned at the cut-off were sufficiently effective, as demonstrated in Figure 11. Figure 14 shows the CPU usage percentage for the NMPC solver node in each Raspberry Pi. Because it is a quad-core processor, the percentage can be up to 400%. For Agents 1 and 2, the peak percentages are 116.2% and 117.3%, respectively, while the average percentages are 98.2% and 97.82%, respectively. This means that the NMPC solver utilized approximately one core on average.