Autonomous aerial manipulation at arbitrary pose in SE(3) with robust control and whole-body planning

Abstract

Aerial manipulators based on conventional multirotors can conduct manipulation only in small roll and pitch angles due to the underactuatedness of the multirotor base. If the multirotor base is capable of hovering at arbitrary orientation, the robot can freely locate itself at any point in $S E (3)$ , significantly extending its manipulation workspace and enabling a manipulation task that was originally not viable. In this work, we present a geometric robust control and whole-body motion planning framework for an omnidirectional aerial manipulator (OAM). To maximize the strength of OAM, we first propose a geometric robust controller for a floating base. Since the motion of the robotic arm and the interaction forces during manipulation affect the stability of the floating base, the base should be capable of mitigating these adverse effects while controlling its 6D pose. We then design a two-step optimization-based whole-body motion planner, jointly considering the pose of the floating base and the joint angles of the robotic arm to harness the entire configuration space. The devised two-step approach facilitates real-time applicability and enhances convergence of the optimization problem with non-convex and non-Euclidean search space. The proposed approach enables the base to be stationary at any 6D pose while autonomously carrying out sophisticated manipulation near obstacles without any collision. We demonstrate the effectiveness of the proposed framework through experiments in which an OAM performs grasping and pulling of an object in multiple scenarios, including near 90° and even 180° pitch angles.

Keywords

mobile manipulation aerial manipulation geometric control robust control whole-body motion planning model predictive control

Introduction

Background and motivation

Mobile manipulators with various base platforms (quadruped (Sleiman et al., 2021), ball-balancing robot (Minniti et al., 2019), multirotor (Wang et al., 2023)) have been introduced to expand the workspace of the manipulator. In particular, thanks to the capability to locate itself in wider workspace, aerial manipulators utilizing aerial robots as a base have been actively studied (Ollero et al., 2022). However, existing aerial manipulators based on conventional multirotors can conduct manipulation only in small roll, pitch angles due to the underactuatedness of the multirotor base. If additional freedom exists for a multirotor base to hover at arbitrary orientation, the workspace of the manipulator can be considerably enlarged, and such enlarged workspace may enable a manipulation task that is originally not viable. Although aerial manipulators based on a fully actuated multirotor base (Ryll et al., 2019) are studied in Tognon et al. (2019) and Nava et al. (2020) to provide hoverability in non-zero roll, pitch angles, there still exist non-trivial regions in orientation space (e.g., near 90° pitch angle) where the multirotor cannot hover. Several studies have been proposed to enhance versatility by serially connecting multiple aerial robots (Zhao et al., 2018, 2023). However, when the task involves object grasping, these platforms require substantially large, collision-free workspace as they utilize aerial robot modules to enclose the object.

In this study, we aim to investigate a mobile manipulator that can locate the floating base in any 3D space with arbitrary position and orientation, as depicted in Figure 1. To achieve this goal, we consider an omnidirectional aerial manipulator (OAM) which is a combination of an omnidirectional multirotor (Allenspach et al., 2020) and a multi-degrees-of-freedom (multi-DoF) robotic arm. Our study focuses on addressing two main issues: developing a method (1) to stably control the position and orientation of the multirotor base regardless of the arm movement or manipulation task and (2) to enable whole-body motion planning while utilizing the omnidirectionality of the multirotor’s orientation and taking into account the surrounding environment for collision avoidance and manipulation.

Figure 1.

An omnidirectional aerial manipulator (OAM) conducting a precise manipulation task of grasping-and-pulling a bar (a) on the ground and (b) on a table. The whole-body motion is computed from the proposed cascaded motion planner, and the reference trajectory is tracked by the proposed geometric robust controller.

Problem description

First, from a control perspective, two types of techniques are required: geometric control technique allowing the control input to be defined at arbitrary orientation on $S O (3)$ and robust control technique capable of providing robustness irrespective of the arm movement or manipulation. If a local representation like Euler angles is used to express the orientation of the multirotor, it may become unstable near the 90° pitch angle due to the singularity issue, preventing the full utilization of omnidirectionality. Additionally, if the position and orientation of the base are not properly regulated by the movement of the robotic arm or the manipulation task, it may lead to failure in sophisticated manipulation tasks. Such tracking error can result in collision with the surrounding environment, leading to a crash. Although the two issues have been tackled individually for aerial manipulators, to the best of the authors’ knowledge, they have not been resolved simultaneously for OAM.

Next, from a planning perspective, it is essential to perform whole-body motion planning that considers both the position and orientation of the multirotor base, as well as the joint angles of the robotic arm. In doing so, it is necessary to: (1) consider the non-Euclidean configuration space $S O (3)$ of orientation and (2) enable online replanning. If motion planning is conducted separately for the multirotor and robotic arm, the search space of the planning problem is restricted, potentially resulting in sub-optimality or infeasibility despite the existence of feasible solutions. Additionally, when using local parameterization such as Euler angles to represent the orientation of the multirotor base, solution may not be obtained in certain initial conditions due to degeneracy of such local parameterization. Finally, to respond promptly to uncertainties especially during manipulation, online replanning capability is required.

For clarity, we will provide a complete formulation of these control and planning problems in Problem formulation section.

Method overview and contribution

To resolve these problems, we first propose a geometric robust integral of the tanh of the error (gRITE) controller. The proposed controller augments an integral of the tanh of the error term to a geometric nonlinear PID controller (Goodarzi et al., 2013) which has been applied to most existing OAMs (Su et al., 2023; Zhao et al., 2023; Bodie et al., 2021b), and stability of the closed-loop system is formally analyzed. The added integral term is shown to be effective in suppressing the ultimate error bound, which can be made arbitrarily small with sufficiently large control gain. Next, we propose a two-step trajectory-optimization-based whole-body motion planning method. There exist two main computation bottlenecks in applying optimization-based algorithms which may hinder real-time computation and even convergence of the solution: (1) high-dimensional, non-convex search space and (2) non-Euclidean space of $S O (3)$ .¹ Moreover, to represent the orientation of the base in a non-Euclidean space, nonlinear parameterizations such as quaternions or rotation matrices must be employed. To overcome these bottlenecks, we divide the problem into two steps where the first step solves an end-effector level subproblem, and the whole body motion considering end-effector trajectory tracking, whole-body kinematics, and physical constraints is computed in the second step.

We conduct comparative experiments for the proposed controller where the proposed one outperforms the counterpart (i.e., geometric nonlinear PID controller) in regulating both position and orientation in the presence of the robotic arm’s motion. Then, the proposed control and planning framework is experimentally validated where an omnidirectional aerial manipulator (OAM) (Figure 3) performs grasping-and-pulling tasks in various environments requiring precise manipulation. Mobile manipulation using the OAM is successfully executed in five different scenarios: (1) ground-basic, (2) ground-yaw, (3) ground-pitch, (4) table-far, and (5) table-close. Through the experiments, we demonstrate precise control performance regardless of disturbance, such as the movement of the robotic arm and ground effect, at arbitrary position and orientation of the base. Furthermore, the proposed planning algorithm is shown to effectively utilize whole-body motion including omnidirectionality of the floating base in conducting the manipulation task while taking into account multiple physical constraints in real-time faster than 10 Hz.

In summary, the main contribution of this work can be summarized as follows:

• We present a gRITE (geometric Robust Integral of the Tanh of the Error) controller for an omnidirectional multirotor base to enable precise mobile manipulation of the OAM. We formally prove that the proposed controller guarantees arbitrarily small ultimate bound with sufficiently large control gains.

• We present a two-step trajectory-optimization-based whole-body motion planning method for an omnidirectional aerial manipulator with a multi-DoF robotic arm. The proposed method is capable of online replanning in a confined space faster than 10 Hz and exploiting the entire space of $S O (3)$ .

• We conduct multiple experiments where an OAM with a multi-DoF robotic arm performs grasping-and-pulling an object either on the ground or on a table, which would not be possible for a conventional underactuated aerial manipulator with the same manipulator configuration. They demonstrate effectiveness and applicability of the proposed controller and planner.

Related work

In this section, we discuss related work on control and whole-body motion planning for the developed OAM. Comparison with existing state-of-the-art works on OAMs is summarized in Table 1.

Table 1.

Comparison with state-of-the-art works on omnidirectional aerial manipulators.

Study	OAM platform		Control		Motion planning
Study	Multirotor (# of actuators)	Manipulator (DoF)	Method	Disturbance rejection	Method	Whole-body kinematics	Collision avoidance	Online replanning
Ours	Tiltable (12)	Serial (4)	gRITE	✓	Two-step TO^a	✓	✓	✓
Bodie et al. (2021b)	Tiltable (12)	Parallel (3)	FF + PID	△^b	IK^a	✗^c	✗	✓
Su et al. (2023)	Tiltable (16)	Serial (3)	PID	△	VKC^a	△^d	✓	✗
Brunner et al. (2022)	Tiltable (12)	Fixed (0)	Impedance	✗	MPPI^a	△^e	✗	✓
Zhao et al. (2023)	Tiltable (16)	Fixed (0)	PID	△	N/A (wrench planning)
Bodie et al. (2021a)	Tiltable (12)	Fixed (0)	Force and impedance	✗	N/A (force planning)

^aTrajectory optimization (TO), inverse kinematics (IK), virtual kinematic chain (VKC), and model predictive path integral control (MPPI).

^bFeed forward (FF) term compensates only the disturbance from the manipulator’s motion and requires the acceleration measurement.

^cWhole-body motion is not jointly tackled in that only the joint angles are considered.

^dSingularity issue may occur due to the use of Euler angles.

^eNo joint angle is considered, but there is a possibility of extension to OAM with a multi-DoF manipulator.

Control

The first objective of this study is to design a geometric robust controller for an omnidirectional aerial manipulator (OAM). Accordingly, we first review robust/adaptive or geometric robust/adaptive control techniques applied to existing floating base robotic systems including multirotors and aerial/space manipulators, then investigate control methods applied to an OAM. Lastly, we discuss related work on the proposed control technique, which is the geometric robust integral of the tanh of the error (gRITE).

For the multirotor bases without considering robotic arms, various geometric robust/adaptive controllers have been proposed. Goodarzi et al. (2015) introduced an adaptive controller with performance analysis, while Bisheban and Lee (2021) extended this approach by proposing a neural network-based adaptive control method that handles not only constant parametric but also state-dependent uncertainties. Further advancing this direction, O’Connell et al. (2022) presented a framework that integrates learning-based and adaptive control techniques to accommodate state-independent uncertainties, such as wind disturbances. However, these adaptive control approaches still exhibit performance degradation when encountering time-varying uncertainties that are not included in the training data or lack parametric modeling. Recently, Wu et al. (2025) proposed an $L_{1}$ adaptive control method that ensures the ultimate boundedness of state errors even in the presence of time-varying uncertainties. Nevertheless, achieving a sufficiently small ultimate bound with this method requires a high cutoff frequency for the accompanying low-pass filter and high gain control for state errors. As a result, in practical experiments, the performance is likely to be limited by measurement noise.

Next, aerial/space manipulators inherently involve dynamic coupling effect between a floating base and a rigidly attached robotic arm (Kim et al., 2017), and the base can become unstable if this coupling effect is not suitably addressed in a controller (Huber et al., 2013). To resolve this problem, various studies on a conventional multirotor-based aerial manipulator (Lee et al., 2022; Kim et al., 2017; Lee et al., 2021; Liang et al., 2023) design robust controllers where the motion of the robotic arm is treated as external disturbance. However, their use of Euler angles in attitude control renders the controllers numerically unstable near 90° pitch angle, limiting their applicability to an OAM. Although Yu et al. (2020) propose a geometric robust controller using orientation error directly defined on $S O (3)$ , its validation is limited to simulation, and its performance may degrade when applied to a real robot due to the control law not being Lipschitz continuous. Similar technical challenges of handling uncertainties have been considered in the space robotics field where the bases are satellite or spacecraft freely floating in space (Zhu et al., 2019; Jia and Shan, 2020; Yao, 2021). However, these studies either assume planar dynamics or represent the attitude using Euler angles. Xu et al. (2024) tackled the control problem of a space manipulator with singularity-free attitude representation, but only ideal dynamics without uncertainties is considered.

Studies on an OAM utilize a controller considering the non-Euclidean property of $S O (3)$ to fully harness the omnidirectionality of the robot. However, as they consider only an OAM with a zero-DoF robotic arm (Bodie et al., 2021a; Brunner et al., 2022; Cuniato et al., 2023), robustness against the relative motion of the robotic arm to the multirotor base is not tackled in their controllers. An OAM with a multi-DoF parallel robotic arm is considered in Bodie et al. (2021b), and the authors design a controller with a feedforward term to compensate for the dynamic coupling effect of the robotic arm. However, as the method requires a precise dynamical model of the coupling effect and acceleration measurement, the performance may degrade when external disturbance (e.g., interaction wrench while grasping an object of an unknown mass) or excessive measurement noise exists. Su et al. (2023) present a nonlinear PID controller for an OAM equipped with a multi-DoF serial robotic arm, but its performance in disturbance attenuation may be insufficient for precise manipulation tasks like grasping-and-pulling.

This study proposes a gRITE controller, which is an extension to a RISE (robust integral of the sign of the error) controller first presented in Xian et al. (2004). The RISE control achieves asymptotic stabilizability for an uncertain system using a continuous control input, and it has been adopted in various platforms (Kamaldin et al., 2019; Deng and Yao, 2021; Shin et al., 2011). However, due to the derivative of the control input not being continuous, input chattering may occur when applying high gains. Accordingly, in the hardware perspective where desired force/torque is tracked by actuators, the actuators’ tracking performance may deteriorate, resulting in overall performance degradation. To alleviate such problem, Kidambi et al. (2021) replace the sign function with the tanh function when validating its controller in simulation, but analysis for the replaced tanh function is not conducted. The original stability analysis with the sign function can no longer be applied to our case where the sign function is substituted by the tanh function because there appears a residual term in the derivative of the Lyapunov function that cannot be shown to be non-positive. Xian and Zhang (2016) present formal stability analysis for a smooth counterpart of the RISE control using the tanh function, but the analysis is conducted only in $R^{n}$ space. A geometric RISE controller is designed in Gu et al. (2022) and is applied to a quadrotor, but its analysis is built upon a lemma that is not applicable to a case with external disturbance (Lemma 1 showing boundedness of the attitude error function), and the input chattering issue that may occur by high gains is not taken into account.

Whole-body motion planning

Our second objective is to present a whole-body motion planner exploiting the omnidirectionality of an OAM while abiding by various state constraints such as collision avoidance. Considering this objective, we first review whole-body planning techniques for ground robot-based mobile manipulators. Then, we investigate planning algorithms designed for aerial manipulators including OAMs. Lastly, we discuss related work on optimization-based motion planning where the whole configuration space of orientation, that is, $S O (3)$ , is addressed.

A whole-body motion generation algorithm developed for a mobile manipulator whose base is a planar wheeled robot can be found in Zhang et al. (2020). Planar mobile robots only demand single scalar values representing heading angles to fully describe the orientation whereas at least three variables are needed for the OAM’s base. Thus, it is not directly applicable to the OAM. Quadruped robots have widely been used as bases of mobile manipulators (Sleiman et al., 2021; Arcari et al., 2023; Chiu et al., 2022). However, since they conduct mobile manipulation while maintaining the pedals to have a stable contact with the ground, roll and pitch angles of the base orientation is restricted below 90°. Accordingly, they rely on Euler angles to represent the base orientation. Another widely studied mobile manipulation platform is humanoids. Burget et al. (2013) and Qin et al. (2023) leverage mobility such as standing up from a seated posture, and Murooka et al. (2021) incorporate bipedal locomotion for loco-manipulation. However, they either do not fully exploit bipedal walking, or when whole-body motion is considered, computation time is on the order of seconds even when using Euler angles due to the high-dimensional state space. If the full $S O (3)$ space were considered, the computational complexity would increase abruptly, making real-time replanning practically infeasible. Meanwhile, the OAM is capable of hovering with arbitrary base pose. Thus, to fully utilize such capability during manipulation while applying the methods for quadruped robots, one should additionally consider the entire $S O (3)$ and the corresponding nonlinear kinematics.

Similarly, existing studies on aerial manipulators (Lee et al., 2020; Lee and Kim, 2017) have limitations in that they utilize Euler angles in designing a motion planner. Although there exist studies employing the whole configuration space of $S O (3)$ (Tognon et al., 2018; Welde et al., 2021), their approaches do not tackle state constraints including obstacle and self-collision avoidance and are only validated in simulation. As presented in the previous subsection, many studies on OAM consider a platform equipped with a zero-DoF manipulator (Bodie et al., 2021a; Brunner et al., 2022; Cuniato et al., 2023). Among these, only Brunner et al. (2022) tackle a problem of motion planning for OAM, but manipulator-related constraints including self-collision avoidance are not considered. Recent work (Su et al., 2023) also investigates whole-body motion planning for an OAM with a multi-DoF manipulator. However, the authors represented OAM’s base orientation using only three angles, leading to singularity issues, and the method can only be computed offline.

Several studies have explored trajectory generation on $S E (3)$ for different robotic platforms. Quadrotors have been studied in Wehbeh and Sharf (2022) and Sun et al. (2022), while Brescianini and D’Andrea (2018) focused on fully actuated multirotors. Meduri et al. (2023) studied humanoids and quadrupeds, and Kalabić et al. (2017) addressed motion planning for spacecraft and omnidirectional multirotors. Although these approaches enable real-time replanning, their applicability can be limited when handling collision avoidance which is typically formulated as non-convex constraints.

On the other hand, there exist studies that incorporate collision-free trajectory generation on $S E (3)$ . Ding et al. (2021) and Liu et al. (2025) can generate a reference trajectory while ensuring collision avoidance by modeling the entire system as a single rigid body. However, this simplification yields an overly conservative behavior of systems with high-DoF robotic manipulators, as the rigid-body model must cover all possible reachable regions of the arm. Fankhauser et al. (2018) consider foot collision avoidance using a quaternion-based representation but neglects the base, which is critical in omnidirectional aerial manipulation. While Schulman et al. (2014) proposed a platform-agnostic framework, its high computational load prevents real-time implementation. Since aerial manipulators operate in free space, they are highly susceptible to disturbances and require rapid replanning to react to potential disturbances.

Problem formulation

This section presents objectives and corresponding formulation for each control and planning problems. Also, we introduce notations used throughout the paper in Table 2.

Table 2.

List of notations and variables.

Notations	Definition
[ a ; b ]	${[a^{⊤} b^{⊤}]}^{⊤}$ with two vectors a and b
a _i	i^th element of a vector a
W _i	i^th element of a diagonal matrix W
‖ v ‖	Euclidean norm of a vector v
$‖ v ‖_{P}^{2}$	v ^⊤ Pv with a vector v and a positive definite matrix P
I _n	Identity matrix in $R^{n \times n}$
(⋅)^∧	Matrix representation of the vector cross product in $R^{3}$
(⋅)^∨	Inverse of the hat map
c∗, s∗	cos(∗), sin(∗)
$A \oplus B$	Minkowski sum between two sets $A and B$
$S^{c}$	Complement of a set $S$
Tanh( v )	Element-wise tanh of a vector v
tr( A )	Trace of a square matrix A
λ_m( A )	Minimum eigenvalue of a square matrix A

Variables	Definition
p	Position of the multirotor base
R	Orientation of the multirotor base
ω	Body angular velocity of the multirotor base
θ	Joint angles of the manipulator
${}^{E}p,^{E} v$	End-effector position and linear velocity
${}^{E}R,^{E} ω$	End-effector orientation and angular velocity
d _t, d _r	External disturbance in translation and rotational dynamics
m, J _b	True mass and MoI* of the aerial manipulator
$\bar{m}, \bar{J_{b}}$	Nominal mass or MoI* of the aerial manipulator
f , τ	Net force and torque in body frame, i.e., control inputs
F , α	Thrusts and servo angles of rotors
(⋅)_n, (⋅)_r	Nominal and robust control law
(⋅)_d, (⋅)_g	Desired and goal value
$O_{i}$	Set of all points in $R^{3}$ constructing i^th obstacle
N _o	Number of obstacles
T _f	Desired reaching time to the goal used in offline planner
T _H	Time horizon used in online planner
Δt	Time discretization
g	Magnitude of the gravitational acceleration
b ₃	[0, 0, 1]^⊤

MoI: Moment of inertia.

The controller is designed with the following objectives:

• Robustness to external disturbance and model uncertainty to ensure robustness against multiple sources of disturbance including the robotic arm’s motion, ground effect and model uncertainty arising from an object at the end-effector.

• Well-defined control law in the entire state space without singularity to fully exploit the omnidirectionality of the aerial robot which can be hindered if using local coordinates such as Euler angles for representing orientation.

As for whole-body motion planning, the objectives are:

• Collision-free trajectory generation to avoid both self-collision between the manipulator and the base, and collision with static obstacles in the environment.

• Simultaneous exploration of the joint angles and base pose over the configuration space $R^{3} \times S O (3) \times R^{n}$ to maximize the feasible solution set.

• Fast computation of trajectories to ensure real-time replanning capability for reacting to potential uncertainties.

Based on these objectives, we define the following control and planning problems under assumptions:

Geometric robust control

For any given smooth reference trajectory for the omnidirectional multirotor base (i.e., position and orientation of the base), design a geometric controller capable of bounding the state tracking errors arbitrarily small under the presence of disturbances and model uncertainties.

Whole-body motion planning

For the given goal pose of the end-effector, find a whole-body motion trajectory of the OAM (i.e., pose of the floating base and joint angles of the robotic arm) that utilizes the entire configuration space $C = R^{3} \times S O (3) \times R^{n}$ and is free from self-collision and avoids obstacles while enforcing the end-effector to reach the goal.

Assumption 1

External disturbances and their time-derivatives $d_{t}, {\dot{d}}_{t}, d_{r}, {\dot{d}}_{r}$ are continuously differentiable and bounded, and ${\ddot{d}}_{t}, {\ddot{d}}_{r}$ are bounded.

Assumption 2

All obstacles are static, known, and modeled as composites of multiple ellipsoids.

Assumption 3

There exists at least one curve in ${O B S}^{c} \subset R^{3}$ starting from any points in ${O B S}^{c}$ to the goal position in ${O B S}^{c}$ where the set $O B S$ is defined as $O B S = ⋃_{i = 1}^{N_{o}} O_{i}$ .

The first assumption is about the boundedness of the external disturbance and its time-derivatives, which is common among other papers tackling robust control against time-varying disturbances (Kim et al., 2017; Lee et al., 2022; Hua et al., 2021). The second is to allow us to formulate collision avoidance constraints with obstacles using finite dimensional parameters. The last assumption can be interpreted as the goal not being positioned within a fully obstructed region enclosed by obstacles. This is to ensure existence of a path from any initial position to the goal position of the end-effector.

To address the control problem, a geometric robust integral tanh of the error (gRITE) controller is proposed, and we show that the proposed method guarantees an arbitrarily small state error bound. For the planning problem, a two-step trajectory-optimization-based whole-body motion planning method is introduced, which considers the entire configuration space while maintaining real-time performance ( $> 10$ Hz). The flow between the algorithmic and hardware components of the proposed framework is illustrated in Figure 2.

Figure 2.

Overall algorithm flow for hardware experiments. EE and WB stand for end-effector and whole-body, respectively.

Controller design

In this section, we propose a geometric Robust Integral Tanh of the Error (gRITE) controller which guarantees ultimate boundedness of the closed-loop system with arbitrarily small ultimate bound by choosing proper control gains.

System dynamics

We consider the following system dynamics:

\begin{align} m \ddot{p} & = R f - m g b_{3} + d_{t} \end{align}

(1a)

\begin{align} J_{b} \dot{ω} & = - ω^{\land} J_{b} ω + τ + d_{r} \end{align}

(1b)

(1a) models translational dynamics and (1b) is for rotational dynamics. The motion of the robotic arm induces a change in the moment of inertia of the aerial manipulator J _b and reaction force and torque to the multirotor, and these are considered as model uncertainty in J _b and external disturbance d _t and d _r, respectively. If the end-effector of the robotic arm grasps an object, then the added inertia of the attached object is accounted as uncertainty in m and J _b since the added inertia is generally unknown. Control inputs are force $f \in R^{3}$ and torque $τ \in R^{3}$ represented in the multirotor body frame, indicating fully actuatedness.

Control allocation

The OAM considered in this paper is illustrated in Figure 3. The omnidirectional multirotor base consists of six rotors and six servomotors. As visualized in Figure 3, actuator inputs are $F_{i} \in R_{> 0}$ and $α_{i} \in R$ , which are thrust of the i^th rotor and servo angle of the i^th servomotor, respectively. To realize a control input $[f; τ] \in R^{6}$ in the multirotor, control allocation is required which solves a problem of finding actuator inputs $F = {[F_{1}, \dots, F_{6}]}^{⊤} \in R^{6}$ and $α = {[α_{1}, \dots, α_{6}]}^{⊤} \in R^{6}$ for given control input [ f ; τ ]. Using the variable transformation method in Kamel et al. (2018), the relationship between the actuator inputs F , α and control input [ f ; τ ] can be written as

[\begin{matrix} f \\ τ \end{matrix}] = A b, b = [\begin{matrix} F_{1} c α_{1} \\ F_{1} s α_{1} \\ ⋮ \\ F_{6} c α_{6} \\ F_{6} s α_{6} \end{matrix}]

(2)

where

A \in R^{6 \times 12}

is a constant matrix, whose definition can be found in Appendix 9.1. Such equation is derived from the geometric arrangement of actuators in the multirotor base. We compute the vector b using a weighted pseudo-inverse of A as

b = A_{W}^{†} [\begin{matrix} f \\ τ \end{matrix}]

(3)

where

A_{W}^{†} = W A^{⊤} {(A W A^{⊤})}^{- 1}

for a positive definite weight matrix

W \in R_{> 0}^{12 \times 12}

. Then, the actuator inputs F_i, α_i ∀i = 1, …, 6 are calculated as

\begin{aligned} F_{i} & = \sqrt{b_{2 i - 1}^{2} + b_{2 i}^{2}} \\ α_{i} & = atan 2 (b_{2 i}, b_{2 i - 1}) . \end{aligned}

(4)

Figure 3.

CAD for the omnidirectional aerial manipulator. Actuator inputs are visualized which are rotor thrust $F = {[F_{1}, \dots, F_{6}]}^{⊤}$ and servo angle $α = {[α_{1}, \dots, α_{6}]}^{⊤}$ .

The proposed control law: gRITE controller

In this subsection, we construct control laws for f in the translational dynamics (1a) and τ in the rotational dynamics (1b).

Translational dynamics

We design a controller for the translational part first. We define error variables as

\begin{aligned} e_{p} & = p_{d} - p, & e_{t 1} & = {\dot{e}}_{p} + Λ_{t} e_{p} \\ e_{t 2} & = {\dot{e}}_{t 1} + e_{t 1}, & e_{t} & = [e_{p}; e_{t 1}; e_{t 2}] \end{aligned}

(5)

where

Λ_{t} \in R_{> 0}^{3 \times 3}

is a diagonal control gain. Then, the proposed gRITE controller for the translational dynamics is formulated as follows:

\begin{align} f & = f_{n} + f_{r} \end{align}

(6a)

\begin{align} f_{n} & = \bar{m} R^{⊤} (g b_{3} + K_{t p} e_{p} + K_{t d} {\dot{e}}_{p} + {\ddot{p}}_{d}) \end{align}

(6b)

\begin{array}{l} f_{r} & = R^{⊤} {(K_{t i} + ρ_{t} I_{3}) (e_{t 1} (t) - e_{t 1} (0)) + \end{array}

\begin{align} \int_{0}^{t} (K_{t i} + ρ_{t} I_{3}) e_{t 1} (τ) + Γ_{t} Tanh (Θ_{t} e_{t 1} (τ)) d τ} \end{align}

(6c)

$K_{t p}, K_{t d}, K_{t i}, Γ_{t}, Θ_{t} \in R_{> 0}^{3 \times 3}$ and $ρ_{t} \in R_{> 0}$ are diagonal control gains. Briefly speaking, the nominal control law f _n exponentially stabilizes the nominal dynamics (i.e., (1a) with nominal parameters and no disturbance d _t) while the robust control law f _r mitigates the gap between the nominal and actual dynamics.

Rotational dynamics

Similar to the controller design for the translational dynamics, we take error variables first in designing a controller for the rotational counterpart as follows:

\begin{gathered} e_{R} = \frac{1}{2} {(R^{⊤} R_{d} - R_{d}^{⊤} R)}^{\lor}, e_{ω} = R^{⊤} R_{d} ω_{d} - ω \\ e_{r 1} = e_{ω} + Λ_{r} e_{R}, e_{r 2} = {\dot{e}}_{r 1} + e_{r 1}, e_{r} = [e_{R}; e_{r 1}; e_{r 2}] \end{gathered}

(7)

The error variables for the orientation e _R and the angular velocity e _ω are defined following Lee et al. (2010) where

R_{d} \in S O (3)

and

ω_{d} \in R^{3}

are the desired orientation and body angular velocity, respectively.

Λ_{r} \in R_{> 0}^{3 \times 3}

is a diagonal control gain. Then, the proposed gRITE control law for the rotational dynamics is as follows:

\begin{align} τ & = τ_{n} + τ_{r} \end{align}

(8a)

\begin{array}{l} τ_{n} & = ω^{\land} {\bar{J}}_{b} ω - {\bar{J}}_{b} (ω^{\land} R^{⊤} R_{d} ω_{d} - R^{⊤} R_{d} {\dot{ω}}_{d}) + \end{array}

\begin{align} {\bar{J}}_{b} K_{r p} e_{R} + {\bar{J}}_{b} K_{r d} e_{ω} \end{align}

(8b)

\begin{array}{l} τ_{r} & = (K_{r i} + ρ_{r} I_{3}) (e_{r 1} (t) - e_{r 1} (0)) + \end{array}

\begin{align} \int_{0}^{t} (K_{r i} + ρ_{r} I_{3}) e_{r 1} (τ) + Γ_{r} Tanh (Θ_{r} e_{r 1} (τ)) d τ \end{align}

(8c)

$K_{r p}, K_{r d}, K_{r i}, Γ_{r}, Θ_{r} \in R_{> 0}^{3 \times 3}$ and $ρ_{r} \in R_{> 0}$ are diagonal control gains, and the nominal control law τ _n exponentially stabilizes the nominal error dynamics where the nominal moment of inertia ${\bar{J}}_{b}$ is considered and no disturbance d _r exists (Lee et al., 2010).

Stability analysis

Using the derived control laws for the translational and rotational dynamics (6a)–(6c) and (8a)–(8c), we analyze stability of each closed-loop system. Note that the following analysis is motivated by Xian and Zhang (2016).

Translational dynamics

From (1a), (6a)–(6c), dynamics of e _t2 can be written as

m {\dot{e}}_{t 2} = N_{t} - (K_{t i} + ρ_{t} I_{3}) e_{t 2} - Γ_{t} Tanh (Θ_{t} e_{t 1}) - e_{t 1}

(9)

where

\begin{aligned} N_{t} = & (m - \bar{m}) {\overset{\dots}{p}}_{d} - {\dot{d}}_{t} - \bar{m} (K_{t p} {\dot{e}}_{p} + K_{t d} {\ddot{e}}_{p}) + \\ m (Λ_{t} {\ddot{e}}_{p} + {\dot{e}}_{t 1}) + e_{t 1} \\ N_{t d} = & (m - \bar{m}) {\overset{\dots}{p}}_{d} - {\dot{d}}_{t} . \end{aligned}

Define

{\tilde{N}}_{t} = N_{t} - N_{t d} = - \bar{m} (K_{t p} {\dot{e}}_{p} + K_{t d} {\ddot{e}}_{p}) + m (Λ_{t} {\ddot{e}}_{p} + {\dot{e}}_{t 1}) + e_{t 1}

, then there exist a constant μ_t > 0 satisfying

‖ {\tilde{N}}_{t} ‖ \leq μ_{t} ‖ e_{t} ‖ .

(10)

Now, we define a Lyapunov candidate function V_t as

V_{t} = \frac{1}{2} e_{p}^{⊤} e_{p} + \frac{1}{2} e_{t 1}^{⊤} e_{t 1} + \frac{1}{2} m e_{t 2}^{⊤} e_{t 2} + Q_{t} (t)

(11)

where

Q_{t} = \sum_{i = 1}^{n} \frac{Γ_{t, i}}{Θ_{t, i}} ln (cosh (Θ_{t, i} e_{t 1, i})) - e_{t 1, i} N_{t d, i} + \frac{Γ_{t, i}}{Θ_{t, i}} ln 2 .

For the ease of analysis, here we also derive the time-derivative of V_t as

\begin{gathered} {\dot{V}}_{t} = - e_{p}^{⊤} Λ_{t} e_{p} - ‖ e_{t 1} ‖^{2} - e_{t 2}^{⊤} (K_{t i} + ρ_{t} I_{3}) e_{t 2} + \\ e_{p}^{⊤} e_{t 1} + e_{t 2}^{⊤} {\tilde{N}}_{t} + O_{t} (t) \end{gathered}

where

O_{t} = e_{t 1}^{⊤} (N_{t d} - {\dot{N}}_{t d}) - e_{t 1}^{⊤} Γ_{t} Tanh (Θ_{t} e_{t 1})

Lemma 1

Take $Γ_{t, i} \geq ‖ N_{t d, i} ‖_{\infty} + ‖ {\dot{N}}_{t d, i} ‖_{\infty} \forall i$ . Then, Q_t satisfies the following inequalities:

\begin{aligned} \sum_{i = 1}^{n} (Γ_{t, i} - ‖ N_{t d, i} ‖_{\infty}) | e_{t 1, i} | \leq Q_{t} (t) \leq \\ \sum_{i = 1}^{n} (Γ_{t, i} + ‖ N_{t d, i} ‖_{\infty}) | e_{t 1, i} | + \frac{Γ_{t, i}}{Θ_{t, i}} ln 2 . \end{aligned}

Proof . Finite Γ_t,i exists by the Assumption 1, and the inequalities can be derived by using the fact that |x| ≤ ln(cosh(x)) + ln2 = ln(e^x + e^−x) and ln(cosh(x)) ≤ |x| for any $x \in R$ .

Lemma 2

Take $Γ_{t, i} \geq ‖ N_{t d, i} ‖_{\infty} + ‖ {\dot{N}}_{t d, i} ‖_{\infty} \forall i$ . Then, with c = 0.2785, the upper bound of O_t can be obtained as

O_{t} \leq \sum_{i = 1}^{n} Γ_{t, i} | e_{t 1, i} | (1 - tanh (Θ_{t, i} | e_{t 1, i} |)) \leq \sum_{i = 1}^{n} \frac{Γ_{t, i}}{Θ_{t, i}} c .

Proof . Finite Γ_t,i exists by Assumption 1, and the upper bound can be obtained by using the fact that $\max_{x \in R} {| x | - x tanh (x)} \leq c$ (Polycarpou and Ioannou, 1996; Jia et al., 2019) and the control gain condition on Γ_t,i.

Lemma 3

Assume that the following scalar differential equation holds for any sufficiently smooth $α (\cdot) \in K_{\infty}$ and constant scalar c_s:

\dot{s} \leq - α (s) + α (c_{s}) .

Then, there exist

β (\cdot, \cdot) \in K L

such that the following holds:

s (t) \leq β (s (0) - c_{s}, t) + c_{s} \forall t \geq 0 .

Proof . See the proof in Appendix 9.2.

Theorem 1

For control gains satisfying

\begin{aligned} Γ_{t, i} & \geq ‖ N_{t d, i} ‖_{\infty} + ‖ {\dot{N}}_{t d, i} ‖_{\infty} \forall i \\ λ_{m} (Λ_{t}) & \geq 0.5 \\ η_{t}^{*} & \geq μ_{t}^{2} / (2 λ_{m} (K_{t i})) \end{aligned}

where

η_{t}^{*} = \min {λ_{m} (Λ_{t}) - 1 / 2,1 / 2, ρ_{t}}

, the closed-loop system of the translational dynamics consisting of (5) and (9) is ultimately bounded, and the ultimate bound can be made arbitrarily small.

Proof . See the proof in Appendix 9.3.

Briefly speaking, to satisfy the control gain conditions, one should take large enough K _ti and Γ_t. Furthermore, the steady-state error bound can be arbitrarily shrunk by taking large enough Θ_t, and this corresponds to the fact that no steady-state error exists if the sign function is used instead of tanh (Xian et al., 2004). However, as will be discussed at the end of this section, using tanh instead of the sign function has advantages in attenuating input chattering.

Rotational dynamics

From (1b) and (8a)–(8c), dynamics of e _r2 can be obtained as

\begin{aligned} J_{b} {\dot{e}}_{r 2} & = - (K_{r i} + ρ_{r} I_{3}) e_{r 2} - Γ_{r} Tanh (Θ_{r} e_{r 1}) - \frac{1}{2} {\dot{J}}_{b} e_{r 2} \\ - e_{r 1} + {\tilde{N}}_{r} + N_{r d} \end{aligned}

(12)

where

\begin{aligned} N_{r d} & = (\dot{E} - \dot{\bar{E}}) - {\dot{d}}_{r}, \\ {\tilde{N}}_{r} & = - {\dot{\bar{J}}}_{b} (K_{r p} e_{R} + K_{r d} e_{ω}) - {\bar{J}}_{b} (K_{r p} {\dot{e}}_{R} + K_{r d} {\dot{e}}_{ω}) \\ + e_{r 1} + \frac{1}{2} {\dot{J}}_{b} e_{r 2} + (J_{b} - {\dot{J}}_{b}) {\dot{e}}_{r 1} + {\dot{J}}_{b} Λ_{r} C e_{ω} + \\ J_{b} Λ_{r} (\dot{C} e_{ω} + C {\dot{e}}_{ω}), \\ E & = J_{b} (R^{⊤} R_{d} {\dot{ω}}_{d} - ω^{\land} R^{⊤} R_{d} ω_{d}), \\ C & = \frac{1}{2} [tr (R^{⊤} R_{d}) I_{3} - R^{⊤} R_{d}] . \end{aligned}

Here, we use the fact that

{\dot{e}}_{R} = C e_{ω}

where ‖ C ‖₂ ≤ 1 for any

R^{⊤} R_{d} \in S O (3)

, and

\bar{E} = E |_{J_{b} = {\bar{J}}_{b}}

. From Remark 3 in Xian et al. (2004), there exists

μ_{r} (\cdot) \in K_{\infty}

such that

‖ {\tilde{N}}_{r} ‖ \leq μ_{r} (‖ e_{r} ‖) ‖ e_{r} ‖

holds.

Now, we define a Lyapunov candidate function V_r as

V_{r} = \frac{1}{2} e_{r 1}^{⊤} e_{r 1} + \frac{1}{2} e_{r 2}^{⊤} J_{b} e_{r 2} + Q_{r} + Ψ

(13)

where

\begin{aligned} Q_{r} & = \sum_{i = 1}^{n} \frac{Γ_{r, i}}{Θ_{r, i}} ln (cosh (Θ_{r, i} e_{r 1, i})) - e_{r 1, i} N_{r d, i} + \frac{Γ_{r, i}}{Θ_{r, i}} ln 2 \\ Ψ & = \frac{1}{2} tr [I_{3} - R^{⊤} R_{d}] . \end{aligned}

Note that to capture non-Euclidean property of

S O (3)

, Ψ is introduced in (13) instead of

e_{p}^{⊤} e_{p}

in (11).

Lemma 4

Assume that control gains satisfy the following:

\begin{aligned} λ_{m} (K_{r i}) & \geq \frac{1}{2 η_{r}^{*}} μ_{r}^{2} (\sqrt{\frac{1}{η_{r}} β_{r} (| V_{r} (0) - Ξ_{r} |, t) + \frac{Ξ_{r}}{η_{r}}}) \\ λ_{m} (Λ_{r}) & \geq 0.5 \\ Γ_{r, i} & \geq ‖ N_{r d, i} ‖_{\infty} + ‖ {\dot{N}}_{r d, i} ‖_{\infty} \forall i \end{aligned}

where

η_{r}^{*} = \min {λ_{m} (Λ_{r}) - 1 / 2,1 / 2, ρ_{r}}

. If Ψ(t) ≤ ψ holds for some 0 < ψ < 2 and t ∈ [t₁, t₂] for any 0 ≤ t₁ < t₂, then the following holds for all t ∈ [t₁, t₂]:

{\dot{V}}_{r} \leq - Ω_{r} (V_{r}) + Ω_{r} (Ξ_{r}) .

(14)

Here,

β_{r} (\cdot, \cdot) \in K L

, and Ξ_r = χ(σ_r) for some

χ (\cdot) \in K_{\infty}

and σ_r = ∑Γ_r,i/Θ_r,iln2 whose detailed definitions are in Appendix 9.4.

Proof . See the proof in Appendix 9.4.

Theorem 2

Assume that control gains Θ_r, Γ_r are selected so that Ξ_r < ψ holds for some ψ ∈ (0, 2). We also assume that control gain conditions in Lemma 2 hold, and the initial condition satisfies

V_{r} (0) \leq ψ - ϵ_{ψ}

(15)

for arbitrarily small positive scalar ϵ_ψ. Then, the closed-loop system of the rotational dynamics consisting of (1b) and (8) is ultimately bounded, and the ultimate bound can be made arbitrarily small.

Proof . See the proof in Appendix 9.5.

Although the sign function can provide additional merit of asymptotic stability (Gu et al., 2022; Xian et al., 2004) if used instead of the tanh function in the control law (6), (8), it may result in input chattering. Considering the omnidirectional aerial robot where some actuators (i.e., servomotors) show unknown, non-negligible time delay in tracking the input command, such input chattering can deteriorate the tracking performance. The tanh function in the proposed control law relieves this problem by providing sufficient smoothness in the control input while endowing the necessary disturbance attenuation property with moderately high control gains of Θ_t, Θ_r.

As done in Xian and Zhang (2016), gRITE control only with the robust control law f _r and τ _r can provide the same property of boundedness with arbitrary small bound in theory. However, since the robust control law does not exploit the structure of the system dynamics, feedforward terms such as gravity $\bar{m} g R^{⊤} b_{3}$ in (6b) and centrifugal force $ω^{\land} {\bar{J}}_{b} ω$ in (8b) cannot be utilized, and this may degrade initial transient performance. Furthermore, since proportional and derivative gains for position/orientation errors e _p, e _R cannot be tuned independently to the integral gains, the gain tuning process can become demanding. The nominal control law mitigates the two problems because it is designed to involve feedforward terms and has proportional, derivative gains independent to the integral gains.

Whole-body motion planning

This section overviews our whole-body trajectory generation algorithm designed for an OAM. We utilize both offline and online planning to enhance convergence to the predefined end-effector goal pose and to enable reactive real-time replanning with respect to potential disturbance. This allows the OAM to reach the predefined end-effector goal pose and refine the entire state trajectory in real time. In the initial stage, our algorithm focuses on the end-effector trajectory. The end-effector pose is modeled as a particle in a 3D Euclidean space with orientation. As a result, the globally optimal, collision-free end-effector pose trajectory can be obtained within a few seconds even under a setting where the problem is highly nonlinear due to the rotational motion, and the trajectory horizon is longer than 10 s. Then, our algorithm proceeds to local online whole-body motion planning. Here, the primary consideration is to consider the whole-body motion while tracking the previously determined optimal end-effector pose trajectory. Before presenting algorithmic details, we rewrite the Assumption 2 mathematically.

Only for notational simplicity, we describe an obstacle with a single ellipsoid from the following. That is,

\begin{gathered} O_{i} = E ({\bar{p}}_{i}, {\bar{Q}}_{i}) \subset R^{3} \\ E ({\bar{p}}_{i}, {\bar{Q}}_{i}) : = {p \in R^{3} | (p - {\bar{p}}_{i}) {}^{⊤}{\bar{Q}}_{i}^{- 1} (p - {\bar{p}}_{i}) \leq 1} \end{gathered}

where

{\bar{p}}_{i} \in R^{3}

and

{\bar{Q}}_{i} \in R_{> 0}^{3 \times 3}

denote the center and the shape matrix of the i^th ellipsoid, respectively. This assumption allows us to formulate collision avoidance constraints with obstacles using finite dimensional parameters of

{\bar{p}}_{i}

and

{\bar{Q}}_{i}

. When it is difficult to represent a particular obstacle as a single ellipsoid, we can construct a composite of multiple ellipsoids that fully encompass the obstacle.

Now, we formulate the following optimal control problems (OCPs) for our cascaded whole-body motion planner.

Offline end-effector trajectory generation

From Assumption 2, it becomes apparent that the end-effector trajectory avoids collisions with obstacles if and only if the following inequalities are satisfied:

\begin{matrix} h_{i} ({}^{E}p_{d} (k)) > 0 \forall i, k \\ h_{i} ({}^{E}p_{d}) ≔ ({}^{E}p_{d} - {\bar{p}}_{i}) {}^{⊤}{\bar{Q}}_{i}^{- 1} ({}^{E}p_{d} - {\bar{p}}_{i}) - 1 \end{matrix}

where k denotes the discretized time index throughout this section. However, directly incorporating these constraints into discrete-time OCPs may lead to a numerical issue of generating an optimal trajectory that penetrates very thin ellipsoids within a single time interval. In such cases, the system may collide with ellipsoids although the optimized trajectory is feasible. Therefore, instead, we formulate the following collision avoidance constraints, which impose constraints on the linear velocity.

\begin{matrix} {\tilde{h}}_{i} ({}^{E}p_{d} (k), {}^{E}v_{d} (k)) > 0 \forall i, k \\ {\tilde{h}}_{i} ({}^{E}p_{d}, {}^{E}v_{d}) ≔ \frac{\partial h_{i}}{\partial {}^{E}p_{d}} ({}^{E}p_{d}) {}^{E}v_{d} + γ_{i} (h_{i} ({}^{E}p_{d})) \end{matrix}

(16)

where

γ_{i} (\cdot) \in K_{\infty}

can be chosen arbitrarily. We designed γ_i as linear functions for simplicity. Provided that the inequalities (16) and

h_{i} ({}^{E}p_{d} (0)) > 0

hold, the forward invariance of h_i > 0 is guaranteed by the comparison lemma (Khalil and Grizzle, 2002, Lemma 3.4). A similar technique is utilized in the framework of control barrier function (Ames et al., 2016, 2019), where the decay rate of a control barrier function is bounded so that a safe and smooth motion is obtained.

The modified collision avoidance constraints (16) rely only on the optimization variables in the translational motion. Thus, we can construct the following decoupled jerk minimization problem.

\begin{aligned} \min_{{}^{E}x_{p}, {}^{E}{\ddot{v}}_{d}} & \sum_{k = 0}^{N_{T} - 1} ‖^{E} {\ddot{v}}_{d} (k) ‖_{R_{v}}^{2} \\ s . t . & {}^{E}x_{p} (0) = [{}^{E}p (0); 0; 0] \\ {}^{E}x_{p} (N_{T}) = [{}^{E}p_{g}; 0; 0] \\ \forall_{k} & {}^{E}x_{p} (k + 1) = {}^{E}f_{p} ({}^{E}x_{p} (k), {}^{E}{\ddot{v}}_{d} (k)) \\ \forall_{i, k} & {\tilde{h}}_{i} ({}^{E}p_{d} (k), {}^{E}v_{d} (k)) > 0 \end{aligned}

(17)

\begin{aligned} \min_{{}^{E}x_{R}, {}^{E}{\ddot{ω}}_{d}} & \sum_{k = 0}^{N_{T} - 1} ‖^{E} {\ddot{ω}}_{d} (k) ‖_{R_{ω}}^{2} \\ s . t . & {}^{E}x_{R} (0) = ({}^{E}R (0), 0, 0) \\ {}^{E}x_{R} (N_{T}) = ({}^{E}R_{g}, 0, 0) \\ \forall_{k} & {}^{E}x_{R} (k + 1) = {}^{E}f_{R} ({}^{E}x_{R} (k),^{E} {\ddot{ω}}_{d} (k)) \end{aligned}

(18)

Here,

{}^{E}x_{p} ≔ [{}^{E}p_{d}; {}^{E}v_{d}; {}^{E}{\dot{v}}_{d}] \in R^{9}

{}^{E}x_{R} ≔ ({}^{E}R_{d}, {}^{E}ω_{d}, {}^{E}{\dot{ω}}_{d}) \in S O (3) \times R^{6}

, and N_T = T_f /Δt.

{}^{E}f_{p}

and

{}^{E}f_{R}

are time-discretized linear and nonlinear kinematics of the translational and rotational motion. The detailed definitions of

{}^{E}f_{p}

and

{}^{E}f_{R}

can be found in Appendix 9.6. R _v and R _ω are positive definite weight matrices.

{}^{E}{p (0)}

and

{}^{E}{R (0)}

are obtained from the forward kinematics with the measured values of p (0), R (0), and θ (0) when the planner algorithm initiates. It is worth noting that, if the objective is only related to the goal position, (18) can be omitted thanks to the fact that (17) and (18) are decoupled.

Online whole-body motion planning

Unlike the offline end-effector trajectory generation, whole-body motion planning necessitates the consideration of collision avoidance among rigid bodies, identification of the optimal configuration among those achieving the same end-effector pose, and online replanning to react to uncertainties and refine the end-effector trajectory if unavoidable. This subsection provides details of how each of these considerations is incorporated into the OCP.

Collision avoidance constraint

Let us define the i^th ellipsoid $A_{i} (t)$ comprising the aerial manipulator at time t as

\begin{gathered} A_{i} (t) ≔ E ({}^{A}{\bar{p}}_{i} (t), {}^{A}{\bar{Q}}_{i} (t)) \\ {}^{A}{\bar{Q}}_{i} (t) ≔ {}^{A}R_{i} (t) {}^{A}{\bar{Q}}_{i} (0) {}^{A}R_{i} {(t)}^{⊤} \end{gathered}

In the above,

{}^{A}{\bar{p}}_{i}

and

{}^{A}R_{i}

represents the position and orientation of the i^th link obtained through the forward kinematics, where the multirotor base corresponds to i = 0.

{}^{A}{\bar{Q}}_{i} (0)

is the shape matrix calculated at

{}^{A}R_{i} = I_{3}

When obstacles and robot components are all modeled with ellipsoids, collision avoidance can be ensured if there is no intersection between each pair of robot and obstacle ellipsoids. Let us consider a set generated by the Minkowski sum of two sets of ellipsoids as $\bar{B} = B_{1} \oplus (- B_{2})$ . The condition for the absence of intersection between the two ellipsoids can be expressed as $0 \notin \bar{B}$ . However, in general, $\bar{B}$ is not an ellipsoid. To resolve this, similar to the work done in Son et al. (2020) where the most compact ellipsoid is used that encompasses $\bar{B}$ through trace minimization, we can guarantee the absence of intersection between the given two ellipsoids if the following condition is satisfied (Seo et al., 2019).

\begin{gathered} \hat{h} (B_{1}, B_{2}) > 0 \\ \hat{h} (B_{1}, B_{2}) ≔ {({\bar{p}}_{B_{1}} - {\bar{p}}_{B_{2}})}^{⊤} {\bar{Q}}^{- 1} (B_{1}, B_{2}) ({\bar{p}}_{B_{1}} - {\bar{p}}_{B_{2}}) - 1 \\ \bar{Q} (B_{1}, B_{2}) ≔ \sum_{i = 1}^{2} \sum_{j = 1}^{2} \frac{{\bar{Q}}_{B_{i}}}{\sqrt{tr ({\bar{Q}}_{B_{i}})}} \sqrt{tr ({\bar{Q}}_{B_{j}})} \end{gathered}

(19)

Hence, the collision avoidance is guaranteed if (19) holds for all pairs of ellipsoids of robots and obstacles for all t ≥ 0.

Cost function

In comparison with ground robot-based mobile manipulators, aerial manipulators are more susceptible to disturbance caused by interaction forces between the end-effector and the target object during manipulation. To mitigate this, it is imperative to secure the manipulability of the robot arm. Consequently, we incorporate the following manipulability index into our cost function:

ϕ_{m} (θ) ≔ μ_{v} \det (J_{v} (θ) J_{v}^{⊤} (θ)) + μ_{ω} \det (J_{ω} (θ) J_{ω}^{⊤} (θ))

(20)

Here, J _v and J _ω are Jacobian matrices of relative position and orientation of the end-effector with respect to the multirotor base. μ_v and μ_ω are positive weights. A higher value of ϕ_m indicates greater manipulability, signifying that the robot arm configuration is positioned farther away from singularity (Lynch and Park, 2017). Finally, denoting

x_{d} ≔ (p_{d}, R_{d}, θ_{d}) \in R^{3} \times S O (3) \times R^{n}

{}^{E}x_{d} ≔ ({}^{E}p_{d}, {}^{E}R_{d}) \in R^{3} \times S O (3)

and

u_{d} ≔ [v_{d}; ω_{d}; {\dot{θ}}_{d}] \in R^{6 + n}

, we design the following state cost ϕ_x and input cost ϕ_u:

\begin{aligned} ϕ_{x} (x_{d}, {}^{E}x_{d}) ≔ & ‖ {}^{E}p (x_{d}) - {}^{E}p_{d} ‖_{Q_{p}}^{2} - ϕ_{m} (θ_{d}) \\ + tr (Q_{R} (I_{3} - {}^{E}R_{d^{⊤}} {}^{E}R (x_{d}))) \\ ϕ_{u} (u_{d}) ≔ & ‖ u_{d} ‖_{R_{u}}^{2}, \end{aligned}

(21)

where Q _p, Q _R, and R _u are all positive definite weight matrices.

{}^{E}p (x_{d})

and

{}^{E}R (x_{d})

are computed by forward kinematics using x _d.

Kinematics-level nonlinear model predictive control

With the obstacle avoidance constraints (19) and the cost functions (21), we formulate and solve the following OCP at each replanning interval, employing a nonlinear model predictive control (NMPC) approach:

\begin{aligned} \min_{x_{d}, u_{d}} & ϕ_{x} (x_{d} (N_{H}), {}^{E}x_{d} (N_{H})) \\ + \sum_{k = 0}^{N_{H} - 1} ϕ_{x} (x_{d} (k), {}^{E}x_{d} (k)) + ϕ_{u} (u_{d} (k)) \\ s . t . & x_{d} (0) = (p, R, θ) \\ \forall_{k} & x_{d} (k + 1) = f_{x} (x_{d} (k), u_{d} (k)) \\ \forall_{k} & u_{m} ⪯ u_{d} (k) ⪯ u_{M} \\ \forall_{k} & A_{θ} θ_{d} (k) ⪯ b_{θ} \\ \forall_{i, j, k} & \hat{h} (A_{i} (k), O_{j}) > 0 \end{aligned}

(22)

Here, p , R and θ are the measured values at every replanning time. f _x represents discretized kinematic relations for the position, rotation matrix, and joint angle. See Appendix 9.7 for the detail. The upper and lower bounds for input u _d are denoted as u _M and u _m. The fourth inequality constraint, that is, A _θ θ _d(k)⪯ b _θ, prevents self-collision between the manipulator and the multirotor base, where ⪯ denotes element-wise inequality. The horizon length N_H is T_H/Δt. After solving the optimization problem (22) and refining the whole-body trajectory, we utilize p _d, v _d, R _d, and ω _d as desired values for the controller in (6a)–(6c) and (8a)–(8c). Simultaneously, θ _d and

{\dot{θ}}_{d}

serve as desired values for the external robot arm controller.

In scenarios where the manipulator is constrained to planar motion, the manipulability index ϕ_m in (20) becomes 0 as J _v and J _ω are not full rank. In such instances, ϕ_m can be computed utilizing the Jacobian matrices projected onto the plane of motion. Moreover, if the manipulator comprises solely revolute joints rotating in the same direction, the term associated with J _ω remains constant and can be excluded from the cost function. For the case when the goal end-effector orientation is deemed inconsequential and omitted to be solved in (18), excluding the term $tr (Q_{R} (I_{3} - {}^{E}R_{d^{⊤}} {}^{E}R (x_{d})))$ in (21) suffices. This is because the remaining state cost also contains all of the state variable x _d.

Experimental results

Setup

We construct a customized OAM whose CAD model can be found in Figure 3. Table 3 lists main components of the platform whose net weight is about 2.13 kg. The manipulator is composed of four revolute joints where the first three joints rotate in the same direction while the last joint is for a gripper. Algorithms including the proposed controller, planner, and state estimator run in an onboard computer where we use robot operating system (ROS) in Ubuntu 20.04. The current state of the robot is estimated by a state estimation algorithm (Kim, 2023) which executes sensor fusion for measurements from Optitrack motion capture system and inertial measurement unit (IMU). The overall algorithm for hardware experiments is visualized in Figure 2.

Table 3.

Main components of the system.

Component	Product name	Quantity
Onboard computer	Intel NUC i3	1
Rotor	Armattan Oomph TITAN 2306/2450 KV	6
Propeller	APC BD6x4.2E-3-B4	6
Servo (base)	Dynamixel XC330 series	6
Servo (manipulator)	Dynamixel XM430 & XC330 series	4
ESC	Hobbywing XRotor Micro 40A 4in1	2
PWM generator	Nucleo F446RE	1
IMU sensor	Vectornav VN-100	1
Battery	Turnigy 4200 mAh 4S LiPo	1

Three experiments are conducted. In the first experiment, we show the performance of the proposed controller by comparing the result that is obtained with baseline controllers. The objective is to regulate the pose of the multirotor base in the presence of the robotic arm’s motion, and better regulation performance can be observed with the proposed control law. The second and third experiments are to demonstrate the proposed framework in one precise manipulation task of grasping-and-pulling an object. We consider two different environment settings: grasping-and-pulling an object (1) on the ground and (2) on a table. Compared to a conventional aerial manipulator based on an underactuated multirotor base inheriting a limited workspace, the OAM equipped with the proposed framework could accomplish the task in both environments by leveraging omnidirectionality and the extended workspace.

Implementation details

Control gains we used during the whole experiments are listed in Table 4. In the gain tuning process, we first tune PID gains K _tp, K _td, K _ti, K _rp, K _rd, K _ri while setting all the other gains to be zero. After finding proper PID gains, we freeze the PID gains and tune the other gains related to the integral of the tanh of the error, which are Λ_t, Γ_t, Θ_t, ρ_t, Λ_r, Γ_r, Θ_r, ρ_r. We first set ρ_t, Γ_t and ρ_r, Γ_r to be the same scale as K _ti and K _ri, respectively, and adjust them afterward. Similarly, we initially take Λ_t, Λ_r to be slightly larger than 1 as they indicate the decay rate and tune them afterward. Lastly, we initially set Θ_t, Θ_r to be 1 and increase them until achieving sufficient performance in disturbance rejection since larger Θ_t, Θ_r are shown to be effective in reducing the error bound.

Table 4.

Controller and control allocation parameters (diagonal elements for matrices).

Controller
Parameter	Value	Parameter	Value
$\bar{m}$ (kg)	2.13	${\bar{J}}_{b}$ (kgm²)	(0.02,0.025,0.035)
K _tp	(8,8,8)	K _rp	(15,20,10)
K _td	(5,5,5)	K _rd	(10,9,5)
K _ti	(2,2,4)	K _ri	(0.08,0.08,0.08)
Λ _t	(3,2,2)	Λ _r	(8,8,8)
Γ _t	(2,2,2)	Γ _r	(0.2,0.2,0.2)
Θ	(3,3,3)	Θ _r	(10,10,10)
ρ _t	1	ρ _r	0.02
Allocation
Parameter		Value
W		(1,1,0.6,0.6,1,1,1,1,0.6,0.6,1,1)

We empirically find that the 2^nd and 5^th rotors depicted in Figure 3 go near saturation when the robot hovers at 90° pitch angle with the identity weight matrix W in control allocation. This is because when hovering at 90° pitch angle, (1) the pseudo-inverse solution finds the minimum norm solution and (2) other rotors are less effective in compensating gravity as they are tilted with respect to the gravity direction. Considering the robotic arm configuration in Figure 3, manipulation while maintaining non-zero or even 90° pitch angle can frequently occur during experiments. To relieve this issue, we allocate values smaller than 1 to the elements related to the 2^nd and 5^th rotors in the weight matrix W in (3). The value we use during all experiments can be found in Table 4.

Our experiments involve grasping a target object. Thus, a certain criterion is needed to determine whether the object is firmly grasped or not. For the criterion, we choose the current of the gripper servomotor. Before the experiments, the threshold for successful grasping is determined by securing various test objects with the gripper and observing the required value for it. In actual experiments, a low-pass filter is applied to the gripper servomotor’s current. If the filtered value surpasses the pre-determined threshold after 1.5 s, the grasping is considered successful, and a new trajectory is planned for the subsequent mission, which is pulling. The command to grasp is given when the position error between the end-effector and the goal is less than 3 cm. If the criterion is not met after the command, the gripper is reopened. This process is repeated until successful grasping occurs.

The collision avoidance with the ground is efficiently addressed by representing both the base and individual links of the OAM as spheres. This pragmatic simplification allows us to express the constraint in the form of r_i ≤ z_i(t) where r_i is the radius and z_i is the height of the i^th component of the OAM. For OCPs described in (17), (18), and (22), we employed the open-source tool CasADi (Andersson et al., 2019). The chosen nonlinear optimization solver was IPOPT with HSL libraries (HSL, 2007) serving as a linear solver. With this setup, the entire offline OCPs in (17) and (18) are solved within 3–5 s. The parameters used for our whole-body motion planner algorithm are shown in Table 5. Our OAM’s revolute joints related to the motion of the manipulator rotate along the same direction, meaning that J _ω becomes constant. Thus, in the table, the value of μ_ω is not listed.

Table 5.

Planner parameters and numerical integration method (block diagonal matrices for R _u).

Offline
Parameter	Value	Parameter	Value
R _v, R _ω	I ₃	γ _i	3
T_f (sec)	15	Δt (sec)	0.1
Online
Parameter	Value	Parameter	Value
μ _v	0.01	μ _ω	−
Q _p	5 I ₃	R _u	(0.01 I ₃, 0.01 I ₃, 0.1 I ₃)
Q _R	4 I ₃	u _M, − u _m	[1₃; 0.5π1₃; 0.25π1₃]
T_H (sec)	1.5	Δt (sec)	0.1
Numerical integration
Parameter	Method	Parameter	Method
${}^{E}f_{p}, {}^{E}f_{R}$	RK4	f _x	Euler
1₃≔[1; 1; 1]

Experiment 1: Controller comparison

The first experiment is to show effectiveness of the proposed controller under disturbance, in comparison with the baseline controllers: (1) Euler-angle-based disturbance-observer (DOB) (Lee et al., 2021) which is a nonlinear robust controller, (2) geometric PID controller (gPID) (Goodarzi et al., 2013; Su et al., 2023), (3) geometric $L_{1}$ adaptive controller (g $L_{1}$ ) (Wu et al., 2025), and (4) geometric RISE controller (gRISE) (Gu et al., 2022). Unlike prior works on OAM that typically employ only gPID, our evaluation includes a broader range of robust and adaptive controllers, such as those tailored for multirotor platforms.

The baseline controllers share the same PD control structure, with an additional control term designed to reject disturbance. For fair comparison, we set the corresponding PD gains of the proposed controller to be identical to those used in the baseline controllers. Only the gains associated with the additional control terms were tuned. Although g $L_{1}$ , DOB, and gRISE theoretically achieve better performance with higher gains, in real-world applications, measurement noise and physical limitations of actuators prevented arbitrarily increasing the gains. Accordingly, for each controller, the gains were tuned until a sufficiently low tracking error was achieved.

The experiment is carried out in two different settings: pose regulation at 0° pitch angle and −30° pitch angle. We intentionally oscillate the manipulator to provide external disturbance to the multirotor base. In all settings, only the first and second joints of the manipulator are commanded to move from −45° to 45° with a period of 10 s. As a representative example that most clearly shows the difference between controllers in the two settings, we first present comparison results between the gPID and the proposed controller in Figures 4(a) and (b). In both figures, the above composite images show the motions of the manipulator and the multirotor base, exhibiting the superior performance of the proposed controller. The below graphs in both figures display pose error e _p, e _R and body force and torque f , τ obtained during the experiments.

Figure 4.

Results of Experiment 1: Comparison between gPID and the proposed controller (gRITE).

In both experiments, while the maximum error in the translational motion does not exceed 0.02 m in the results obtained with the proposed controller that in the results obtained without the proposed controller nearly reaches 0.04 m. The performance gap is more significant in the rotation direction which can be found in the second row of the graphs in Figures 4(a) and (b). We presume that not the control law itself but the mechanical vibration of the manipulator and the resultant measurement noise in angular velocity are responsible for the jittering in τ₂ during all experiments with or without the proposed control law. This is because such jittering does not occur in any other control inputs, and we have experienced mechanical vibration due to backlash and clearance of servomotors which may be improved by fabricating the OAM using high-end servomotors.

The comparison results, including all baseline controllers, are summarized in Figure 5 and Table 6. Due to the motion of the robotic arm, the external disturbance is most significant along the body y − axis. Accordingly, the disturbance attenuation capability of each controller is most clearly reflected in the bottom-left plots of Figures 5(a) and (b). The first, second, and third quartiles (Q₁, Q₂, Q₃) of the gPID controller are more than twice as large as those of the other controllers. Thus, we can confirm that all other controllers are substantially robust to disturbance. For DOB, which is designed based on Euler angles, both position and orientation errors noticeably increase when the pitch angle is −30° compared to the 0° case. This implies the importance of fully accounting for the nonlinear structure of $S O (3)$ for accurate attitude control. In the case of gRISE, the presence of the sign function leads to discontinuities in the first derivative of the control input. As a result, its performance in directions that are relatively less influenced by external disturbances, namely position and the body x − and z − axes, is inferior to that of the other controllers, including gPID. The $g L_{1}$ controller employs a low-pass filter (LPF) to smooth out the rapidly varying control signal generated by its adaptation law. This suppresses oscillatory inputs and improves tracking accuracy compared to other baseline controllers in practice. As seen in the plots, it maintains relatively low position and orientation errors across most directions. This is best illustrated when the pitch angle is 0°, where it achieves the lowest RMS and mean position tracking errors among all controllers, as summarized in Table 6. While the LPF improves performance in the 0° pitch case, its trade-off with performance leads to reduced accuracy at −30°, where the gRITE controller outperforms it. The proposed gRITE controller not only produces smooth control inputs via the tanh function, but also exhibits strong robustness against disturbances. The proposed controller consistently outperforms the baselines in almost all real-world experiments.

Figure 5.

Box plot of Experiment 1 for comparison with baseline controllers.

Table 6.

Performance metrics of Experiment 1.

		Setting 1 (0° pitch angle)					Setting 2 (−30° pitch angle)
		gPID	gRISE	DOB	g $L_{1}$	Proposed	gPID	gRISE	DOB	g $L_{1}$	Proposed
Position	RMS (cm)	1.31	1.04	1.59	0.451	0.488	1.44	0.786	1.76	0.723	0.518
Tracking	Mean (cm)	1.19	0.934	1.43	0.418	0.462	1.29	0.731	1.61	0.657	0.471
Error^a	Std (cm)	0.537	0.442	0.702	0.171	0.156	0.623	0.289	0.712	0.303	0.216
Orientation	RMS (deg)	14.5	6.27	5.44	5.69	5.49	12.5	6.23	6.54	5.79	4.53
Tracking	Mean (deg)	11.6	5.29	4.50	4.78	3.89	10.2	5.11	5.82	4.59	3.48
Error^b	Std (deg)	8.64	3.36	3.06	3.08	3.87	7.13	3.56	2.99	3.54	2.91

Bold numbers indicate the best performance (i.e., lowest error) among all controllers for each metric.

^aEuclidean norm of e_p

^bGeodesic distance d_g between the two rotation matrices R and R_d, that is, $d_{g} = \cos^{- 1} (trace (R^{⊤} R_{d}) - 1 / 2)$ .

Experiment 2: Grasping-and-pulling an object on the ground

The second experiment is grasping-and-pulling an object on the ground which can hardly be conducted with a conventional aerial manipulator whose manipulator is attached on the top as the OAM in Figure 1. We carry out three different scenarios which we call (a) ground-basic, (b) ground-yaw, and (c) ground-pitch. All scenarios and the data collected during the experiments are visualized in Figure 6. Compared to (a) ground-basic, the initial orientation of the OAM in (b) ground-yaw is 180° rotated in the yaw direction. (c) ground-pitch starts with the same orientation as (a) ground-basic, but the target end-effector orientation is 180° rotated in the pitch direction.

Figure 6.

Results of Experiment 2: Grasping-and-pulling an object on the ground. The notations (a), (b), and (c) indicate to which scenario the figure corresponds to: (a) ground-basic, (b) ground-yaw, and (c) ground-pitch. We use quaternion q = [q_w, q_x, q_y, q_z] to represent the orientation of the multirotor base.

We could confirm that the proposed whole-body motion planning algorithm can compute a collision-free and goal-reaching trajectory in all three scenarios having either different initial conditions or different target poses. For the ground-pitch scenario, we visualize the end-effector trajectory computed by the first-step offline planner (red), the whole-body trajectory generated by the second-step online planner (green) and the actual traversed end-effector trajectory (black) in Figure 7. Each ellipsoid represents each single rigid body comprising the OAM, used in deriving the collision avoidance constraints. We could also validate online replannability of the second-step NMPC-based planning algorithm in Table 7. The maximum computation time is less than 20 ms in all scenarios, indicating online replannability faster than 50 Hz.

Figure 7.

Visualization of trajectories computed by the whole-body motion planner in Experiment 2 setting (c) ground-pitch. The blue line indicates the motion of the multirotor base, and the red and green lines are end-effector trajectories computed by the offline and online planners. The black line is the actual trajectory traversed by the end-effector.

Table 7.

Performance metrics of Experiments 2 and 3.

		Experiment 2			Experiment 3
		Ground-basic	Ground-yaw	Ground-pitch	Table-far	Table-close
Position	RMS (cm)	1.84	1.69	1.29	0.955	0.960
Tracking	Mean (cm)	1.34	1.23	0.960	0.814	0.833
Error^a	Std (cm)	1.26	1.17	0.858	0.500	0.477
Orientation	RMS (deg)	2.64	2.74	1.94	1.87	1.46
Tracking	Mean (deg)	1.82	1.92	1.18	1.31	1.23
Error^b	Std (deg)	1.91	1.96	1.54	1.34	0.788
NMPC	Min (ms)	5.23	4.80	6.18	14.2	13.8
Computation	Max (ms)	12.0	12.2	15.9	69.4	50.5
Time	Mean (ms)	8.50	8.42	10.7	27.8	29.2

^aEuclidean norm of e_p

^bGeodesic distance d_g between the two rotation matrices R and R_d, that is, $d_{g} = \cos^{- 1} (trace (R^{⊤} R_{d}) - 1 / 2)$

The composite images at the top of Figure 6 show the motion of the OAM during experiments. Position p and orientation q of the multirotor base and joint angles of the manipulator θ are visualized in the below graphs. As the multirotor rotates more than 90° pitch angle, we use quaternion q = [q_w, q_x, q_y, q_z] in representing the orientation. Thanks to the omnidirectionality of the OAM platform and the capability of the proposed framework to address such merit in both planning and control, the mobile manipulator successfully performs precise manipulation while enjoying the extended workspace. As described in the composite images of Figure 6, the three experiments demonstrate precise manipulation of grasping-and-pulling while maintaining the pitch angle over 90° and even near 180°.

Quantitative results are summarized in Table 7, and a detailed analysis of state errors and computation time is illustrated in the box plots in Figure 12. All scenarios in experiment 2 exhibit position tracking errors in terms of both RMS and mean below 2 cm, and orientation tracking errors remain within 3°. These errors are computed using the Euclidean norm of e_p and the geodesic distance between the desired and actual rotation matrices. Figure 12 further provides component-wise distributions of the position and orientation errors. For each element of e_p and e_R, the box plots capture the median and quartiles over time. This offers insight into axis-wise behavior of the tracking performance. For instance, the position error in z-direction $e_{p_{3}}$ tends to show slightly larger deviation across different scenarios, which can be attributed to the object not being instantly detached during the initial pulling motion.

To compare the overall behaviors with and without the collision avoidance constraint, simulation results for the ground-basic scenario are shown in Figure 8. When the collision constraints (19) are not considered, the planned trajectory penetrates the ground, as shown in (b), and the constraint value drops significantly below zero, as seen in (c). In contrast, as illustrated in (a), when the constraints are imposed, the planner avoids collision by tilting the OAM’s base toward the target, effectively utilizing its omnidirectional flight capability. In this case, we can guarantee collision avoidance, as the minimum value among the collision avoidance constraints remains above zero for all time, as shown in (c). These results suggest that under the collision avoidance constraint, the planner can utilize the OAM’s omnidirectional flight capability to avoid collision and promote effective use of the extended workspace when reaching the target.

Figure 8.

Simulated behaviors in the ground-basic scenario under the same initial hovering condition, with (a) and without (b) the collision avoidance constraint in (19). The minimum constraint at time $t {\hat{h}}_{i, j} (t) = \hat{h} (A_{i} (t), O_{j})$ is plotted in (c), while the translucent blue surface in (a) and (b) represents the ground.

Experiment 3: Grasping-and-pulling an object on a table

The last experiment is to validate applicability of the proposed framework in a different setting of grasping-and-pulling an object on top of a table. The OAM should now additionally avoid collision with the table while accomplishing the task. Two different scenarios are considered: (a) table-far and (b) table-close. The results are summarized in Figure 9. The proposed whole-body motion planner computes a trajectory so that the OAM simultaneously tilts the pitch angle and stretches the manipulator to satisfy both the collision avoidance constraint and the goal-reaching objective. It is noticeable that when the object on the table is far from the robot (i.e., scenario (a)), to ensure safety, the proposed whole-body motion planner computes a trajectory not only to tilt the pitch angle and stretch the manipulator but also to stay above the table. Although a non-negligible ground effect from the table exists while the OAM being above the table in scenario (a), sufficient tracking performance to accomplish the precise manipulation task can be achieved with the proposed controller.

Figure 9.

Results of Experiment 3: Grasping-and-pulling an object on a table. The notations (a) and (b) indicate to which scenario the figure corresponds to: (a) table-far and (b) table-close. We use quaternion q = [q_w, q_x, q_y, q_z] to represent the orientation of the multirotor base.

The computation time taken for solving the second step NMPC is listed in Table 7. As an additional object to avoid exists compared to the Experiment 2, a little longer computation time is required, but still about 30 Hz on average is obtained, which is sufficient for online replanning. Trajectories computed from the offline and online planning algorithms and the actual traversed trajectory are visualized in Figure 10 for scenario (a).

Figure 10.

Visualization of trajectories computed by the whole-body motion planner in Experiment 3 setting (a) table-far. The blue line indicates the motion of the multirotor base, and the red and green lines are end-effector trajectories computed by the offline and online planner. The black line is the actual trajectory traversed by the end-effector.

Figure 11.

Simulated behaviors in the table-far scenario under the same initial hovering condition, with (a) and without (b) the collision avoidance constraint in (19). The minimum constraint at time $t {\hat{h}}_{i, j} (t) = \hat{h} (A_{i} (t), O_{j})$ is plotted in (c), while the green ellipsoid and the gray surface in (a) and (b) represent the table and the ground, respectively.

Figure 12.

Box plot for state errors (e_p and e_R) and NMPC computation time of Experiments 2 and 3.

Conclusion

In this work, we presented a control and planning framework to enable mobile manipulation with arbitrary base position and orientation. To achieve this objective, we first constructed an omnidirectional aerial manipulator composed of an omnidirectional multirotor and a multi-DoF robotic arm. Then, a geometric robust controller is proposed for the multirotor base which we call a geometric robust integral of the tanh of the error (gRITE) controller. The controller is designed to ensure sufficient performance in the presence of external disturbance including aerodynamic effect, interaction wrench, and the uncertain motion of the robotic arm. The stability showed that the proposed controller can guarantee arbitrarily small error bound by choosing sufficiently large control gains. Next, a two-step trajectory-optimization-based whole-body motion planning algorithm was proposed while taking omnidirectionality of the OAM and physical constraints including collision avoidance into account. We composed the two offline and online planning phases to formulate a numerically stable optimization problem.

The proposed control and whole-body motion planning framework is validated in hardware experiments. In the first experiment, we compared the proposed controller with a nonlinear PID controller. The proposed controller outperformed the counterpart by showing better regulation performance in the presence of external disturbance by the manipulator. The second and third experiments demonstrated effectiveness of the proposed framework in precise manipulation where the OAM conducted grasping-and-pulling of an object on (1) the ground and (2) a table. We accomplished precise manipulation even in the presence of external disturbance involving ground effect while whole-body motion was exploited in realizing abidance to physical constraints and task completion. During the experiments of precise manipulation, the OAM maintained stable flight around the pitch angle over 90° and even 180°, showing mobile manipulation in arbitrary base pose.

As a future work, we aim to explore collaborative transportation where multiple OAMs transport a common object while each agent exploiting the enlarged workspace. Compared to aerial manipulators based on conventional underactuated multirotors, the additional advantage of the OAMs’ enlarged workspace would enhance manipulability of the transporting object, particularly in confined environments. In structured environments, such as warehouses and automated manufacturing facilities, obstacle locations are typically known in advance. However, we also consider future research directions involving dynamic and unknown obstacles, which are common in settings like disaster response or human–robot coexistence. Our framework may be combined with high-confidence motion prediction for moving agents to address dynamic obstacles. Furthermore, for unknown environments with static obstacles, the detection of a potential collision using onboard sensors such as cameras can serve as a trigger to re-initiate the planner. This allows the robot to adaptively update its motion plan while preserving whole-body manipulation capabilities and ensuring collision avoidance. In addition to these, when manipulating heavy objects, it can be beneficial to consider the effects of gravity and the feasible wrench space. Thus, we also consider real-time adaptation of the OAM’s configuration to simultaneously minimize the adverse effects caused by the payload’s weight at the end-effector.

Supplemental Material

Footnotes

ORCID iDs

Dongjae Lee

Byeongjun Kim

Hyoun Jin Kim

Funding

The authors disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was supported by the National Research Foundation of Korea (NRF) grant funded by the Korea government (MSIT) (grant number RS- 2024-0436984). Byeongjun Kim was supported by the Hyundai MotorChung Mong-Koo Foundation.

Declaration of conflicting interests

The authors declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Supplemental Material

Supplemental material for this article is available online.

Note

Appendix

References

Allenspach

Bodie

Brunner

, et al. (2020) Design and optimal control of a tiltrotor micro-aerial vehicle for efficient omnidirectional flight. The International Journal of Robotics Research 39(10–11): 1305–1325.

Ames

Grizzle

, et al. (2016) Control barrier function based quadratic programs for safety critical systems. IEEE Transactions on Automatic Control 62(8): 3861–3876.

Ames

Coogan

Egerstedt

, et al. (2019) Control barrier functions: theory and applications. In: 2019 18th European Control Conference (ECC), Naples, Italy, 25–28 June 2019, pp. 3420–3431. IEEE.

Andersson

Gillis

Horn

, et al. (2019) Casadi: a software framework for nonlinear optimization and optimal control. Mathematical Programming Computation 11: 1–36.

Arcari

Minniti

Scampicchio

, et al. (2023) Bayesian multi-task learning mpc for robotic mobile manipulation. IEEE Robotics and Automation Letters 8: 3222–3229.

Bisheban

Lee

(2021) Geometric adaptive control with neural networks for a quadrotor in wind fields. IEEE Transactions on Control Systems Technology 29(4): 1533–1548.

Bodie

Brunner

Pantic

, et al. (2021a) Active interaction force control for contact-based inspection with a fully actuated aerial vehicle. IEEE Transactions on Robotics 37(3): 709–722.

Bodie

Tognon

Siegwart

(2021b) Dynamic end effector tracking with an omnidirectional parallel aerial manipulator. IEEE Robotics and Automation Letters 6(4): 8165–8172.

Brescianini

D’Andrea

(2018) Computationally efficient trajectory generation for fully actuated multirotor vehicles. IEEE Transactions on Robotics 34(3): 555–571.

10.

Brunner

Rizzi

Studiger

, et al. (2022) A planning-and-control framework for aerial manipulation of articulated objects. IEEE Robotics and Automation Letters 7(4): 10689–10696.

11.

Burget

Hornung

Bennewitz

(2013) Whole-body motion planning for manipulation of articulated objects. In: 2013 IEEE International Conference on Robotics and Automation, Karlsruhe, Germany, 06–10 May 2013, pp. 1656–1662. IEEE.

12.

Chiu

Sleiman

Mittal

, et al. (2022) A collision-free mpc for whole-body dynamic locomotion and manipulation. In: 2022 International Conference on Robotics and Automation (ICRA), Philadelphia, PA, USA, 23–27 May 2022, pp. 4686–4693. IEEE.

13.

Cuniato

Geles

Zhang

, et al. (2023) Learning to open doors with an aerial manipulator. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Detroit, MI, USA, 01–05 October 2023, pp. 6942–6948. IEEE.

14.

Deng

Yao

(2021) Asymptotic tracking control of mechanical servosystems with mismatched uncertainties. IEEE/ASME Transactions on Mechatronics 26(4): 2204–2214.

15.

Ding

Pandala

, et al. (2021) Representation-free model predictive control for dynamic motions in quadrupeds. IEEE Transactions on Robotics 37(4): 1154–1171.

16.

Fankhauser

Bjelonic

Bellicoso

, et al. (2018) Robust rough-terrain locomotion with a quadrupedal robot. In: 2018 IEEE International Conference on Robotics and Automation (ICRA), Brisbane, QLD, Australia, 21–25 May 2018, pp. 5761–5768. IEEE.

17.

Goodarzi

Lee

(2013) Geometric nonlinear PID control of a quadrotor UAV on SE(3). In: 2013 European Control Conference (ECC), Zurich, Switzerland, 17–19 July 2013, pp. 3845–3850. IEEE.

18.

Goodarzi

Lee

(2015) Geometric adaptive tracking control of a quadrotor unmanned aerial vehicle on se (3) for agile maneuvers. Journal of Dynamic Systems, Measurement, and Control 137(9): 091007.

19.

Xian

Wang

(2022) Agile flight for a quadrotor via robust geometry control: theory and experimental verification. International Journal of Robust and Nonlinear Control 32(7): 4236–4250.

20.

HSL

(2007) Collection of Fortran codes for large-scale scientific computation. https://www.hsl.rl.ac.uk

21.

Hua

Fang

Zhang

, et al. (2021) A novel robust observer-based nonlinear trajectory tracking control strategy for quadrotors. IEEE Transactions on Control Systems Technology 29(5): 1952–1963.

22.

Huber

Kondak

Krieger

, et al. (2013) First analysis and experiments in aerial manipulation using fully actuated redundant robot arm. In: 2013 IEEE/RSJ international conference on intelligent robots and systems, Tokyo, Japan, 03–07 November 2013, pp. 3452–3457. IEEE.

23.

Jia

Shan

(2020) Finite-time trajectory tracking control of space manipulator under actuator saturation. IEEE Transactions on Industrial Electronics 67(3): 2086–2096.

24.

Jia

Wang

Zhou

(2019) Robust adaptive prescribed performance control for a class of nonlinear pure-feedback systems. International Journal of Robust and Nonlinear Control 29(12): 3971–3987.

25.

Kalabić

Gupta

Di Cairano

, et al. (2017) Mpc on manifolds with an application to the control of spacecraft attitude on so (3). Automatica 76: 293–300.

26.

Kamaldin

Chen

Teo

, et al. (2019) A novel adaptive jerk control with application to large workspace tracking on a flexure-linked dual-drive gantry. IEEE Transactions on Industrial Electronics 66(7): 5353–5363.

27.

Kamel

Verling

Elkhatib

, et al. (2018) The voliro omniorientational hexacopter: an agile and maneuverable tiltable-rotor aerial vehicle. IEEE Robotics & Automation Magazine 25(4): 34–44.

28.

Khalil

Grizzle

(2002) Nonlinear Systems. Prentice-Hall, Vol. 3.

29.

Kidambi

Fermüller

Aloimonos

, et al. (2021) Robust nonlinear control-based trajectory tracking for quadrotors under uncertainty. IEEE Control Systems Letters 5(6): 2042–2047.

30.

Kim

(2023) ROS package for error state kalman filter. https://github.com/ChanghyeonKim93/error_state_kalman_filter_ros

31.

Kim

Choi

Kim

, et al. (2017) Robust control of an equipment-added multirotor using disturbance observer. IEEE Transactions on Control Systems Technology 26(4): 1524–1531.

32.

Lee

Kim

(2017) Estimation, control, and planning for autonomous aerial transportation. IEEE Transactions on Industrial Electronics 64(4): 3369–3379.

33.

Lee

Leok

McClamroch

(2010) Geometric tracking control of a quadrotor UAV on SE(3). In: 49th IEEE Conference on Decision and Control (CDC), Atlanta, GA, USA, 15–17 December 2010, pp. 5420–5425. IEEE.

34.

Lee

Seo

Kim

, et al. (2020) Aerial manipulation using model predictive control for opening a hinged door. In: 2020 IEEE International Conference on Robotics and Automation (ICRA), Paris, France, 31 May 2020–31 August 2020, pp. 1237–1242. IEEE.

35.

Lee

Seo

Jang

, et al. (2021) Aerial manipulator pushing a movable structure using a dob-based robust controller. IEEE Robotics and Automation Letters 6(2): 723–730.

36.

Lee

Byun

Kim

(2022) Rise-based trajectory tracking control of an aerial manipulator under uncertainty. IEEE Control Systems Letters 6: 3379–3384.

37.

Liang

Chen

, et al. (2023) Adaptive prescribed performance control of unmanned aerial manipulator with disturbances. IEEE Transactions on Automation Science and Engineering 20(3): 1804–1814.

38.

Liu

Meng

, et al. (2025) Variational-based geometric nonlinear model predictive control for robust locomotion of quadruped robots. IEEE Transactions on Automation Science and Engineering 22: 12975–12985.

39.

Lynch

Park

(2017) Modern Robotics. Cambridge University Press.

40.

Meduri

Shah

Viereck

, et al. (2023) Biconmp: a nonlinear model predictive control framework for whole body motion planning. IEEE Transactions on Robotics 39(2): 905–922.

41.

Minniti

Farshidian

Grandia

, et al. (2019) Whole-body mpc for a dynamically stable mobile manipulator. IEEE Robotics and Automation Letters 4(4): 3687–3694.

42.

Murooka

Kumagai

Morisawa

, et al. (2021) Humanoid loco-manipulation planning based on graph search and reachability maps. IEEE Robotics and Automation Letters 6(2): 1840–1847.

43.

Nava

Sablé

Tognon

, et al. (2020) Direct force feedback control and online multi-task optimization for aerial manipulators. IEEE Robotics and Automation Letters 5(2): 331–338.

44.

Ollero

Tognon

Suarez

, et al. (2022) Past, present, and future of aerial robotic manipulators. IEEE Transactions on Robotics 38(1): 626–645.

45.

O’Connell

Shi

, et al. (2022) Neural-fly enables rapid learning for agile flight in strong winds. Science Robotics 7(66): eabm6597.

46.

Polycarpou

Ioannou

(1996) A robust adaptive nonlinear control design. Automatica 32(3): 423–427.

47.

Qin

Escande

Kanehiro

, et al. (2023) Dual-arm mobile manipulation planning of a long deformable object in industrial installation. IEEE Robotics and Automation Letters 8(5): 3039–3046.

48.

Ryll

Muscio

Pierri

, et al. (2019) 6d interaction control with aerial robots: the flying end-effector paradigm. The International Journal of Robotics Research 38(9): 1045–1062.

49.

Schulman

Duan

, et al. (2014) Motion planning with sequential convex optimization and convex collision checking. The International Journal of Robotics Research 33(9): 1251–1270.

50.

Seo

Lee

Son

, et al. (2019) Robust trajectory planning for a multirotor against disturbance based on hamilton-jacobi reachability analysis. In: 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Macau, China, 03–08 November 2019, pp. 3150–3157. IEEE.

51.

Shin

Kim

, et al. (2011) Autonomous flight of the rotorcraft-based uav using rise feedback and nn feedforward terms. IEEE Transactions on control systems technology 20(5): 1392–1399.

52.

Sleiman

Farshidian

Minniti

, et al. (2021) A unified mpc framework for whole-body dynamic locomotion and manipulation. IEEE Robotics and Automation Letters 6(3): 4688–4695.

53.

Son

Seo

Jang

, et al. (2020) Real-time optimal trajectory generation and control of a multi-rotor with a suspended load for obstacle avoidance. IEEE Robotics and Automation Letters 5(2): 1915–1922.

54.

Jiao

, et al. (2023) Sequential manipulation planning for over-actuated unmanned aerial manipulators. In: 2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Detroit, MI, USA, 01–05 October 2023, pp. 6905–6911. IEEE.

55.

Sun

Romero

Foehn

, et al. (2022) A comparative study of nonlinear mpc and differential-flatness-based control for quadrotor agile flight. IEEE Transactions on Robotics 38(6): 3357–3373.

56.

Tognon

Cataldi

Chavez

HAT

, et al. (2018) Control-aware motion planning for task-constrained aerial manipulation. IEEE Robotics and Automation Letters 3(3): 2478–2484.

57.

Tognon

Chávez

HAT

Gasparin

, et al. (2019) A truly-redundant aerial manipulator system with application to push-and-slide inspection in industrial plants. IEEE Robotics and Automation Letters 4(2): 1846–1851.

58.

Wang

Chen

Guo

, et al. (2023) Millimeter-level pick and peg-in-hole task achieved by aerial manipulator. IEEE Transactions on Robotics 40: 1242–1260.

59.

Wehbeh

Sharf

(2022) An mpc formulation on so(3) for a quadrotor with bidirectional thrust and nonlinear thrust constraints. IEEE Robotics and Automation Letters 7(2): 4945–4952.

60.

Welde

Paulos

Kumar

(2021) Dynamically feasible task space planning for underactuated aerial manipulators. IEEE Robotics and Automation Letters 6(2): 3232–3239.

61.

Cheng

Zhao

, et al. (2025) L1quad: L1 adaptive augmentation of geometric control for agile quadrotors with performance guarantees. IEEE Transactions on Control Systems Technology 33(2): 597–612.

62.

Xian

Zhang

(2016) A new smooth robust control design for uncertain nonlinear systems with non-vanishing disturbances. International Journal of Control 89(6): 1285–1302.

63.

Xian

Dawson

de Queiroz

, et al. (2004) A continuous asymptotic tracking control strategy for uncertain nonlinear systems. IEEE Transactions on Automatic Control 49(7): 1206–1211.

64.

Chen

Wen

, et al. (2024) Predefined-time tracking control of a free-flying space robot on SE(3). IEEE Transactions on Aerospace and Electronic Systems 60(5): 5906–5919.

65.

Yao

(2021) Adaptive trajectory tracking control of a free-flying space manipulator with guaranteed prescribed performance and actuator saturation. Acta Astronautica 185: 283–298.

66.

Gong

(2020) Finite-time geometric control for underactuated aerial manipulators with unknown disturbances. International Journal of Robust and Nonlinear Control 30(13): 5040–5061.

67.

Zhang

Yang

Chen

, et al. (2020) A vector-based constrained obstacle avoidance scheme for wheeled mobile redundant robot manipulator. IEEE Transactions on Cognitive and Developmental Systems 13(3): 465–474.

68.

Zhao

Kawasaki

Anzai

, et al. (2018) Transformable multirotor with two-dimensional multilinks: modeling, control, and whole-body aerial manipulation. The International Journal of Robotics Research 37(9): 1085–1112.

69.

Zhao

Okada

Inaba

(2023) Versatile articulated aerial robot dragon: aerial manipulation and grasping by vectorable thrust control. The International Journal of Robotics Research 42(4-5): 214–248.

70.

Zhu

Qiao

Zhang

, et al. (2019) High-precision trajectory tracking control for space manipulator with neutral uncertainty and deadzone nonlinearity. IEEE Transactions on Control Systems Technology 27(5): 2254–2262.

Supplementary Material

Please find the following supplemental material available below.

For Open Access articles published under a Creative Commons License, all supplemental material carries the same license as the article it is associated with.

For non-Open Access articles published, all supplemental material carries a non-exclusive license, and permission requests for re-use of supplemental material or any part of supplemental material shall be sent directly to the copyright owner as specified in the copyright notice associated with the article.

0.00 MB