KHAOS Improvements Using BSpline

ome Truc

, Daniel Sidobre

and Rachid Alami

LAAS-CNRS, Universit

e de Toulouse, CNRS, UPS, Toulouse, France

Keywords:

Human-Robot Interaction, Trajectory Planning, Autonomous Aerial Manipulators.

Abstract:

The introduction of BSpline to generate the trajectories for a Kinematic Human Aware Optimization-based

System for Reactive Planning of a Flying-Coworker, a multi-rotor drone that collaborates with workers to

fetch small objects, is presented. This allows to improve the quality of the motions and helps to better respect

the constraints relative to the collaboration with a human.

1 INTRODUCTION

In our previous work, we presented KHAOS

(Truc et al., 2022), a Kinematic Human Aware

Optimization-based System for Reactive Planning of

Flying-Coworker in the context of the ”Flying Co-

Worker” project: a multi-rotor drone that collaborates

with workers to fetch small objects. The robot must

ﬂy in a human-populated area, where only a hand-

ful of human workers may be “aware” of the robot’s

current task and mission, and a fraction of them may

be involved in the physical interaction (e.g., object

delivery): the robot must assume that most humans

are “observers”, i.e., they ignore its current mission

and are not involved with it. In such conditions, the

drone needs to carefully plan its 3D motion in a reac-

tive way to navigate and act safely in close proxim-

ity to humans. Beyond safety, the drone should aim

at exhibiting navigation strategies that are, as much

as possible, socially aware: for example, it should

avoid fast movements that could scare the observers;

it should maximize its visibility (Sisbot et al., 2007)

for workers, especially when engaging in an interac-

tion. To address the navigation requirements men-

tioned above, we proposed KHAOS for reactive plan-

ning, which produces trajectories in the 3D space sat-

isfying the kinematic constraints of the drone and en-

suring the visibility and ease of the humans present in

the environment. The human-aware behavior is real-

ized by proposing a visibility cost and a novel discom-

fort cost and including these along with the kinematic

constraints into a stochastic optimization process in-

https://orcid.org/0000-0002-2244-3385

https://orcid.org/0000-0002-5564-2735

https://orcid.org/0000-0002-9558-8163

spired by the STOMP algorithm (Kalakrishnan et al.,

2011).

In this paper, we ﬁrst show the beneﬁts of an

extension of Softmotion (Sidobre and Desormeaux,

2019) using BSpline as input to KHAOS to smooth

the initial path. Then, in two different scenarios, we

show the interest of using BSpline to improve the tra-

jectories generated by KHAOS and to ensure that they

are feasible.

2 INITIAL PATH SMOOTHING

STOMP algorithm optimize an initial path from a

set of costs to generate a new smoothed path. This

smoothing is dependent on the initial path, indeed

STOMP does not allow to smooth the too important

breaks of the initial path. These breaks will remain in

the ﬁnal result and may even be ampliﬁed. By inher-

itance, KHAOS suffers from the same problem and

thus requires smoothing the initial input path in order

to obtain a satisfactory output. Moreover, it gener-

ates a 3D trajectory constrained by kinematic limits

(bounded velocity, acceleration and jerk) and human-

aware costs that can also have an impact on the kine-

matic parameters. This trajectory must therefore re-

spect the feasibility from a kinematic point of view

by avoiding, for example, too sharp turns that would

force the robot to slow down during execution.

To this end, we propose the use of the SoftMo-

tion library and in particular an extension of the well

known Non Uniform B-Spline (Piegl and Tiller, 1996;

Rousseau, 2019) to generate feasible trajectories from

a list of waypoints. The main advantage of this ex-

tension is to allow the control of the kinematic pa-

Truc, J., Sidobre, D. and Alami, R.

KHAOS Improvements Using BSpline.

DOI: 10.5220/0011957000003622

In Proceedings of the 1st International Conference on Cognitive Aircraft Systems (ICCAS 2022), pages 43-46

ISBN: 978-989-758-657-6

 2023 by SCITEPRESS – Science and Technology Publications, Lda. Under CC license (CC BY-NC-ND 4.0)

rameters at the beginning and at the end of the trajec-

tory, which is necessary to ensure continuity between

consecutive trajectories generated by two successive

plannings. Many planners such as PRM or RRT al-

low to generate an initial path connecting a start point

and an end point. Between each waypoint composing

this initial path, we have segments that by nature do

not allow to respect a continuity. The use of BSpline

will allow to generate a smooth trajectory from this

list of waypoints even when they are numerous and

very close. In Fig.1, We compare the outputs of the

standard version of KHAOS when no smoothing is

applied (Fig.1a) and when a B-Spline is used (Fig.1b).

When no smoothing is performed, we show that even

after many iterations, the continuity defects present in

the initial path are transmitted to the KHAOS result.

On the other hand, when using B-Spline, we can ob-

serve that the trajectory generated by KHAOS is very

smooth too. This allows our algorithm to start on a

good basis and will facilitate the optimization phase

because naturally the continuity will be respected.

3 KINEMATIC COMPLIANCE

In addition to smoothing and giving a harmonious

shape to the trajectory, this SoftMotion extension al-

lows, from the list of waypoints from KHAOS, to en-

sure continuity from a kinematic point of view but

also to respect the human-aware constraints. Indeed,

KHAOS provides the position and speed for each

waypoint according to constraints adapted to humans

in the environment but also considering the kinematic

limits of the drone. For example, if the drone starts far

enough from any human and outside its visual ﬁeld,

it will accelerate to its maximum speed at maximum

acceleration (Fig.2 between 0 s and 2 s). When it is

close enough to a human, the human-aware costs will

become important. When approaching a human, for

a handover for example, the drone will undergo a de-

celeration (Fig.2 between 8 s and 14 s) dictated by the

human-aware costs but KHAOS also veriﬁes that this

deceleration is achievable by the drone considering its

deceleration limits. If it is not feasible from the kine-

matic limits point of view, KHAOS will generate a

trajectory where the drone will slow down earlier so

that it is feasible and that the human-aware constraints

are also respected.

We wish that between each of these waypoints

(segments), the respect of these constraints is ensured.

For example, in order not to cause too much discom-

fort to humans in the environment, KHAOS makes

sure, among other things, to limit the speed accord-

ing to the distance to the human. It is therefore neces-

(a) Standard KHAOS.

(b) KHAOS using BSpline.

Figure 1: Shapes of the initial path (red) and trajectory gen-

erated by KHAOS (green) after several iterations for two

cases: a) when no smoothing is applied to the initial path

and b) when BSpline are used to smooth.

sary to be sure that between two positions provided by

KHAOS, the speed along the segment is in agreement

with the speeds given at each of these two positions.

If we look for example at the KHAOS curve not us-

ing BSpline (Fig.2 in green), we notice around 1.8 s

and 8 s, sharp changes if we consider simple line seg-

ments connecting the waypoints. These defects are

corrected by using the BSpline (Fig.2 in orange), al-

lowing on the one hand to ensure that it is physically

feasible by the drone, but especially to obtain a more

ICCAS 2022 - International Conference on Cognitive Aircraft Systems

ﬂuid movement and therefore less uncomfortable for

the human present in the vicinity.

Figure 2: Magnitude of the drone’s speed before (green)

and after (orange) using BSpline in the KHAOS output in

the scenario where the drone approaches a human from the

front in a straight line.

The time bases do not match between the two curves be-

cause the calculation of the times is different between the

calculation of the waypoints and SoftMotion.

4 TWO HUMANS

We take the example of the corridor with two hu-

mans presented in our previous paper (Truc et al.,

2022). The objective for the drone is to navigate from

one end of the corridor (Fig.3a), pass the human in

blue, then arrive from the back of the human in green

(Fig.3b) in order to position itself in front of him as if

it wanted to exchange an object with him. We show

in Fig.4, the magnitude speed proﬁle along the trajec-

tory actually executed by the drone by concatenating

the trajectory portions generated at each iteration of

KHAOS. Indeed, another advantage of our SoftMo-

tion extension, is to generate trajectories from a non-

zero speed and acceleration set-point needed as input

by KHAOS in a reactive planning context. In the situ-

ation presented here, the human-aware constraints as

well as the environment of the corridor constrains the

trajectory of the drone, forcing it to pass close to the

human in blue and thus reduce its speed (Fig.4 be-

tween 3 s and 13 s), once out of the inﬂuence of the

ﬁrst human, it can accelerate again. Once it gets close

enough and in the back of the human in green, it must

slow down again (Fig.4 between 15 s and 20 s) and

turn around the human to appears in the human vi-

sual ﬁeld before ﬁnishing its approach. We can notice

how the speed proﬁle is smoothed when the drone ap-

proaches the speed limit, or when it has to make a

sharp turn (around 20 s).

5 CONCLUSIONS

The use of BSpline is a real gain for KHAOS, allow-

ing our algorithm to propose more realistic trajecto-

(a) Start trajectory.

(b) Later trajectory.

Figure 3: Trajectory execution in a two-humans scenario.

a) First trajectory generated by KHAOS, the drone starts by

navigating in the corridor. b) A few iterations later, after

passing the blue human in the corridor. Red line: represents

initial path before smoothing using BSpline. Red arrows:

highest speed magnitude. Green arrows: lowest speed mag-

nitude.

ries that can be directly used by a basic controller.

As we have shown, this makes it possible to improve

the initial path at the input of KHAOS and also to en-

sure to provide at the output a trajectory achievable by

the drone and respecting the human-aware constraints

even better. From the human-aware point of view, it

also improves the behavior of the drone whose move-

ments are more ﬂuid and cause even less discomfort

than with the standard version. We also presented in

our scenario with two humans, the ﬁnal result of a tra-

KHAOS Improvements Using BSpline

Figure 4: Magnitude of the drone’s speed using BSpline

composed of many iterations of KHAOS in the scenario

with two humans.

jectory executed by the drone and composed of many

iterations of KHAOS, showing the ability of our Soft-

motion extension to generate BSpline resuming the

non-zero speed and acceleration set point of the previ-

ous iterations. In the near future, we want to improve

this extension of SoftMotion by adding an optimiza-

tion step to further reﬁne the kinematic parameters.

Also, we plan to explore the possibility of implement-

ing the use of BSpline directly in the core of KHAOS,

in order to work directly on feasible trajectories at the

optimization level.

ACKNOWLEDGMENTS

This work was partially supported by the French Na-

tional Research Agency (ANR) (project The Flying

Co-Worker, grant ANR-18-CE33-0001), the Euro-

pean Commission (AnDy, GA. n. 731540; CHIST-

ERA - HEAP) and the Artiﬁcial and Natural Intelli-

gence Toulouse Institute - Institut 3iA (ANITI) under

grant agreement No: ANR-19-PI3A-0004.

REFERENCES

Kalakrishnan, M., Chitta, S., Theodorou, E., Pastor, P., and

Schaal, S. (2011). STOMP: Stochastic trajectory opti-

mization for motion planning. In 2011 IEEE Interna-

tional Conference on Robotics and Automation, pages

4569–4574, Shanghai, China. IEEE.

Piegl, L. and Tiller, W. (1996). The NURBS book. Springer

Science & Business Media.

Rousseau, G. (2019). Optimal trajectory planning and pre-

dictive control for cinematographic ﬂight plans with

quadrotors. PhD thesis, Universit

e Paris-Saclay.

Sidobre, D. and Desormeaux, K. (2019). Smooth cubic

polynomial trajectories for human-robot interactions.

Journal of Intelligent & Robotic Systems, 95(3):851–

869.

Sisbot, E., Marin-Urias, L., Alami, R., and Simeon, T.

(2007). A Human Aware Mobile Robot Motion Plan-

ner. IEEE Transactions on Robotics, 23(5):874–883.

Truc, J., Singamaneni, P.-T., Sidobre, D., Ivaldi, S., and

Alami, R. (2022). Khaos: a kinematic human aware

optimization-based system for reactive planning of

ﬂying-coworker. In ICRA 2022.

ICCAS 2022 - International Conference on Cognitive Aircraft Systems