A Novel Control Architecture Based on Behavior Trees for an Omni-Directional Mobile Robot

Abstract

Robotic systems are increasingly present in dynamic environments. This paper proposes a hierarchical control structure wherein a behavior tree (BT) is used to improve the flexibility and adaptability of an omni-directional mobile robot for point stabilization. Flexibility and adaptability are crucial at each level of the sense–plan–act loop to implement robust and effective robotic solutions in dynamic environments. The proposed BT combines high-level decision making and continuous execution monitoring while applying non-linear model predictive control (NMPC) for the point stabilization of an omni-directional mobile robot. The proposed control architecture can guide the mobile robot to any configuration within the workspace while satisfying state constraints (e.g., obstacle avoidance) and input constraints (e.g., motor limits). The effectiveness of the controller was validated through a set of realistic simulation scenarios and experiments in a real environment, where an industrial omni-directional mobile robot performed a point stabilization task with obstacle avoidance in a workspace.

1. Introduction

Mobile robots are the core components of industrial automation systems, especially mobile manipulation applications [1]. The way in which their motion planning is carried out is an important research topic [2]. Mobile robot motion studies can be classified into three distinctive problem sets: path-following control [3,4], trajectory tracking [5,6], and point stabilization [7]. This paper presents a solution for the point stabilization of an omni-directional mobile robot with four mechanical wheels (a holonomic robot). Several control strategies have been implemented and validated in simulation and real environments, but they have mostly been applied to differential robots [8,9,10]. Omni-directional mobile robots have gained increasing attention as they are more maneuverable than non-omni-directional mobile robots, thus being more suitable for traveling in complex environments; their ability to perform simultaneous and independent translation and rotation movements allows them to obtain relatively sensitive and accurate responses when executing tasks [11]. Today, there are many advanced control alternatives for omni-directional robots: PID control [12], Fuzzy Logic control [13,14], Adaptive Fuzzy Sliding-Mode control [15], Sliding-Mode control [16], the Linear Quadratic Regulator (LQR) [17], and model predictive control (MPC) [18]. Nevertheless, due to their highly non-linear behavior and the number of actuators required to govern each wheel independently, the control of omni-directional mobile robots is a complex task. The above control proposals are effective but have very complex structures that are hard to adjust. Thus, a large amount of theoretical and technical knowledge is required to implement these controllers in practice. Also, the computational cost of certain alternatives makes them less affordable for real-world applications. Thus, many of the works found in the literature only present results validated in a simulation environment.

MPC, also known as receding horizon control, is considered one of the most attractive control strategies for non-linear constrained multiple-input multiple-output (MIMO) systems [19]. In MPC, a future control sequence minimizing an objective function is computed over a finite prediction horizon. The MPC problem is an optimal control problem (OCP) case. The type of MPC controller depends on the OCP formulation, and there are many efficient solution techniques [20]. The direct optimal control method is used in this work, which applies Newton-type optimization schemes to convert the OCP into a non-linear program (NLP) in the form of multiple-shooting discretization [21].

With the growing complexity of robotic systems, using higher-level representations has become increasingly important to improving their software quality and maintainability. State machines have long been among the most common modeling notations for mission specification. Still, in recent years, behavior trees (BTs) have attracted roboticists’ attention to express such high-level coordination [22]. A BT is a popular control architecture in the computer game industry and has more recently been applied in the robotics field [23,24,25]. A BT is a graphical representation of the control logic for autonomous agents. Thus, it provides a powerful and flexible tool for controlling robotic systems, allowing them to handle complex scenarios and adapt to changing situations while maintaining robustness and reliability [24]. By combining controller policies with BTs, relatively simple controllers can be combined to generate complex maneuvering capabilities [26]. BTs can offer a modular and robust framework for hybrid control, enabling goal-based and deliberative task execution [27].

Several control algorithms for mobile robots have been explored and developed over the last few years. However, approaches for implementing them in combination with higher-level representations to generate complex maneuvering capabilities have yet to be examined; ref. [26] was the only relevant work found in the literature in this regard. Other efforts have been made by the Nav2 (https://navigation.ros.org, accessed on 10 July 2023) developers, who used BTs to integrate navigation control logic.

This paper proposes a BT control architecture combined with a non-linear model predictive control (NMPC)-based motion planning scheme to control an omni-directional four-wheeled robotic system. The BT defines the high-level control structure determining the robot’s behavior. Further, the NMPC algorithm is encapsulated inside a behavior node to generate low-level optimal control actions for the robotic agent to execute the desired behavior. The BT specifies the goal (e.g., reaching a specific object) and the constraints (e.g., avoiding obstacles). At the same time, the NMPC algorithm generates the optimal control action to achieve the goal while satisfying the constraints. The proposed behavior tree is scalable and extensible, i.e., as the complexity of the robotic system increases or the complexity of the environment in which the robotic system is present increases, additional behaviors or control strategies can be added to the tree without the need to reformulate the complete tree or the NMPC algorithm. We propose a flexible system that quickly switches to an alternative behavior in a fault state. This is particularly useful in real-world robotic applications where unexpected situations can arise. This framework is integrated into the robot operating system (ROS) and applied to a real environment.

The paper is organized as follows: Section 2 presents the robotic agent used in the study and its kinematics. Section 3 presents the controller design formulation. Section 4 presents the validation of the proposed controller in simulated and real environments; a discussion of the approach is also presented. Finally, Section 5 presents the paper’s conclusions and future work.

2. Robot Modeling

An omni-directional mobile base with mecanum wheels (Figure 1) allowed the robot to move in any direction. The mechanical wheel mainly comprised a wheel hub and some passive rollers evenly distributed along the wheel’s outer edge at a certain angle. Based on the mechanic wheel’s characteristics and the system model analysis, the platform could conduct omni-directional movements with strong adaptability, high sensitivity, good stability, and flexible rotation.

Kinematics Modeling

Figure 1 shows the configurations of the robot and its wheels.

According to Figure 1a, the velocity of wheel i in frame $x^{\prime },o^{\prime },y^{\prime }$ can be obtained by

$$\left[\begin{array}{c}{v}_{i{x}^{\prime }}\\ v_{i{y}^{\prime }}\end{array}\right]=\left[\begin{array}{cc}0\hfill & sin{\alpha }_i\hfill \\ r_i\hfill & cos{\alpha }_i\hfill \end{array}\right]\left[\begin{array}{c}{\omega }_i\\ v_{ir}\end{array}\right]=K_{i1}\left[\begin{array}{c}{\omega }_i\\ v_{ir}\end{array}\right],$$

(1)

where all parameters are considered in terms of the wheel coordinates, wheel speed ($v_{i{x}^{\prime }},v_{i{y}^{\prime }}$), and angular velocity (${\omega }_i$). $v_{ir}$ represents the norm velocity of the wheel, and r represents the radius of the wheel. It is necessary to translate $v_{i{x}^{\prime }},v_{i{y}^{\prime }}$, written in terms of $x^{\prime }{o}^{\prime }{y}^{\prime }$ (wheel coordinates) to $v_{i{x}_r},v_{i{y}_r}$, written in terms of $x_r{o}_r{y}_r$ (robot coordinates). From Figure 1b, the equation in terms of the robot coordinates is as follows:

$$\left[\begin{array}{c}{v}_{i{x}_r}\\ v_{i{y}_r}\end{array}\right]=\left[\begin{array}{cc}cos{\theta }_i& -sin{\theta }_i\\ sin{\theta }_i& cos{\theta }_i\end{array}\right]\left[\begin{array}{c}{v}_{i{x}^{\prime }}\\ v_{i{y}^{\prime }}\end{array}\right]=K_{i2}{K}_{i1}\left[\begin{array}{c}{\omega }_i\\ v_{ir}\end{array}\right],$$

(2)

where $K_{i2}$ is the transition matrix.

According to the geometric properties, the transformation relationship of robot speed between different coordinate systems is obtained by

$$\left[\begin{array}{c}{v}_{i{x}_r}\hfill \\ v_{i{y}_r}\hfill \end{array}\right]=\left[\begin{array}{ccc}1& 0& -l_{iy}\\ 0& 1& l_{ix}\end{array}\right]\left[\begin{array}{c}{v}_{x_r}\hfill \\ v_{y_r}\hfill \\ {\omega }_z\hfill \end{array}\right]=K_{i3}\left[\begin{array}{c}{v}_{x_r}\hfill \\ v_{y_r}\hfill \\ {\omega }_z\hfill \end{array}\right],$$

(3)

where $l_{ix}$ and $l_{iy}$ are the distances between the wheel coordinates and the robot coordinates (see Figure 1b), and $l_i$ is the distance between the wheels and the base (the center of the robot).

The inverse kinematics model of the omni-directional four-wheel base can be obtained from (1) and (2):

$$K_{i2}{K}_{i1}\left[\begin{array}{c}{\omega }_i\\ v_{ir}\end{array}\right]=K_{i3}\left[\begin{array}{c}{v}_{x_r}\\ v_{y_r}\\ {\omega }_z\end{array}\right],\mathrm{i}=1,2,3,4.$$

(4)

Equations (1) and (2) relate the variables of the wheel structures and the center of the robot. With the inverse kinematics, the velocity of the system can be obtained by $v_{ir}$ (the linear velocity) and $w_i$ (the angular velocity) of wheel i in (5) and the inverse (forward kinematics) in (6).

$$\left[\begin{array}{c}{v}_{x_r}\\ v_{y_r}\\ {\omega }_z\end{array}\right]=K_i^{+}\left[\begin{array}{c}{\omega }_i\hfill \\ v_{i_r}\hfill \end{array}\right],$$

(5)

$$\left[\begin{array}{c}{\omega }_i\hfill \\ v_{ir}\hfill \end{array}\right]=K_i\left[\begin{array}{c}{v}_{x_r}\hfill \\ v_{y_r}\hfill \\ {\omega }_z\hfill \end{array}\right],$$

(6)

where

$$K_i=K_{i2}^{-1}{K}_{i1}^{-1}{K}_{i3},K_i^{+}={\left(K_i^T·K_i\right)}^{-1}{K}_i^{\top }.$$

(7)

Assuming ${\varphi }_i={\theta }_i-{\alpha }_i$ and considering the fact that $l_{ix}=l_{i}cos\beta i$, $l_{iy}=l_{i}sin\beta i$, and the wheels are the same size,

$$K_i=\frac{1}{-rsin{\alpha }_i}\left[\begin{array}{ccc}cos{\varphi }_i& sin{\varphi }_i& l_{i}sin({\varphi }_i-{\beta }_i)\\ rcos{\theta }_i& -rsin{\theta }_i& l_{i}sin(-{\beta }_i+{\theta }_i)r\end{array}\right].$$

(8)

The forward kinematics are used to determine the robot’s velocity, and the inverse kinematics are used to determine the wheels’ angular velocities ${\mathit{\omega }}_{\mathit{r}}={[{\omega }_1,{\omega }_2,{\omega }_3,{\omega }_4]}^{\top }$. Assuming that the wheels do not slip on the ground, the system inverse kinematics can be obtained by

$$\left[\begin{array}{c}{\omega }_1\\ {\omega }_2\\ {\omega }_3\\ {\omega }_4\end{array}\right]=\frac{-1}{r}\left[\begin{array}{ccc}\frac{cos{\varphi }_1}{sin{\alpha }_1}\hfill & \frac{sin{\varphi }_1}{sin{\alpha }_1}\hfill & \frac{l_{1}sin\left({\varphi }_1-{\beta }_1\right)}{sin{\alpha }_1}\hfill \\ \frac{cos{\varphi }_2}{sin{\alpha }_2}\hfill & \frac{sin{\varphi }_2}{sin{\alpha }_2}\hfill & \frac{l_{2}sin\left({\varphi }_2-{\beta }_2\right)}{{error}_2}\hfill \\ \frac{cos{\varphi }_3}{sin{\alpha }_3}\hfill & \frac{sin{\varphi }_3}{sin{\alpha }_3}\hfill & \frac{l_{3}sin\left({\varphi }_3-{\beta }_3\right)}{sin{\alpha }_3}\hfill \\ \frac{cos{\varphi }_4}{sin{\alpha }_4}\hfill & \frac{sin{\varphi }_4}{sin{\alpha }_4}\hfill & \frac{l_{4}sin\left({\varphi }_4-{\beta }_4\right)}{sin{\alpha }_4}\hfill \end{array}\right]\left[\begin{array}{c}{v}_{x_r}\\ v_{y_r}\\ {\omega }_z\end{array}\right].$$

(9)

The Jacobian matrix for the inverse kinematics is given by

$$K_i=\frac{-1}{r}\left[\begin{array}{ccc}\frac{cos{\varphi }_1}{sin{\alpha }_1}\hfill & \frac{sin{\varphi }_1}{sin{\alpha }_1}\hfill & \frac{l_{1}sin\left({\varphi }_1-{\beta }_1\right)}{sin{\alpha }_1}\hfill \\ \frac{cos{\varphi }_2}{sin{\alpha }_2}\hfill & \frac{sin{\varphi }_2}{sin{\alpha }_2}\hfill & \frac{l_{2}sin\left({\varphi }_2-{\beta }_2\right)}{{error}_2}\hfill \\ \frac{cos{\varphi }_3}{sin{\alpha }_3}\hfill & \frac{sin{\varphi }_3}{sin{\alpha }_3}\hfill & \frac{l_{3}sin\left({\varphi }_3-{\beta }_3\right)}{sin{\alpha }_3}\hfill \\ \frac{cos{\varphi }_4}{sin{\alpha }_4}\hfill & \frac{sin{\varphi }_4}{sin{\alpha }_4}\hfill & \frac{l_{4}sin\left({\varphi }_4-{\beta }_4\right)}{sin{\alpha }_4}\hfill \end{array}\right],$$

(10)

and the forward kinematics are given by

$$\left[\begin{array}{c}{v}_{x_r}\hfill \\ v_{y_r}\hfill \\ {\omega }_z\hfill \end{array}\right]=K_i^{+}\left[\begin{array}{c}{\omega }_1\hfill \\ {\omega }_2\hfill \\ {\omega }_3\hfill \\ {\omega }_4\hfill \end{array}\right].$$

(11)

The autonomous mobile manipulator robot (AMMR) used in this study comprised an omni-directional mobile base consisting of four mechanical wheels (Figure 1c), with the configuration parameters shown in

Table 1. Robot parameters.

i	Wheel	${\mathit{\beta }}_{\mathit{i}}$	${\mathit{\theta }}_{\mathit{i}}$	${\mathit{\alpha }}_{\mathit{i}}$	${\mathit{l}}_{\mathit{i}}$	${\mathit{l}}_{\mathit{ix}}$	${\mathit{l}}_{\mathit{iy}}$
1	${\omega }_1$	$\frac{\pi }{4}$	$\frac{\pi }{2}$	$-\frac{\pi }{4}$	l	$l_x$	$l_y$
2	${\omega }_2$	$-\frac{\pi }{4}$	$-\frac{\pi }{2}$	$\frac{\pi }{4}$	l	$l_x$	$l_y$
3	${\omega }_3$	$\frac{3\pi }{4}$	$\frac{\pi }{2}$	$\frac{\pi }{4}$	l	$l_x$	$l_y$
4	${\omega }_4$	$-\frac{3\pi }{4}$	$-\frac{\pi }{2}$	$-\frac{\pi }{4}$	l	$l_x$	$l_y$

.

Inserting the parameters presented in Table 1 into (9), the inverse kinematics are

$$\left[\begin{array}{c}{\omega }_1\\ {\omega }_2\\ {\omega }_3\\ {\omega }_4\end{array}\right]=\frac{-1}{r}\left[\begin{array}{ccc}\hfill 1& \hfill -1& \hfill -\left(l_x+l_y\right)\\ \hfill 1& \hfill 1& \hfill \left(l_x+l_y\right)\\ \hfill 1& \hfill 1& \hfill -\left(l_x+l_y\right)\\ \hfill 1& \hfill -1& \hfill \left(l_x+l_y\right)\end{array}\right]\left[\begin{array}{c}{v}_{x_r}\\ v_{y_r}\\ {\omega }_z\end{array}\right].$$

(12)

Inserting the parameters presented in Table 1 into (11), the forward kinematics are

$$\left[\begin{array}{c}{v}_{x_r}\hfill \\ v_{y_r}\hfill \\ {\omega }_z\hfill \end{array}\right]=\frac{r}{4}\left[\begin{array}{cccc}\hfill 1& \hfill 1& \hfill 1& \hfill 1\\ \hfill -1& \hfill 1& \hfill 1& \hfill -1\\ \hfill -\frac{1}{\left(l_x+l_y\right)}& \hfill \frac{1}{\left(l_x+l_y\right)}& \hfill -\frac{1}{\left(l_x+l_y\right)}& \hfill \frac{1}{\left(l_x+l_y\right)}\end{array}\right]\left[\begin{array}{c}{\omega }_1\hfill \\ {\omega }_2\hfill \\ {\omega }_3\hfill \\ {\omega }_4\hfill \end{array}\right].$$

(13)

3. Proposed Control Architecture

A behavior tree (BT) is a way of structuring the switching between different tasks of an autonomous agent. BTs are a very efficient means to create complex systems that are both modular and reactive. These properties are crucial in many applications. Figure 2 shows the control architecture that uses a BT to create a hierarchical control structure and an NMPC for the point stabilization of a generic autonomous mobile manipulator robot (AMMR). In short, it is an architecture that uses modularity, hierarchies, and feedback. Exploring the potential of BTs has already proved effective [26,27].

The architecture shown in Figure 2 comprises five main modules. The human–robot interface (HRI) module allows the human “operator” to send/update or cancel a goal for the robot. The low-level control block comprises the NMPC controller. A module for detecting environmental obstacles (Perception—Object detection) and the BT are responsible for the different modules’ hierarchical management control. These modules are discussed in more detail in the following sections.

3.1. Detection of Obstacles for Collision Avoidance

As the mobile robot moves, obstacles (either static or moving) may appear in the environment. It is then necessary to check whether these could make the robot’s trajectory unfeasible. A common simplification is to model moving and static obstacles as circles [28]. The following sections present the strategies used to update the properties of an obstacle if needed. At each sampling instant, the model verifies if new obstacles have appeared in the planned path. Whenever a new obstacle is detected, the control process is interrupted, and the controller constraints are updated with information regarding the new obstacles.

3.1.1. Autonomous Obstacle Detection

To automatically detect obstacles in the environment, the measurements made by the front and rear lasers equipping the mobile robot are processed before the controller’s state constraints are updated. It is necessary to transform the laser’s measurements into global coordinates. Figure 3 presents the proposed obstacle detection mechanism. The lasers measurements $d_i$, $i=1,2,\dots ,p$, are collected at each sampling time, while parameters $d_{max}$ and R are set as constant values. Parameters $d_{max}$ and R represent the maximum allowed distance between the mobile robot and the obstacle detected and the safety region radius, respectively. The obstacle encircles the safety region, not allowing the robot to penetrate it.

If one of the measurements $d_i$ satisfies $d_i\le d_{max}$, then the coordinates of the detected obstacle are stored and serve as input to Equation (14) (e.g., obstacles 1 and 2 in Figure 3), where p is the number of detected obstacles. In our approach, the safety regions are excluded from the optimization search. Furthermore, the state ${\theta }_w$ (robot orientation) is assumed to have no limitations. If the scan performed by the lasers detects relevant obstacles, then the set of inequalities of the state constraints are given by

$$\sqrt{{\left(x_w-x_{i_{obs}}\right)}^2+{\left(y_w-y_{i_{obs}}\right)}^2}\ge R,i=0,\dots ,p.$$

(14)

where $(x_w,y_w)$ are the coordinates of the location of the mobile robot, and $x_{i_{obs}}$, $y_{i_{obs}}$ are the coordinates of the points of the objects detected. All coordinates are in the global coordinate system (world frame).

3.1.2. Previously Known Obstacles

Obstacles may be previously known (i.e., previously detected by other robotics agents, environmental sensors, etc.). We have the information of the position in the world frame and the dimensions of these obstacles. For obstacle avoidance, a Euclidean distance (see Equation (15)) is used between the predicted location of the robot $(x_w,y_w)$ and the location of the obstacles $(x_{i_{obs}},y_{i_{obs}})$ in the world frame. This distance must be greater than the sum of $r_r$ and $r_{i_{obs}}$, representing the robot radius and obstacle radius, respectively.

$$-\sqrt{{\left(x_w-x_{i_{obs}}\right)}^2+{\left(y_w-y_{i_{obs}}\right)}^2}+\left(r_r+r_{i_{obs}}\right)\le 0,i=0,\dots ,.$$

(15)

3.2. Controller Formulation

A kinematic model can provide acceptable performance at a low speed and acceleration, and the effect of the robot mass can be neglected to reduce the computational effort [29]. Due to the specific requirements of the robot’s practical applicability, it will only work at low speeds, making the system less sensitive to disturbances such as inertia and friction, reducing the need for a complex dynamic model to consider external forces. Low speeds have slower dynamics, which allows for more flexible control requirements and potentially simpler models [30]. We aimed to design a feedback controller using the kinematics model given in (13) to find the optimal control inputs—in this case, the independent angular velocities at each wheel—so that the robot can move to a predetermined point and avoid previously known obstacles.

The state vector describes the omni-directional robot state $\mathit{x}={[x_w,y_w,{\theta }_w]}^{\top }$, which consists of the (spatial) position $(x_w,y_w)$ and the orientation angle ${\theta }_w$ of the robot in the world frame (Figure 1c). Additionally, the robot control input is defined by the vector $\mathit{u}={[u_1,u_2,u_3,u_4]}^{\top }$, where each input $u_i$, $i=1,2,3,4$ represents a corresponding wheel angular velocity. Note that $\mathit{u}={\mathit{\omega }}_{\mathit{r}}$.

This paper addresses a robotic system described by non-linear and time-invariant differential equations with state $\mathit{x}:\mathbb{R}\mapsto \mathcal{X}$ and control $\mathit{u}:\mathbb{R}\mapsto \mathcal{U}$. The discrete-time kinematics model of an omni-directional mobile robot is given by

$$\mathit{x}(k+1)=\mathit{f}\left(\mathit{x}\right(k),\mathit{u}(k\left)\right),$$

(16)

where $\mathit{x}\left(k\right)\in {\mathbb{R}}^n$ and $\mathit{u}\left(k\right)\in {\mathbb{R}}^m$ are the n-dimensional state and m-dimensional control vectors, respectively.

The control objective is to compute an admissible control input driving the system to move toward the equilibrium point defined by $\left(\mathit{e}\left(k\right)=0\right)$, where $\mathit{e}$ is the difference between the reference value and the system state vector in the world frame. The state signal vector is denoted as $\mathit{x}={[x_w,y_w,{\theta }_w]}^{\top }$. The reference state vector ${\mathit{x}}_{\mathit{ref}}={[x_{ref},y_{ref},{\theta }_{ref}]}^{\top }$ maintains the same value over time, corresponding to the desired goal. The error state vector $\mathit{e}$ is defined as

$$\mathit{e}\left(k\right)={\mathit{x}}_{\mathit{ref}}\left(k\right)-\mathit{x}\left(k\right).$$

(17)

The global control architecture is shown in Figure 4, where the control loop of an autonomous mobile manipulator robot using an NMPC law is represented. NMPC was developed for the point stabilization of an omni-directional mobile robot with four wheels. NMPC calculates the low-level control actions required at each step as a function of the error (see Equation (17)) using predictions of the system’s future behavior to determine the optimal control actions. The aim is to move the robot from one point to another with minimum error and drift while satisfying the constraints on the control input $\mathit{u}$. Each control input is subject to an inequality constraint as follows:

$$u_{min}\le u_i\le u_{max};i=1,2,3,4.$$

(18)

where $u_{min}$ and $u_{max}$ are the motor rated angular velocities or the required control limits.

We propose using an NMPC scheme to achieve the control objective. To this end, the objective of the control algorithm is to minimize a cost function given by [31]:

$$J(k,\mathit{e},\mathit{u})=F\left(\mathit{e}\left(N\right)\right)+\sum _{j=0}^{N-1}l\left(\mathit{e}\left(j\right),\mathit{u}\left(j\right)\right),$$

(19)

Here,

$$\begin{array}{cc}\hfill F\left(\mathit{e}\left(N\right)\right)& =\mathit{e}{(k+N)}^{\top }R\mathit{e}(k+N),\hfill \\ \hfill l\left(\mathit{e}\left(j\right),\mathit{u}\left(j\right)\right)& =\mathit{e}{(k+j)}^{\top }Q\mathit{e}(k+j)+\mathit{u}{(k+j)}^{\top }R\mathit{u}(k+j),\hfill \end{array}$$

(20)

where N is the prediction horizon, k is the sampling instant, and $Q\in {\mathbb{R}}^{n\times n}$ (penalizing deviation state) and $R\in {\mathbb{R}}^{m\times m}$ (penalizing deviation control) are positive definite symmetric weighting matrices with appropriate dimensions. The control horizon in this approach equals the prediction horizon. The computational efforts are not a problem due to the system characteristics; this way, we have more degrees of freedom to control. A control horizon shorter than the prediction horizon is typically used in constant-reference tracking problems. The optimal control solution is obtained as follows:

$${\mathit{u}}^{\ast }=\underset{\mathit{u}}{\arg \min }J\left(k,\mathit{e},\mathit{u}\right),$$

(21)

subject to

$$\mathit{x}\left(0\right)={\mathit{x}}_0,$$

(22a)

$$\mathit{x}(k+j+1)=f\left(\mathit{x}\right(k+j),\mathit{u}(k+j\left)\right),$$

(22b)

$$u_{min}\le \mathit{u}\le u_{max},$$

(22c)

$$\sqrt{{\left(x_{w(k+j)}-x_{i_{obs(k+j)}}\right)}^2+{\left(y_{w(k+j)}-y_{i_{obs(k+j)}}\right)}^2}\ge R,$$

(22d)

$$-\sqrt{{\left(x_{w(k+j)}-x_{i_{obs(k+j)}}\right)}^2+{\left(y_{w(k+j)}-y_{i_{obs(k+j)}}\right)}^2}+\left(r_r+r_{i_{obs(k+j)}}\right)\le 0,$$

(22e)

$$\mathit{x}(k+N)\in {\mathit{x}}_f,$$

(22f)

where $i=0,\dots ,p$, $j=0,\dots ,N-1$, ${\mathit{x}}_f$ is the terminal state constraint, and ${\mathit{x}}_0$ is an initial state. The solution of the optimal control problem represents the vector of optimal controls ${\mathit{u}}^{\ast }$. For state $\mathit{x}$ at time k, the optimal control approach produces the vector of optimal controls ${\mathit{u}}^{\ast }$ = $\left[u_0^{\ast },u_1^{\ast },\dots ,u_{N-1}^{\ast }\right]$.

The low-level control applied in this paper is equivalent to that deduced by Hsieh et al. in [31]. Hsieh et al. proved the controller’s stability, and the proof of stability is valid in this paper.

3.3. NMPC Implementation

The CasADi toolbox [32] was used to implement NMPC. CasADi is an open-source tool for non-linear optimization and algorithmic differentiation that sets up, solves, and performs forward and adjoint sensitivity analysis for systems of ordinary differential equations (ODEs) or differential-algebraic equations (DAEs) as well as formulating and solving non-linear programming (NLP) problems (https://web.casadi.org/docs, accessed on 12 July 2023). NLP problems are solved using a robust optimization solver, the IPOPT (Interior Point Optimizer) [33]. The interface for the IPOPT was provided by CasADi.

3.4. Implementation in the ROS Environment

The software system used to control the robot was entirely implemented under ROS Noetic (http://wiki.ros.org/noetic, accessed on 12 July 2023), running on Ubuntu 20.04.5 LTS. All ROS nodes were written in C++. The BT library BehaviorTree.CPP (https://github.com/BehaviorTree/BehaviorTree.CPP, accessed on 12 July 2023), an open-source C++ library supporting type-safe asynchronous actions, composable trees, and logging/profiling infrastructures for development, was used to implement a task-managing architecture (see Figure 5). BTs are a powerful and flexible tool for designing the control logic of autonomous agents in robotics, allowing for modular and scalable behavior design that can be easily adapted to different tasks and environments [34]. BTs are typically composed of nodes that determine the flow of execution and the outcome of actions. The primary statuses in a BT are running, failure, and success. There are four types of control flow nodes (fallback, sequence, parallel, and decorator) and two execution nodes (action and condition) [24]. The creators of the Naviagation2 [35] package for ROS2 [36] designed the recovery node (“RecoveryNode”), which was used in this work. This node links an action and a recovery action, as the name suggests. The first action is usually the “primary” behavior, and the second action is some action to be performed in the event of the failure of the main behavior. The ticking of the second child action often promotes the chance of the first action succeeding.

The BT presented in Figure 5 was designed to navigate a robotic agent from a starting pose to a goal pose in an environment. The behavior tree (BT) contains custom recovery behaviors in specific sub-contexts and a global recovery node (“GlobalRecoveryAction”) for system-level failures. In this BT, all navigation tasks are tried up to three times before returning to the failure state. It also allows the user to send up to five goals that must be reached before it is restarted. Initially, the process waits for a goal pose to be published (“WaitGoal” node); as soon as the goal is sent, the “GoalUpdate” node checks if it is different from the goals sent previously. The “ComputePathToPose” and “ReachPose” nodes use the NMPC presented in Section 3.2. Based on the available environmental information, the “ComputePathToPose” node checks whether designing a safe, collision-free trajectory for the robot from the starting pose to the goal pose is possible. The planned trajectory depends on the environmental information. If the goal is attainable, it publishes the calculated path. Otherwise, it calls the recovery node to recover from the fault conditions or wait for transient problems to pass, such as temporary sensor failures. The “ReachPose” node has the function of executing the control actions and sending them to the robot so that it reaches the goal position. Together with this node, the “PoseisFeasible” node marks all iterations to ensure the responsiveness of new goal objectives and verify the presence of new obstacles that make the goal unfeasible. If these contextual recoveries fail, the BT enters the global recovery node (“GlobalRecoveryAction”). This node is reserved for system-level failures. After each recovery from this node, the main navigation subtree is executed again.

To estimate the robot pose (position and orientation) in the environment based on sensor measurements, Adaptive Monte Carlo Localization (AMCL (http://wiki.ros.org/amcl), accessed on 12 July 2023)) is used.

The exact tuning of weighting matrices Q and R to minimize the overall cost function with reliable controller performance is a challenge and an ongoing research problem [37]. The lack of a unified procedure and the setup/perturbation-specific nature of these weights are additional challenges that increase the complexity of the problem. In this paper, we opted to adjust these weights manually using the ROS package “dynamic_reconfigure” (http://wiki.ros.org/dynamic_reconfigure, accessed on 12 July 2023), which provides a means to update parameters during the runtime without having to restart the node.

Previously known environmental obstacles are published via a topic created in the ROS environment. All obstacle information can be obtained based on sensors in the robot and sensors present in the environment through a semantic knowledge base [38]. The semantic information provides the controller with richer information regarding the constraints of the environment [39].

4. Simulation and Experimental Results

The proposed BT control architecture was implemented in an autonomous mobile manipulator robot (AMMR). These robots combine autonomous navigation with autonomous grasping. An omni-directional mobile base with mecanum wheels allows the robot to move in any direction. In this work, only the mobile base was considered. The AMMR contains sensors, such as lasers and cameras, for autonomous navigation and obstacle recognition. A simulated environment was created to validate the proposed controller. After validating the controller in the simulation environment, a strategy was developed to implement it in the ROS environment. All simulations and experimental results were conducted using a laptop running an Ubuntu 20.04.5 LTS operating system with an Intel^® Core^TM i7-7740X CPU @ 3.30 GHz × 8 processor, 16 GB RAM, and Quadro P2000/PCIe/SSE2 Graphics. A large number of experiments were performed that aimed to validate the potential of the proposed BT control architecture and define the best parameters for NMPC.

4.1. Simulation Results

NMPC achieved the point stabilization task in the simulation with obstacle avoidance. The robot started from the initial pose $\mathit{x}={[0.23,-1.37,0]}^{\top }$ (m, m, rad) and was commanded to stabilize at pose ${\mathit{x}}_{\mathit{ref}}={[2.52,0.79,0]}^{\top }$ (m, m, rad). An obstacle was placed at position $(x_{1_{obs}}=1,y_{1_{obs}}=0)$ (m,m) with an obstacle radius of $(r_{1_{obs}}=1)$ (m). NMPC was tested with two prediction horizons, $N=10$ and $N=20$. Furthermore, the sampling time of the controller was set to $0.2$ s, and several different values were tested for the weighting matrices Q and R to minimize the overall cost function with reliable controller performance. The selected values were the following:

$$Q=\left[\begin{array}{ccc}200& 0& 0\\ 0& 200& 0\\ 0& 0& 1000\end{array}\right],R=\left[\begin{array}{cccc}10& 0& 0& 0\\ 0& 10& 0& 0\\ 0& 0& 10& 0\\ 0& 0& 0& 10\end{array}\right].$$

(23)

To achieve the accurate localization of the robot and satisfy its actuator saturation limits, the controller saturation limits for the angular velocity ${\omega }_i=u_i,i=1,2,3,4$ were set as follows:

$$-4\le u_i\le 4;i=1,2,3,4.$$

(24)

Figure 6 presents the simulation results (with $N=10$ and $N=20$) and the controller performance for the case of point stabilization while the robot deviated from an obstacle. In Figure 6a, the robot’s position and orientation are represented by a circle with the center located at the robot’s position and an arrow indicating the robot’s orientation. The robot’s starting position is highlighted with a red circle and arrow. The green circle and arrow represent the robot’s actual position in the simulation. The blue circle and arrow represent the robot’s goal position. The blue dotted circle represents the object. The yellow dashed line represents the trajectory performed by the robot with prediction horizon $N=10$, and the dark dashed line represents the trajectory performed by the robot with prediction horizon $N=20$.

Figure 6a presents the trajectories performed by the AMMR with a prediction horizon of $N=10$ and $N=20$. NMPC with a prediction horizon of $N=20$ achieved a smoother trajectory than with a prediction horizon of $N=10$. This shows that a longer prediction horizon is necessary for the AMMR to encode more information about the environment, detecting obstacles earlier to ensure smoother navigation to the goal. The computational effort was exponential when increasing from N = 10 to N = 20. The computational time needed to determine the control action by optimizing the NMPC for N = 10 and N = 20 increased by ≈28.15% (from 5.72 ms to 7.33 ms); this was not a limitation in our case because the sampling time was 0.2 s, with $0.2>0.0073s$, and therefore there was enough time to deal with this increase in the complexity of the optimization problem associated with NMPC.

Figure 6b shows the linear velocities ($v_x$ and $v_y$) and angular velocity ($w_z$) sent to the robot. As shown in Figure 6c, the controller stabilized the robot to the desired final position. At the same time, it deviated from the obstacle present in the environment. Figure 6d shows the control actions. The robot angular velocities satisfied the saturation limits given by (24).

NMPC achieved the point stabilization task in a corridor (see Figure 7a). The robot started from the initial pose $\mathit{x}={[0,0.5,\frac{\pi }{2}]}^{\top }$ (m, m, rad) and was commanded to stabilize at pose ${\mathit{x}}_{\mathit{ref}}={[5,5,0]}^{\top }$ (m, m, rad). Three obstacles were placed in the corridor to increase the complexity of the environment: obstacle_1 $(x_{1_{obs}}=1,y_{1_{obs}}=4.5,r_{1_{obs}}=1.5)$ (m, m, m); obstacle_2 $(x_{2_{obs}}=1.2,y_{2_{obs}}=1.5,r_{2_{obs}}=1)$ (m, m, m); and obstacle_3 $(x_{3_{obs}}=3.5,y_{3_{obs}}=4,r_{3_{obs}}=1.5)$ (m, m, m). The blue dotted circles represent the objects. The walls were detected by the laser sensors attached to the robot and modeled as explained in Section 3.1, and the set of state constraints was given by (14). This experiment defined 46 circles (“obstacles”), which modeled the corridor’s walls.

The sampling time of the controller was set to $0.2$ s. It was tested with two prediction horizons, $N=20$ and $N=35$, and the Q and R matrices were the same as those used in the simulation (see (23)). To achieve the accurate localization of the robot and satisfy its actuator saturation limits, the controller saturation limits for the angular velocity were set as in (24).

In Figure 7a, the robot’s starting position is highlighted with a red circle and arrow. A green circle and arrow represent the robot’s actual position in the simulation. A blue circle and arrow represent the robot’s goal position. The yellow dashed line represents the trajectory performed by the robot with prediction horizon $N=20$, and the dark dashed line represents the trajectory performed by the robot with prediction horizon $N=35$. As the complexity of the environment increased, the $N=20$ prediction horizon became insufficient to obtain a smooth trajectory, as can be seen in Figure 7a, with the $N=35$ prediction horizon showing satisfactory results; the sampling period was not violated. Figure 7b shows the linear velocities ($v_x$ and $v_y$ ) and angular velocity ($w_z$) sent to the robot. As shown in Figure 7c, the controller stabilized the robot to the desired final position. At the same time, the mobile robot moved along the corridor, avoiding collision with the obstacles and the walls of the corridor. Figure 7d shows the control actions, and the robot’s angular velocities satisfied the saturation limits given by (24).

4.2. Experimental Validation

NMPC achieved the point stabilization task with obstacle avoidance in the experimental validation. The robot started from the initial pose $\mathit{x}={[0.23,-1.37,0]}^{\top }$ (m, m, rad) and was commanded to stabilize at pose ${\mathit{x}}_{\mathit{ref}}={[2.52,0.79,0]}^{\top }$ (m, m, rad). The obstacle was placed at position $(x_{1_{obs}}=1,y_{1_{obs}}=0)$ (m, m) with an obstacle radius of $(r_{1_{obs}}=1)$ (m). The sampling time of the controller was set to $0.2$ s with a prediction horizon of $N=20$, and the Q and R matrices were the same as those used in the simulation (see (23)). To achieve the accurate localization of the robot and satisfy its actuator saturation limits, the controller saturation limits for the angular velocity ${\omega }_i=u_i,i=1,2,3,4$ were set as in (24).

Figure 8 presents an overview of the experimental setup. Figure 8a presents the scenario created for controller validation, where the robot and the object (green cube) placed in the environment are visible. Figure 8b shows the environment where the robot was implemented, as well as its transform frame (TF (http://wiki.ros.org/tf, accessed on 12 July 2023)). The TF is a library used in ROS, an open-source framework that helps manage coordinate transformations between different frames of reference within a robot or a robotic system. This environment was obtained through the environment visualizer ROS RViz (http://wiki.ros.org/rviz, accessed on 12 July 2023). The two faces of the object present in the environment were delimited in green by the laser sensors attached to the robot, as can be seen in Figure 8b. The red arrow corresponds to the robot’s current position and orientation. The robot was in the goal position (${\mathit{x}}_{\mathit{ref}}={[2.52,0.79,0]}^{\top }$ (m, m, rad)). A circle representing the robot area can be seen in green. The red line represents the ideal trajectory generated initially, and the blue line represents the trajectory performed by the robot to reach the goal.

Figure 9a presents the linear velocities $v_x$ and $v_y$, as well as the angular velocity $w_z$, sent to the robot. Figure 9b presents the control actions and shows an acceptable control performance. The results confirmed that the constraints imposed on the maximal and minimal wheel speeds were not violated, i.e., (24) was satisfied for all time steps. The same behavior was observed for the remaining initial conditions.

We quantified the controller’s performance by calculating the root mean square (RMS) between the trajectory performed by the robot and the previously calculated trajectory (“optimal trajectory”). We used the RMS combined with the Euclidean distance to achieve this; see (25).

$$\mathrm{RMS}=\sqrt{\frac{1}{n}\sum _{i=1}^n\left({\left(x_{2i}-x_{1i}\right)}^2+{\left(y_{2i}-y_{1i}\right)}^2\right)}$$

(25)

where n is the number of points in each trajectory, and $\left(x_{1i},y_{1i}\right)$, and $\left(x_{2i},y_{2i}\right)$ are the coordinates of corresponding points in the two trajectories.

We obtained an RMS of 0.11 m, a satisfactory value; since we were not following the previously calculated trajectory, it was only used to check that the goal sent was feasible. The experimental results showed the good performance of the proposed control strategy.

Figure 10 combines the simulated results with the experimental results of the two tests carried out under the same conditions to prove the developed model’s quality. Figure 10a combines the speeds shown in Figure 6b and Figure 9a; Figure 10b combines the control signal shown in Figure 6d with that in Figure 9b. Analyzing the results, we can see that the model developed in the simulation was very close to the experimental validation when a real robot was used.

The controller proved to be effective in meeting the goal, even considering the sensors’ errors.

5. Conclusions

Robotic systems are increasingly present in dynamic environments. This paper proposed a hierarchical control structure wherein a behavior tree (BT) is used to improve the flexibility and adaptability of an omni-directional mobile robot for point stabilization. Flexibility and adaptability are crucial at each level of the sense–plan–act loop to implement robust and effective robotic solutions in dynamic environments. The controller was built based on the kinematics model of the robot. The BT combines high-level decision making and continuous execution monitoring while applying non-linear model predictive control (NMPC) for the point stabilization of an omni-directional mobile robot. The proposed control architecture can guide the mobile robot to any configuration within the workspace while satisfying state constraints (e.g., obstacle avoidance) and input constraints (e.g., motor limits). This proved to be a powerful and flexible tool. The effectiveness of the controller was validated through a set of realistic simulation scenarios and experiments in a real environment, where an industrial omni-directional mobile robot performed a point stabilization task with obstacle avoidance in a workspace. The behavioral hierarchy defined by the BT was shown to be suitable for dealing with events and situations where the robot needed to make decisions quickly based on the conditions of the environment, proving to be a robust strategy with high flexibility, managing to change the agent’s behavior based on unforeseen events such as the appearance of a new obstacle in the path.

Future work will extend the performed analysis to other challenging problems, such as trajectory tracking (i.e., following the trajectory generated by a global planner) [5,6] and path-following control [3,4].