SCTOMP Spatially Constrained Time-Optimal Motion Planning Jon Arrizabalaga1and Markus Ryll12

2025-05-03 0 0 1.7MB 8 页 10玖币

SCTOMP:

Spatially Constrained Time-Optimal Motion Planning

Jon Arrizabalaga1and Markus Ryll1,2

Published in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Detroit, USA, 2023.

including reprinting/republishing this material for advertising or promotional purposes, creating new collective works, for resale or redistribution to servers

or lists, or reuse of any copyrighted component of this work in other works.

Abstract— This work focuses on spatial time-optimal motion

planning, a generalization of the exact time-optimal path

following problem that allows a system to plan within a

predeﬁned space. In contrast to state-of-the-art methods, we

drop the assumption of a given collision-free geometric ref-

erence. Instead, we present a three-stage motion planning

method that solely relies on start and goal locations and a

geometric representation of the environment to compute a time-

optimal trajectory that is compliant with system dynamics

and constraints. The proposed scheme ﬁrst ﬁnds collision-

free navigation corridors, second computes an obstacle-free

Pythagorean Hodograph parametric spline along each corri-

dor, and third, solves a spatially reformulated minimum-time

optimization problem at each of these corridors. The spline

obtained in the second stage is not a geometric reference, but

an extension of the free space associated with its corridor, and

thus, time-optimality of the solution is guaranteed. The validity

of the proposed approach is demonstrated by a well-established

planar example and benchmarked in a spatial system against

state-of-the-art methodologies across a wide range of scenarios

in highly congested environments.

Video:https://youtu.be/zGExvnUEfOY

I. INTRODUCTION

Time-optimal motion planning within cluttered environments

poses multiple challenges. The underlying motion planning

scheme needs to compute a set of input commands that

drive the system from its current state to a goal location

in minimum-time, without compromising system constraints

and spatial bounds. Thus, its solution implies a trading-off

between time-optimality and spatial-awareness.

The de facto approach to solve this problem has been to

decouple it into two stages [1]–[4]. First, the path planning

stage determines a collision-free geometric path according

to high level – task related – commands [5]. Second, the

predeﬁned path is (exactly) tracked either by path tracking or

path following. The former computes a dynamically feasible

timing law for traversing along the predetermined geometric

path – when to be where –, while the latter introduces the

timing law and the (bounded) distance to the path as control

freedoms [6]–[8].

When tackling time-optimality, previous work mainly

focused on the second stage. Time-optimal path-tracking

for robotic manipulators is a long studied problem [9]–

[11]. Its convexity was proven in [6] and in [12] it was

extended to a wider range of systems. Enhanced numerical

implementations to exploit this convexity were presented

1Autonomous Aerial Systems, School of Engineering and

Design, Technical University of Munich, Germany. E-mail:

jon.arrizabalaga@tum.de and markus.ryll@tum.de

2Munich Institute of Robotics and Machine Intelligence (MIRMI), Tech-

nical University of Munich

in [13], [14]. Regarding time-optimal path-following, [15],

[16] leveraged a spatial reformulation of the system dynam-

ics, allowing to compute minimum-time trajectories around

the predeﬁned path. Similarly, assuming waypoints to be

predetermined, [17] made use of Complementary Progress

Constraints (CPC) to compute time-optimal trajectories for

quadrotors. Lastly, exploiting optimal control, nonlinear

model predictive control (NMPC) methods based on the

aforementioned spatial reformulation [18], [19] and con-

touring control [20], [21] have shown to approximate time-

optimal performance in race-alike scenarios, i.e., around

a predeﬁned geometric path (the centerline of the track).

However, the optimality of all these approaches is upper

bounded by the geometric reference predetermined by the

path planning stage. In other words, only if both the path

planning and path tracking/following stages are optimal, will

the resultant planned trajectory be optimal, implying that

the decoupled nature of these methods jeopardizes the time-

optimality of the planned trajectories.

To overcome this conceptual shortcoming, [22] presented

a hierarchical sampling-based method capable of computing

time-optimal trajectories to ﬂy a quadrotor over a set of

waypoints within cluttered environments. This was further

extended in [23], where planning and control were simultane-

ously solved with a deep Reinforcement Learning (deep RL)

approach. Despite the ability to roughly approximate time-

optimal trajectories in congested scenarios, none of these

methods can guarantee that the resultant trajectory is the

true-optimal. On the one hand, the sampling-based nature

of [22] renders it nondeterministic, introducing randomness

into the solution. On the other hand, the additional tracking

capabilities brought by the end-to-end learning approach in

[23] come at the expense of 1) system-speciﬁcity – changes

in the system dynamics require retraining – and 2) sub-

optimal trajectories resulting from learning the trade-off

between ﬂying safe and fast.

This raises the question on how to formulate a motion

planning scheme, applicable to any system and constrained

environment, that guarantees to compute the time-optimal

trajectory compliant with system dynamics, state/input con-

straints and spatial bounds. To answer this question, in this

paper we present a Spatially Constrained Time-Optimal

Motion Planner (SCTOMP): an ofﬂine motion planning ap-

proach capable of computing dynamically feasible, collision-

free and time-optimal trajectories, by solely relying on a goal

location and a geometric representation of the obstacle-free

environment.

For this purpose, we formulate a three-stage approach,

arXiv:2210.02345v2 [cs.RO] 15 Jul 2023

where, ﬁrstly collision-free navigation corridors are found,

secondly parametric paths within all corridors are computed,

and thirdly a spatially reformulated time minimization per

corridor is solved. In contrast to the aforementioned decou-

pled approaches, the parametric paths obtained in the second

stage are not a geometric reference, but an extension of

the free space associated with the respective corridor, and

consequently, have no effect on the optimality of the time-

minimizing problem in the third stage.

The presented scheme consists of three main ingredients:

1) Through the use of a spatial reformulation presented

in [24], we perform a spatial transformation of the time-

based system states to path coordinates. The resultant system

dynamics evolve according to a path parameter ξinstead of

time t. 2) We leverage Pythagorean Hodograph curves to

efﬁciently and analytically compute the parametric functions

required by this spatial reformulation. 3) Using the ﬁrst

and second ingredients, the time minimization problem is

reduced into a ﬁnite horizon spatial problem with convex

spatial bounds.

More speciﬁcally, we make the following contributions:

1) We extend the applicability of the spatial reformula-

tion introduced in [24], by showing how it allows to

conduct a singularity-free spatial transformation of the

dynamics for any arbitrary system.

2) We identify a computationally tractable methodology

to compute Pythagorean Hodograph splines in a closed

form, without the need for additional optimizations.

3) We derive a motion planning methodology that,

given a nonlinear system and constrained environment,

ﬁnds the collision-free and dynamically feasible time-

optimal trajectory.

The remainder of this paper is structured as follows:

Section II introduces the spatially constrained time-optimal

motion planning problem. Section III presents the solution

proposed in this paper, by revisiting the aforementioned

spatial reformulation, its compatibility with PH curves, and

performing a spatial transformation of the time minimization

problem. Experimental results are shown in Section IV

before Section V presents the conclusions.

Notation: We will use ˙

(·) = d(·)

dtfor time derivatives and

(·)′=d(·)

dξfor differentiating over path parameter ξ. For

readability we will employ the abbreviation tξ=ξ(t).

II. PROBLEM STATEMENT

We consider continuous time, nonlinear systems of the form

x(t) = f(x(t),u(t)),x(t0) = x0,(1a)

y(t) = h(x(t)) ,(1b)

where x∈ X ⊆ Rnxand u∈ U ⊆ Rnudeﬁne state and

input constraints. Function f:Rnx×Rm7→ Rnxrefers

to the equations of motion of an arbitrary dynamic system,

while the map h:Rnx7→ Rnydeﬁnes the output of the

system y∈Rny. Both fand hfunctions are assumed to

be sufﬁciently continuously differentiable. The initial state

is contained in a subset of the constrained state set, i.e.,

x0∈ X0⊆ X .

To ensure that the system remains in the obstacle free

space, the geometrically constrained environment is de-

scribed as

Ω = {g(h(x)) <0,∀x∈ X } ∈ Rny,(2)

where we also assume the function g:Rny7→ Rnyto be

sufﬁciently continuously differentiable.

Spatially Constrained Time-Optimal Motion Planning

refers to the problem of planning a trajectory that steers

system (1) from its initial state x0to a goal output state

yf∈Rnyin minimum-time, without compromising the

system dynamics in (1a), ensuring the integrity of state and

input constraints – x∈ X ,u∈ U – and guaranteeing that

output (1b) remains within the free space in (2), i.e., y∈Ω.

This is equivalent to solving the following optimal control

problem (OCP):

min

x(·),u(·)T=ZT

dt (3a)

s.t. x(0) = x0,(3b)

x=f(x(t),u(t)), t ∈[0, T ](3c)

x(t)∈ X ,u(t)∈ U , t ∈[0, T ](3d)

y(t)∈Ω, t ∈[0, T ](3e)

y(T) = yf.(3f)

As mentioned earlier, decoupled approaches separate this

problem into a path planning and path tracking/following

problem. The computational tractability advantages brought

by these methods come at the expense of an inherited

suboptimality in the planned trajectories. We seek to close

this gap by formulating a method that directly solves the

minimum-time problem (3), eliminating the need for the path

planning stage, and thus leveraging the entire free space to

compute the time-optimal trajectory.

III. SOLUTION APPROACH

The motion planning scheme presented in this paper lever-

ages (i) a spatial transformation of the system dynamics with

respect to (ii) an arbitrary parametric curve with an associ-

ated adapted frame to perform (iii) a spatial reformulation

of the time-minimizing problem (3). These three ingredients

are the main building blocks of the proposed methodology.

In this section, we present further details on each of them.

A. SPATIAL TRANSFORMATION OF SYSTEM DYNAMICS

Let Γrefer to a geometric reference and be deﬁned as

a path with an associated adapted-frame, whose position

and orientation are given by two sufﬁciently continuous

functions, γ:R7→ R3and R :R7→ R3×3, that depend on

path parameter ξ:

Γ = {ξ∈[ξ0, ξf]⊆R7→ γ(ξ)∈R3,R(ξ)∈R3×3}(4)

Decomposing the adapted-frame into its components R(ξ) =

{e1(ξ),e2(ξ),e3(ξ)}allows to deﬁne its angular velocity

文档加载中……请稍候！
如果长时间未打开，您也可以点击刷新试试。

下载文档到电脑，查找使用更方便

10 玖币 0人已下载

立即下载

摘要：

SCTOMP:SpatiallyConstrainedTime-OptimalMotionPlanningJonArrizabalaga1andMarkusRyll1,2PublishedinIEEE/RSJInternationalConferenceonIntelligentRobotsandSystems(IROS),Detroit,USA,2023.©2023IEEE.Personaluseofthismaterialispermitted.PermissionfromIEEEmustbeobtainedforallotheruses,inanycurrentorfuturemedia...

展开>> 收起<<

SCTOMP Spatially Constrained Time-Optimal Motion Planning Jon Arrizabalaga1and Markus Ryll12.pdf

共8页,预览2页

还剩页未读，继续阅读

声明：本站为文档C2C交易模式，即用户上传的文档直接被用户下载，本站只是中间服务平台，本站所有文档下载所得的收益归上传人(含作者)所有。玖贝云文库仅提供信息存储空间，仅对用户上传内容的表现方式做保护处理，对上载内容本身不做任何修改或编辑。若文档所含内容侵犯了您的版权或隐私，请立即通知玖贝云文库，我们立即给予删除！

SCTOMP Spatially Constrained Time-Optimal Motion Planning Jon Arrizabalaga1and Markus Ryll12

相关推荐

开通VIP享超值会员特权

作者详情

相关内容

热门标签

举报选择: