Journal of Automation, Mobile Robotics and Intelligent Systems, vol. 15, no. 1 (2021) by Ł-PIAP

VOLUME 15, N° 1, 2021 www.jamris.org

Indexed in SCOPUS



Journal of Automation, Mobile Robotics and Intelligent Systems pISSN 1897-8649 (PRINT) / eISSN 2080-2145

(ONLINE)

WWW.JAMRIS.ORG • pISSN 1897-8649 (PRINT) / eISSN 2080-2145 (ONLINE) • VOLUME 15, N° 1, 2021

Ł-PIAP

logo podstawowe skrót

Publisher: Łukasiewicz – Industrial Research Institute for Automation and Measurements PIAP

logo podstawowe skrót

Łukasiewicz – Industrial Research Institute for Automation and Measurements PIAP

Journal of Automation, Mobile Robotics and Intelligent Systems A peer-reviewed quarterly focusing on new achievements in the following fields: • Fundamentals of automation and robotics • Applied automatics • Mobile robots control • Distributed systems • Navigation • Mechatronic systems in robotics • Sensors and actuators • Data transmission • Biomechatronics • Mobile computing Editor-in-Chief

Typesetting

Janusz Kacprzyk (Polish Academy of Sciences, Łukasiewicz-PIAP, Poland)

PanDawer, www.pandawer.pl

Advisory Board

Webmaster

Dimitar Filev (Research & Advenced Engineering, Ford Motor Company, USA) Kaoru Hirota (Japan Society for the Promotion of Science, Beijing Office) Witold Pedrycz (ECERF, University of Alberta, Canada)

Piotr Ryszawa (Łukasiewicz-PIAP, Poland)

Editorial Office ŁUKASIEWICZ Research Network – Industrial Research Institute for Automation and Measurements PIAP Al. Jerozolimskie 202, 02-486 Warsaw, Poland (www.jamris.org) tel. +48-22-8740109, e-mail: office@jamris.org

Co-Editors Roman Szewczyk (Łukasiewicz-PIAP, Warsaw University of Technology, Poland) Oscar Castillo (Tijuana Institute of Technology, Mexico) Marek Zaremba (University of Quebec, Canada)

The reference version of the journal is e-version. Printed in 100 copies.

Executive Editor ´ Katarzyna Rzeplinska-Rykała, e-mail: office@jamris.org (Łukasiewicz-PIAP, Poland)

Articles are reviewed, excluding advertisements and descriptions of products.

Associate Editor

If in doubt about the proper edition of contributions, for copyright and reprint permissions please contact the Executive Editor.

´ (Poznan ´ University of Technology, Poland) Piotr Skrzypczynski

Statistical Editor ´ Małgorzata Kaliczynska (Łukasiewicz-PIAP, Poland)

Editorial Board:

Mark Last (Ben-Gurion University, Israel) Anthony Maciejewski (Colorado State University, USA) Krzysztof Malinowski (Warsaw University of Technology, Poland) Andrzej Masłowski (Warsaw University of Technology, Poland) Patricia Melin (Tijuana Institute of Technology, Mexico) Fazel Naghdy (University of Wollongong, Australia) Zbigniew Nahorski (Polish Academy of Sciences, Poland) Nadia Nedjah (State University of Rio de Janeiro, Brazil) Dmitry A. Novikov (Institute of Control Sciences, Russian Academy of Sciences, Russia) Duc Truong Pham (Birmingham University, UK) Lech Polkowski (University of Warmia and Mazury, Poland) Alain Pruski (University of Metz, France) Rita Ribeiro (UNINOVA, Instituto de Desenvolvimento de Novas Tecnologias, Portugal) Imre Rudas (Óbuda University, Hungary) Leszek Rutkowski (Czestochowa University of Technology, Poland) Alessandro Saffiotti (Örebro University, Sweden) Klaus Schilling (Julius-Maximilians-University Wuerzburg, Germany) Vassil Sgurev (Bulgarian Academy of Sciences, Department of Intelligent Systems, Bulgaria) Helena Szczerbicka (Leibniz Universität, Germany) Ryszard Tadeusiewicz (AGH University of Science and Technology, Poland) Stanisław Tarasiewicz (University of Laval, Canada) Piotr Tatjewski (Warsaw University of Technology, Poland) Rene Wamkeue (University of Quebec, Canada) Janusz Zalewski (Florida Gulf Coast University, USA) ´ Teresa Zielinska (Warsaw University of Technology, Poland)

Chairman – Janusz Kacprzyk (Polish Academy of Sciences, Łukasiewicz-PIAP, Poland) Plamen Angelov (Lancaster University, UK) Adam Borkowski (Polish Academy of Sciences, Poland) Wolfgang Borutzky (Fachhochschule Bonn-Rhein-Sieg, Germany) Bice Cavallo (University of Naples Federico II, Italy) Chin Chen Chang (Feng Chia University, Taiwan) Jorge Manuel Miranda Dias (University of Coimbra, Portugal) Andries Engelbrecht (University of Pretoria, Republic of South Africa) Pablo Estévez (University of Chile) Bogdan Gabrys (Bournemouth University, UK) Fernando Gomide (University of Campinas, Brazil) Aboul Ella Hassanien (Cairo University, Egypt) Joachim Hertzberg (Osnabrück University, Germany) Evangelos V. Hristoforou (National Technical University of Athens, Greece) Ryszard Jachowicz (Warsaw University of Technology, Poland) Tadeusz Kaczorek (Białystok University of Technology, Poland) Nikola Kasabov (Auckland University of Technology, New Zealand) ´ Marian P. Kazmierkowski (Warsaw University of Technology, Poland) Laszlo T. Kóczy (Szechenyi Istvan University, Gyor and Budapest University of Technology and Economics, Hungary) Józef Korbicz (University of Zielona Góra, Poland) ´ University of Technology, Poland) Krzysztof Kozłowski (Poznan Eckart Kramer (Fachhochschule Eberswalde, Germany) Rudolf Kruse (Otto-von-Guericke-Universität, Germany) Ching-Teng Lin (National Chiao-Tung University, Taiwan) Piotr Kulczycki (AGH University of Science and Technology, Poland) Andrew Kusiak (University of Iowa, USA)

Publisher: ŁUKASIEWICZ Research Network – Industrial Research Institute for Automation and Measurements PIAP

Articles

Journal of Automation, Mobile Robotics and Intelligent Systems Volume 15, N° 1, 2021 DOI: 10.14313/JAMRIS/1-2021

Contents 3

A New Method for Computation of Positive Realizations of Linear Discrete-Time Systems Tadeusz Kaczorek DOI: 10.14313/JAMRIS/1-2021/1 9

Continuous-Curvature Trajectory Planning Jörg Roth DOI: 10.14313/JAMRIS/1-2021/2 24

Humanoid Robot Path Planning Using Rapidly Explored Random Tree and Motion Primitives Maksymilian Szumowski, Teresa Zielińska DOI: 10.14313/JAMRIS/1-2021/3 31

HuBeRo – a Framework to Simulate Human Behaviour in Robot Research Jarosław Karwowski, Wojciech Dudek, Maciej Węgierek, Tomasz Winiarski DOI: 10.14313/JAMRIS/1-2021/4

Articles

A New Approach to the Histogram Based Segmentation of Synthetic Aperture Radar Images Barbara Siemiątkowska, Krzysztof Gromada DOI: 10.14313/JAMRIS/1-2021/5 43

MATLAB Implementation of Direct and Indirect Shooting Methods to Solve an Optimal Control Problem With State Constraints Andrzej Karbowski DOI: 10.14313/JAMRIS/1-2021/6 51

A Novel Merchant Optimization Algorithm for Solving Optimal Reactive Power Problem K. Lenin DOI: 10.14313/JAMRIS/1-2021/7

Journal of Automation, Mobile Robotics and Intelligent Systems

VOLUME 15,

N° 1

2021

A New Method for Computation of Positive Realizations of Linear Discrete-Time Systems Submitted: 26th November 2019; accepted: 14th March 2021

Tadeusz Kaczorek

DOI: 10.14313/JAMRIS/1-2021/1 Abstract: A new method for computation of positive realizations of given transfer matrices of linear discretetime linear systems is proposed. Sufficient conditions for the existence of positive realizations of transfer matrices are given. A procedure for computation of the positive realizations is proposed and illustrated by an example. Keywords: computation, positive, realization, transfer matrix, linear, discrete-time, system.

1. Introduction A dynamical system is called positive if its trajectory starting from any nonnegative initial state remains forever in the positive orthant for all nonnegative inputs. An overview of state of the art in positive systems theory is given in the monographs [2, 11]. Variety of models having positive behavior can be found in engineering, economics, social sciences, biology and medicine, etc. [2, 11]. The determination of the matrices A, B, C, D of the state equations of linear systems for given their transfer matrices is called the realization problem. The realization problem isa classical problem of analysis of linear systems and has been considered in many books and papers [9, 10, 20, 22]. A tutorial on the positive realization problem has been given in the paper [1] and in the books [2, 11]. The positive minimal realization problem for linear systems without and with delays has been analyzed in [3, 5-7, 11-15, 18, 19, 21]. The existence and determination of the set of Metzler matrices for given stable polynomials have been considered in [8]. The realization problem for positive 2D hybrid systems has been addressed in [17]. For fractional linear systems the realization problem has been considered in [16, 20, 22]. In this paper a new method for computation of positive realizations of linear discrete-time systems is proposed. It can be considered as an extension to discrete-time systems of the method presented in [4]. The paper is organized as follows. In section 2 some definitions and theorems concerning the positive discrete-time linear systems are recalled. A new method for computation of positive realizations for

single-input single-output linear systems is proposed in section 3 and for multi-input multi-output systems in section 4. Concluding remarks are given in section 5. The following notation will be used: ℜ – the set of real numbers, ℜn×m – the set of n × m real matrices, ℜn+×m – the set of n × m real matrices with nonnegative entries and ℜn+ = ℜn+×1, In – the n × n identity matrix.

2. Preliminaries

Consider the discrete-time linear system

x i +1 = Ax i + Bui , (2.1a)

yi = Cx i + Dui , (2.1b) n m p where x i ∈ℜ , ui ∈ℜ , yi ∈ℜ are the state, input and output vectors and A ∈ℜn×n , B ∈ℜn×m, C ∈ℜ p×n, D ∈ℜ p×m . Definition 2.1. [2, 11] The system (2.1) is called (internally) positive if x i ∈ℜn+ and yi ∈ℜ +p, i ∈ Z + for any initial conditions x0 ∈ℜn+ and all inputs ui ∈ℜm +, i ∈ Z +. Theorem 2.1. [2, 11] The system (2.1) is positive if and only if

A ∈ℜn+×n , B ∈ℜn+×m , C ∈ℜ +p×n , D ∈ℜ +p×m (2.2)

The transfer matrix of the system (2.1) is given by

T ( z ) = C[I z − A] B + D . (2.3)

Definition 2.2. [1, 22] The matrices (2.2) are called a positive realization of T(z) if they satisfy the equality (2.3). Definition 2.3. [1, 22] The system (2.1) is called asymptotically stable if the matrix A is a Schur matrix. Theorem 2.2. [1, 22] The positive realization (2.2) is asymptotically stable if and only if all coefficients of the polynomial

pA ( z ) = det[In ( z + 1) − A] = z n + an−1 z n−1 + ... + a1 z + a0

(2.4) are positive, i.e. ai > 0 for i = 0,1,...,n – 1. The positive realization problem can be stated as follows. Given a proper transfer matrix T(z) find its positive realization (2.2). Theorem 2.3. [22] If (2.2) is a positive realization of (2.3) then the matrices

A = PAP −1 , B = PB , C = CP −1 , D = D (2.5)

Journal of Automation, Mobile Robotics and Intelligent Systems

are also a positive realization of (2.3) if and only if the matrix P ∈ℜn+×n is a monomial matrix (in each row and in each column only one entry is positive and the remaining entries are zero). Proof. Proof follows immediately from the fact that P −1 ∈ℜn+×n if and only if P is a monomial matrix. □

3. Positive Realizations of Transfer Functions

In this section a method for computation of positive realizations (A, B, C, D) of the given transfer function T(z) =

mn z n + mn−1 z n−1 + ... + m1 z + m0 . (3.1) z n + dn−1 z n−1 + ... + d1 z + d0

will be proposed. Using (2.4) we obtain the matrix D = lim T ( z ) = mn z →∞ and the strictly proper transfer function

T (z) = T(z) − D =

where

mn−1 z n−1 + ... + m1 z + m0 z n + dn−1 z n−1 + ... + d1 z + d0 , (3.3a)

mk = mk − mndk for k = 0,1,..., n − 1 . (3.3b)

 z − z1  −1 C[In z − A]−1 = [0  0 1]  0    0 =

since

0 z − z2 −1  0

2021

d( z ) = z n + dn−1 z n−1 + ... + d1 z + d0 = (3.4) = ( z − z1 )( z − z2 )...( z − zn )

has only nonnegative zeros z1, z2, …, zn (not necessarily distinct). In this case it can be written in the form

n n −1 n −2 n −3 n d( z ) = z − dn−1 z + dn−2 z − dn−3 z + ... + ( −1) d0 , (3.5a) where

dn−1 = z1 + z2 + ... + zn , dn−2 = z1 z2 + z1 z3 + ... + zn−1 zn  (3.5b) d0 = z1 z2 ...zn .

Lemma 3.1. If

then

 z1 1 A=0   0

0 z2 1  0

0 0 z3  0

 0 0  0 0 0 0  ∈ℜn+×n     1 zn  ,

(3.6)

C = [0  0 1]∈ℜ1+×n

1 [1 z − z1 ( z − z1 )( z − z2 )  ( z − z1 )( z − z2 )...( z − zn−1 )] ( z − z1 )( z − z2 )...( z − zn )

Proof. Using (3.6) we obtain

N° 1

It is assumed that the denominator

C[In z − A]−1 =

(3.2)

VOLUME 15,

0   0 0   0 0   0      −1 z − zn 

0 0 z − z3  0

(3.7)

−1

1 [1 z − z1 ( z − z1 )( z − z2 )  ( z − z1 )( z − z2 )...( z − zn−1 )] ( z − z1 )( z − z2 )...( z − zn )

(3.8)

C[In z − A]ad = [1 z − z1 ( z − z1 )( z − z2 )  ( z − z1 )( z − z2 )...( z − zn−1 )] and (3.7) holds. □ Theorem 3.1. There exists the positive realization  z1 1 A=0   0

0 z2 1  0

0 0 z3  0

 0 0  0 0 0 0  ∈ℜn+×n ,     1 zn 

 b1  B =  b2  ∈ℜn+ ,   bn 

of the transfer function (3.1) if the denominator (3.4) has nonnegative zeros z1, z2, …, zn, mk ≥ 0, k = 0,1,..., n – 1 (at least one mk > 0) and mn ≥ 0.

Articles

C = [0  0 1]∈ℜ1+×n ,

D = mn

(3.9)

Proof. If z k ≥ 0, k = 0,1,...,n then A ∈ℜn+×n . To simplify the notation let us assume that n = 4. Using Lemma 3.1 for n = 4 we obtain

Journal of Automation, Mobile Robotics and Intelligent Systems

 z − z1 C[I4 z − A]−1 B = [0 0 0 1]  −1  0  0 = [1 z − z1

VOLUME 15,

0 z − z2 −1 0

0 0 z − z3 −1

N° 1

0   b1  0   b2  0   b3  z − z 4  ad b4 

 b1  ( z − z1 )( z − z2 )  ( z − z1 )( z − z2 )( z − z3 )]  b2   b3  b4 

= b1 − z1 b2 + z1 z2b3 − z1 z2 z3b4 + [b2 − ( z1 + z2 )b3 + ( z1 ( z2 + z3 ) + z2 z3 )b4 ] +[b3 − ( z1 + z2 + z3 )b4 ]z 2 + b4 z 3

= m3 z 3 + m2 z 2 + m1 z + m0

and

1 − z1 0 1 0 0 0 0 

z1 z 2 − z1 − z2 1 0

− z1 z2 z3   b1  m0  z1 ( z2 + z3 ) + z2 z3   b2  = m1  . − z1 − z2 − z3   b3  m2  1  b4  m3 

(3.11)

Multiplying the second row of (3.11) by z1 and adding to its first row, then multiplying third row by z1 + z2 and adding to its second row we obtain 1 0 − z12 − z12 ( z2 + z3 )   b1  0 1 0 − z12 − ( z1 + z2 )z3   b2  = 0 0 1 − z1 − z2 − z3   b3  0 0 0  b 1   4 

 m0 + z1m1  = m1 + ( z1 + z2 )m2  m2   m3   From (3.12) it follows that

b4 = m3 ≥ 0, b3 = m2 + ( z1 + z2 + z3 )m3 ≥ 0,

(3.12)

b2 = m1 + ( z1 + z2 )m2 + [ z12 + ( z1 + z2 )z3 ]m3 ≥ 0, b1 =

z12m2

z12 ( z2

+ z3 )m3 ≥ 0

(3.13)

and the matrix B ∈ℜ4+ . By assumption D = mn ≥ 0. Therefore, there exists the positive realization (3.9) of the transfer function (3.1) if the denominator (3.4) has nonnegative zeros and mk ≥ 0, k = 0,1,...,n – 1, mn ≥ 0. □ Remark 3.1. The positive realization (3.9) is asymptotically stable (Schur) if z k < 1 for k = 0,1,...,n. Example 3.1. Compute the positive realization (3.9) of the transfer function

T(z) =

2z 3 − 2.6z 2 + 2.88z − 0.384 . (3.14) z 3 − 1.8z 2 + 1.04z − 0.192

Using (3.2) and (3.14) we obtain D = lim T ( z ) = 2 z →∞ and

(3.15)

z 2 + 0.8z = z − 1.8z 2 + 1.04z − 0.192 z( z + 0.8) = ( z − 0.4)( z − 0.6)( z − 0.8)

T (z) = T(z) − D = =

m2 z 2 + m1 z , z 3 − d2 z 2 + d1 z − d0

2021

(3.16)

(3.10)

where m2 = 1, m1 = 0.8, m0 = 0 and z1 = 0.4 , z2 = 0.6, z3 = 0.8 . In this case we have  z1 A=1  0

and 1 − z1 0 1 0 0

0 z2 1

0 0  0.4 0 0  =  1 0.6 0  , C = [0 0 1] z3   0 1 0.8  , (3.17)

z1 z2  b1  1 −0.4 0.24  b1  − z1 − z2  b2  = 0 −1  b2  = 1 1   b3  0 0 1   b3   m0   0  =  m1  = 0.8  . (3.18) m 2   1 

Solving (3.18) we obtain

b1  0.48  B = b2  =  1.8  . (3.19)  b3   1 

The positive asymptotically stable realization of the transfer function (3.14) is given by (3.17), (3.19) and (3.15). It is easy to check that the matrices  z1 A=0  0

1 z2 0

0 0  0.4 1 1  =  0 0.6 1  , z3   0 0 0.8 

0 B = 0 , C = [0.48 1.8 1] 1

(3.20)

are also the positive asymptotically stable realization of (3.16). In general case we have the following theorem. Theorem 3.2. There exists positive realization 0  z1 1 0  0 0  0 z2 1  0 A=        ∈ℜn+×n ,  0 0 0  z n −1 1  zn   0 0 0  0 0 B = 0 ∈ℜn+ ,  1

C = [c1 c2  cn ]∈ℜ1+×n , D = mn (3.21) of the transfer function (3.1) if the denominator (3.4) has nonnegative zeros z1, z2, …, zn, mk ≥ 0 , k = 0,1,...,n – 1 (at least one mk > 0) and mn ≥ 0. Articles

Journal of Automation, Mobile Robotics and Intelligent Systems

Proof. The proof is similar (dual) to the proof of Theorem 3.1.

4. Extension of the Method to MIMO Systems

In this section the method presented in section 3 will be extended to multi-input multi-output (MIMO) linear systems. To simplify the notation two-input two-output systems will be considered. The problem can be stated as follows. Given the proper transfer matrix T ( z ) = T11 ( z ) T12 ( z ) , T21 ( z ) T22 ( z ) m z n + ... + mik 1 z + mik 0 , Tik ( z ) = n ikn z + dikn−1 z n−1 + ... + dik 1 z + dik 0 i , k = 1,2 (4.1)

find its positive realization (A, B, C, D) such that

T ( z ) = C[In z − A]−1 B + D . (4.2)

Using

D = lim T ( z ) (4.3) z →∞ find the matrix D ∈ℜ2+×2 and the strictly proper transfer matrix  m11 ( z )  d (z) 1 T ( z ) = T ( z ) − D = C[In z − A]−1 B =   m21 ( z )  d (z)  2 where

m12 ( z )  d1 ( z )   m22 ( z )  d2 ( z ) 

(4.4a)

di ( z ) = z n + din−1 z n−1 + ... + di 1 z + di 0 , i = 1,2

(4.4b) is the least common denominator of Tk(z), i,k = 1,2 and zi1, zi2, …, zin, i = 1,2 are nonnegative zeros of (4.4b) and n −1 mik ( z ) = mikn−1 z + ... + mik 1 z + mik 1 + mik 0 , i = 1,2. (4.4c) The matrix A of the desired positive realization has the form

where

 A1 A= 0

   A =   

0 = blockdiag[ A1 A2 

0 0   0 0

0 0 

A2 ] , (4.5a)

0 0  0

  1   , i = 1,2 (4.5b)    0 0 in   0 0 1 in  The matrices B and C have the forms  B11 B=  B21

and

Articles

 bik 1  b  B12   ik 2  ∈ℜn+i , i , k = 1,2 (4.6a) = , B ik    B22    bikni 

VOLUME 15,

C = blockdiag[C1

C i = [0  0

N° 1

C2 ],

1]∈ℜ1+×ni

, i = 1,2 .

2021

(4.6b)

The entries of Bik, i,k = 1,2 can be calculated in the same way as the entries of B in section 3. Therefore, we have the following theorem. Theorem 4.1. There exists the positive realization given by (4.5), (4.6) and (4.3) of the transfer matrix (4.1) if

(4.7a) 1) D ∈ℜ2+×2 (defined by (4.3)); 2) The denominators (4.4b), i = 1,2 have nonnegative zeros. (4.7b) Proof. If zik ≥ 0, i = 1,2, k = 0,1,...,n then A ∈ℜ(+n1 + n2 )×( n1 + n2 ) . In a similar way as in proof of Theorem 3.1 it can be shown that B ∈ℜ(+n1 + n2 )×2 if the denominators (4.4b), i = 1,2 have nonnegative zeros. The matrix C defined by (4.4b) has always nonnegative entries. Therefore, the realization given by (4.5), (4.6) and (4.3) is positive if D ∈ℜ2+×2 . □ If the conditions of Theorem 4.1 are satisfied then the following procedure can be used for computation of the positive realization for given transfer matrix (4.1). Procedure 4.1. Step 1. Knowing T(z) and using (4.3) and (4.4a) compute the matrix D and the strictly proper transfer matrix T ( z ). Step 2. Compute the zeros zij, i = 1,2, j = 1,...,n of the polynomials (4.4b) and find the matrix (4.5). Step 3. Using the procedure presented in section 3 compute the matrix B ∈ℜ(+n1 + n2 )×2 . Step 4. Write the desired positive realization given by (4.5), (4.6) and (4.3). Example 4.1. Compute the positive realization of the transfer matrix  2z 2 − 1.2z + 1   2  T ( z ) =  z − 1.1z + 0.3  . (4.8)  3z + 0.8    z − 0.4

Using Procedure 4.1 we obtain the following: Step 1. Using (4.3), (4.2) and (4.8) we obtain z + 0.4    z 2 − 1.1s + 0.3  T (s) = T(s) − D =   2     − z 0.4 and . Step 2. The zeros of the polynomial

d1 ( z ) = z 2 − 1.1z + 0.3

(4.9) (4.10)

(4.11a)

are z11 =0.5, z12 =0.6 and the polynomial d2 ( z ) = z − 0.4

has only one zero z21 =0.4. Therefore, the matrix A has the form  A1 A= 0

(4.11b)

0 0.5 0 0  = 1 0.6 0  . (4.12) A2    0 0 0.4 

Journal of Automation, Mobile Robotics and Intelligent Systems

Step 3. In this case

 B1  b11  B =   , B1 =   , B2 = [b13 ]  B2   b12  and

(4.13a)

−1

1 − z11  m10  1 0.5 0.4  0.9 = = B1 =  , 1  m11  0 1   1   1  0 B2 = 2. (4.13b) Thus, we have

0.9 C1 B =  1  and C =  0  2 

0  0 1 0 = . C2  0 0 1 (4.14)

Step 4. The desired positive realization of (4.8) is given by (4.12), (4.14) and (4.9). It is easy to verify that the matrices

0 0.5 0 0 A =  1 0.6 0  , B = 1 ,  0 1 0 0.4 0.9 1 0 2 C = , D=    0 0 2 3

(4.15)

are also the (dual) positive realization of (4.8). Remark 4.1. To the presented method the dual method based on the least common denominator for each column of T(z) can be also applied. Remark 4.2. By Theorem 2.3 if the matrices A, B, C, D are a positive realization of T(z) then the matrices PAP–1, PB, CP–1, D are also its positive realization for any monomial matrix P.

5. Conclusion

A new method for computation of positive realizations of transfer matrices of discrete-time linear systems has been proposed. Sufficient conditions for the existence of the positive realizations have been established (Theorems 3.1, 3.2 and 4.1). A procedure for computation of the positive realizations has been proposed and illustrated by a numerical example (Example 4.1). The presented method can be extended to linear fractional systems.

AUTHOR Tadeusz Kaczorek – Faculty of Electrical Engineering, Białystok University of Technology, Wiejska 45D, 15-351 Białystok, Poland, e-mail: kaczorek@ee.pw. edu.pl.

VOLUME 15,

N° 1

2021

References [1] L. Benvenuti and L. Farina, “A tutorial on the positive realization problem”, IEEE Transactions on Automatic Control, vol. 49, no. 5, 2004, 651-664, 10.1109/TAC.2004.826715.

[2] L. Farina and S. Rinaldi, Positive Linear Systems: Theory and Applications, Wiley-Interscience, 2000. [3] T. Kaczorek, “A modified state variable diagram method for determination of positive realizations of linear continuous-time systems with delays”, International Journal of Applied Mathematics and Computer Science, vol. 22, no. 4, 2012, 897-905, 10.2478/v10006-012-0066-x.

[4] T. Kaczorek, “A new method for determination of positive realizations of linear continuous-time systems”, Bulletin of the Polish Academy of Sciences: Technical Sciences, vol. 66, no. 5, 2018, 605611, 10.24425/BPAS.2018.124276. [5] T. Kaczorek, “A realization problem for positive continuous-time systems with reduced numbers of delays”, International Journal of Applied Mathematics and Computer Science, vol. 16, no. 3, 2006, 325-331.

[6] T. Kaczorek, “Computation of positive stable realizations for linear continuous-time systems”, Bulletin of the Polish Academy of Sciences: Technical Sciences, vol. 59, no. 3, 2011, 273-281, 10.2478/v10175-011-0033-y. [7] T. Kaczorek, “Computation of realizations of discrete-time cone-systems”, Bulletin of the Polish Academy of Sciences: Technical Sciences, vol. 54, no. 3, 2006, 347-350. [8] T. Kaczorek, “Existence and determination of the set of Metzler matrices for given stable polynomials”, International Journal of Applied Mathematics and Computer Science, vol. 22, no. 2, 2012, 389-399, 10.2478/v10006-012-0029-2. [9] T. Kaczorek, Linear Control Systems: Analysis of Multivariable Systems, Wiley, 1992.

[10] T. Kaczorek, Polynomial and Rational Matrices, Springer London, 2007, 10.1007/978-1-84628605-6. [11] T. Kaczorek, Positive 1D and 2D Systems, Springer-Verlag, 2002, 10.1007/978-1-4471-0221-2.

[12] T. Kaczorek, “Positive minimal realizations for singular discrete-time systems with delays in state and delays in control”, Bulletin of the Polish Academy of Sciences: Technical Sciences, vol. 53, no. 3, 2005, 293-298. Articles

Journal of Automation, Mobile Robotics and Intelligent Systems

[13] T. Kaczorek, “Positive stable realizations of fractional continuous-time linear systems”. In: Proc. Conf. Int. Inf. and Eng. Syst., 2012.

[14] T. Kaczorek, “Positive stable realizations for fractional descriptor continuous-time linear systems”, Archives of Control Sciences, vol. 22, no. 3, 2012, 303–313, 10.2478/v10170-011-0026-y.

[15] T. Kaczorek, “Positive stable realizations with system Metzler matrices”, Archives of Control Sciences, vol. 21, no. 2, 2011, 167-188, 10.2478/ v10170-010-0038-z.

[16] T. Kaczorek, “Realization problem for positive fractional continuous-time systems”. In: 2008 16th Mediterranean Conference on Control and Automation, 2008, 1008-1015, 10.1109/ MED.2008.4602000. [17] T. Kaczorek, “Realization problem for positive 2D hybrid systems”, COMPEL – The International Journal for Computation and Mathematics in Electrical and Electronic Engineering, vol. 27, no. 3, 2008, 613-623, 10.1108/03321640810861061.

[18] T. Kaczorek, “Realization problem for positive discrete-time systems with delays”, Systems Science, vol. 30, no. 4, 2004, 17-30.

[19] T. Kaczorek, “Realization problem for positive multivariable discrete-time linear systems with delays in the state vector and inputs”, International Journal of Applied Mathematics and Computer Science, vol. 16, no. 2, 2006, 169-174.

[20] T. Kaczorek, Selected Problems of Fractional Systems Theory, Springer Berlin Heidelberg, 2011, 10.1007/978-3-642-20502-6. [21] T. Kaczorek and M. Busłowicz, “Minimal realization for positive multivariable linear systems with delay”, International Journal of Applied Mathematics and Computer Science, vol. 14, no. 2, 2004, 181-187.

[22] T. Kaczorek and L. Sajewski, The Realization Problem for Positive and Fractional Systems, Springer International Publishing, 2014, 10.1007/978-3319-04834-5.

Articles

VOLUME 15,

N° 1

2021

Journal of Automation, Mobile Robotics and Intelligent Systems

VOLUME 15,

N° 1

2021

Continuous-Curvature Trajectory Planning Submitted: 3rd May 2020; accepted: 14th March 2021

Jörg Roth

DOI: 10.14313/JAMRIS/1-2021/2 Abstract: Continuous-curvature paths play an important role in the area of driving robots: as vehicles usually cannot change the steering angle in zero-time, real trajectories must not have discontinuities in the curvature profile. Typical continuous-curvature paths are thus built of straight lines, arcs and clothoids. Due to the geometric nature of clothoids, some questions in the area of trajectory planning are difficult the answer – usually we need approximations here. In this paper we describe a full approach for continuous-curvature trajectory planning for mobile robots – it covers a maneuver-based planning with Viterbi optimization and geometric approximations required to construct the respective clothoid trajectories. Keywords: Mobile Robots, Trajectory Planning, Continuous-curvature Paths, Clothoids

Fig. 1 shows an example: we see a trajectory sequence and the curvature κ of the driving distance s. Straight driving (L) have zero and arcs (A) have a constant non-zero curvature. Clothoids (C) have a constant derivate of κ. We are able to create sequences of these three primitive trajectories L, A, C without any discontinuity in curvature. We could think of more complex primitive trajectories that steadily change the curvature, however, the clothoid is the most ‘natural’ trajectory with this property. C L

s4 C s5

κ1

1/κ2

1. Introduction

s6 C

C L

Trajectory planning is a fundamental function of a mobile robot. When executing tasks such as transporting goods, the robot has to drive complex trajectories that meet certain measures of optimality. For this, a cost function may consider driving time, energy consumption, mechanical wear or buffer distance to obstacles. Whereas a geometric route planning tried to find a line string with minimal costs that does not cut an obstacle (with respect to the robot’s driving width), the trajectory planning also considers nonholonomic constraints such as curve angles or orientations. Non-holonomic constraints prohibit, e.g., to move sideways or to rotate the body while driving straight. An important constraint is related to curvature: real motion systems can change the steering angle and thus the current curvature only with finite speed. As a result, the change of, e.g., straight driving to driving a curve is not possible instantly. The key approach to create continuous-curvature trajectory sequences is to incorporate clothoid segments – they linearly change the curvature and thus are able to, e.g., connect straight driving and arc trajectories.

s2 A s3

1/κ1

s6 s

s 4 s5 C

κ2

Fig. 1. A continuous-curvature trajectory sequence We have to face the following challenges when we want to compute continuous-curvature paths based on clothoids: • The planning complexity increases, when we increase the configuration dimensions. For continuous-curvature paths we have start, target and intermediate configurations with four parameters (x, y, θ, κ). • We want to consider arbitrary cost functions, not only the length or maximum curvature. Ideally the cost function is represented as functional black box, queried at runtime to compute cost values. In addition, paths have to consider obstacles that already have been perceived by the robot. • In contrast to straight driving and arcs, clothoids are not described by simple, closed formulas. For direct functions (e.g., the position for a driving distance), there exist well-known approximations. For reverse functions (e.g., given a position, what is the driving distance to this position) approximations are often not known. 9

Journal of Automation, Mobile Robotics and Intelligent Systems

Our approach to compute continuous-curvature paths covers the whole range from basic approximation functions up to the overall planning of trajectories. We consider arbitrary cost functions, passed as runtime function. We use a maneuver approach with Viterbi optimization to compute obstacle-free paths with least costs. We suggest different runtime-optimized approximations to compute the required clothoid functions.

2. Related Work We can split related work in approaches that deal with the general problem of trajectory planning and specific problems related to continuous-curvature trajectory planning. A general approach may also create continuous-curvature paths, if the curvature is part of the configuration and the derivate of curvature can be considered in the planning algorithm. Early work investigated shortest paths for vehicles that are able to drive straight forward and circular curves [3, 5, 18]. Without obstacles, we can connect two configurations with only three primitive trajectories, the so-called Dubins paths. Dubins path have discontinuities in the curvature as they instantly change between left and right arcs or arcs and linear driving. More related to our approach is work that investigates longer paths that go through an environment of obstacles. As the space of possible trajectory sequences gets very large, probabilistic approaches are a suitable method to find at least a suboptimal solution [8, 10, 11]. They randomly connect configurations by primitive trajectories and are able to search on the respective graph to plan an actual path. Further work used potential fields [1] or visibility graphs [16]. With the help of geometric route planners, the overall problem of trajectory planning can be reduced. In [9], the route planning step and a local trajectory planning step are recursively applied. Random sampling can also be used to improve generated trajectories. E.g., CHOMP [25] uses functional gradient techniques based on Hamiltonian Monte Carlo to iteratively improve the quality of an initial trajectory. The approach in [15] represented the continuous-time trajectory as a sample from a Gaussian process generated by a linear time-varying stochastic differential equation. Then gradient-based optimization technique optimizes trajectories with respect to a cost function. Further planning approaches are based on the state lattice idea introduced in [17]. State lattices are discrete graphs embedded into the continuous state space. Vertices represent states that reside on a hyperdimensional grid, whereas edges join states by trajectories that satisfy the robot’s motion constraints. The original approach is based on equivalence classes for all trajectories that connect two states and perform inverse trajectory generation to compute the result trajectory. [7] introduced a two-step approach, with

Articles

VOLUME 15,

N° 1

2021

coarse planning of states based on Dynamic Programming, and a fine trajectory planning that connects the formerly generated states. Further work investigated the mathematics behind clothoids and characteristics of clothoids in paths. [2] discovered an important property: if we only consider the path length as costs, then optimal paths may have an infinite number of switching points, i.e., points that change between different primitive trajectories. Apart from trivial paths, sequences with minimal lengths may have an infinite number of clothoid arcs. This finding is important, because real paths (with a finite number of primitive trajectories) are thus usually not optimal. This is contrarily to Dubins paths, where we do not consider curvature. [2] does not consider obstacles or cost functions other than the length. [24] minimized the maximum curvature when constructing clothoids. Up to a certain point their approach provided closed formulas, however, when considering the so-called Fresnel integral (see later), they switched to approximations with fractions of quadratic and cubic polynoms. With this, maneuvers of type CA (see later) can be computed. [13] focused on certain geometric problems when construction clothoids, e.g., clothoids that connect circles. They provide approximation functions to compute clothoid parameters. [14] approximated clothoids by arc splines that are more suitable for some geometric questions. Instead of clothoids, also cubic spirals or splines were considered [4, 12]. [6] took existing planned trajectory sequences and converted them to continuous-curvature sequences. The input sequence only contains linear driving and turning in place. Their approach: each turn in place is transformed to a pair of clothoids that result in the same target configuration. This approach has a certain benefit: planning of the input trajectory sequence with the help of turn-in-place trajectories is very simple, thus this special continuous-curvature sequence can efficiently planned as well. However: replacing turn in place by clothoids may cause large loops that move the robot far away from the planned path. As a result, the robot may collide with obstacles. [22] introduced so-called Simple Continuous Curvature Paths (SCC) and constructed every larger path from SCCs. SCCs contain 8 primitive trajectories. In our notion they are of type CAC-0LCAC-0 (see later). An observation: related work either focused on short clothoid paths without the consideration of different cost functions, or tried to construct a complex paths of a small set of what we later call maneuvers. We strongly believe, the cost function may not only consider the length or maximum curvature, but may be more complex. In particular, we have to deal with obstacles and maybe want to integrate a safety distance to the cost function. We also believe that suitable paths may contain numerous combinations of primitive trajectories, not only a small set of maneuvers.

Journal of Automation, Mobile Robotics and Intelligent Systems

VOLUME 15,

3. The Trajectory Planning Approach 3.1. The Planning Architecture The goal is to provide a mechanism to move a robot from one configuration (x, y, θ, κ) to a target configuration, meanwhile holding the constraint of continuous curvature. We start with the overall architecture of the motion planning and execution (Fig. 2). The application specifies the motion tasks to move to a certain target. A target may define only a position, but also a target orientation or curvature. The Navigation component provides a point-to-point route planning in the workspace. This component does not consider non-holonomic constraints. It computes a line string of minimal costs that avoids obstacles. The Trajectory Planning computes a drivable sequence of trajectories between configurations and considers non-holonomic constraints. As the Navigation already detected a collision-free line string, this component does not have to check for any trajectory sequence between the configurations, but looks for trajectories that connect the turning points. This twophase approach significantly reduces the overall computation complexity. Motion Task

Navigation

Evaluator

Start, Target Conf. Route Points

Obstacle Map

Trajectory Planning Trajectories

Trajectory Regulation Slippage Factors

Slippage Compensation

Driving Commands

Current Pose

SLAM Sensor Input

Odometry

Trajectory Execution

Motion System

Sensors

Fig. 2. Planning and execution data flow

Fig. 2. Planning and execution data flow The Evaluator computes costs of routes and trajectories based on the obstacle map and the desired properties. Cost values may take into account the path length, expected energy consumption or the amount back-driving. Also, the distance to obstacles could be considered, if, e.g., we want the robot to keep a safety distance where possible. As an important decision: the planning approach is fully separated from the evaluation approach that even may change its behaviour at runtime. The lower components are not focus of this paper: the Trajectory Regulation permanently tries to hold the planned trajectories, even if the position drifts off. Simultaneous Localization and Mapping (SLAM) constantly observes the environment and computes the most probable own location and location of obstacles by motion feedback and sensors (e.g., Lidar or camera). The current error-corrected configuration

N° 1

2021

is passed to all planning components. Observed and error-corrected obstacle positions are stored in an Obstacle Map for further planning tasks. The Motion System finally is able to execute and supervise driving commands by formalized trajectories.

3.2. Trajectories and Maneuvers

We now assume the robot drives in the plane in a workspace W with positions (x, y). The configuration space C covers additional dimensions for orientation angle θ and curvature κ, i.e., our configuration space is of (x, y, θ, κ). The goal is to find a collision-free sequence of trajectories that connects two configurations, meanwhile minimizes a cost function. The overall problem has many degrees of freedom. Whereas even small distances can be connected by an infinite number of trajectories, the problem gets worse for larger environments with many obstacles. We thus introduce the following concepts: • A route planning solely operates on workspace W and computes a sequence of collision-free lines of sight (with respect to the robot’s width) that minimize the costs. • As the route planning only computes route points in W, we have to specify additional values in C. From the infinite assignments, we only consider a small finite set. • From the infinite set of trajectories between two route configurations, we only consider a finite set of maneuvers. Maneuvers are trajectories, for which we know formulas that derive the respective parameters (e.g., curve radii) from start and target configurations. • Even though these concepts reduce the problem space to a finite set of variations, this set would by far be too large for a complete iteration. We thus apply a Viterbi-like approach that significantly reduces the number of checked variations to find an optimum. We carefully separated the cost function from all planning components. We assume there is a mapping from a route or trajectory sequence to a cost value according to two rules: first, we have to assign a single, scalar value to a trajectory sequence that indicates its costs. If costs cover multiple attributes (e.g., driving time and battery consumption), the cost function has to weight these attributes and create a single value. Second, a collision with obstacles has to result in infinite costs. The basic capabilities of movement are defined by a set of primitive trajectories. The respective set can vary between different robots. E.g., the Carbot [21] is able to execute the following primitive trajectories: • L(): linear (straight) driving over a distance ; • A(, r): circular arc with radius r over a distance ; • C(, κs, κt): clothoid over a distance  with given start and target curvatures.

Articles

Journal of Automation, Mobile Robotics and Intelligent Systems

Further primitive trajectories may be possible, e.g., to turn in place, however, we are only interested in continuous-curvature trajectories in this paper. We are able to map primitive trajectories directly to driving commands that are natively executed by the robot’s motion system. Implicitly, they specify functions that map two configurations cs to ct. Due to non-holonomic constraints, it usually is not possible to reverse this mapping. I.e., for given cs, ct∈ C there is in general no primitive trajectory that maps cs to ct. At this point, we introduce maneuvers. Maneuvers are small sequences of primitive trajectories (usually up to 10 elements) that are able to map between given cs, ct∈ C. More specifically: • A maneuver is defined by a sequence of primitive trajectories (e.g., denoted ALA or LCA) and further constraints. Constraints may relate or restrict the respective primitive trajectory parameters. • For given cs, ct∈ C there exist formulas that specify the parameters of the involved primitive trajectories, i.e.,  for L, A and C, r for A, κs, κt for C. • Usually, the respective equations are underdetermined. As a result, multiple maneuvers of a certain type (sometimes an infinite number) map cs to ct. Thus, we need further parameters we call free parameters to get a unique maneuver. • We further have maneuvers that do not consider all start and target configuration parameters. E.g., some maneuvers drive to a certain target position, but the target orientation cannot be specified beforehand. We call the specified configuration parameters for a maneuver start and target parameters. At least (x, y, θ) must be start parameters and workspace dimensions (x, y) must be target parameters. In addition, continuous-curvature maneuvers must accept (x, y, θ, κ) as start parameters.

Clotho-ACL-nodir (A C0 L)

S-Clothoid-Arcs (0C C-0 0C C-0)

Clothoid-Arcarc (0C C-0 0C C-0)

C- -Clothoid-Arcs (C0 0C C-0 L 0C C-0)

C-Wing-Clothoid-Arc (C0 L 0C C-0 L)

Clotho-0C (0C)

Clotho-LCA (L 0C A)

VOLUME 15,

Dubins-Clothoid-Arcs (0C C-0 0C C-0 0C C-0)

Clotho-C00C (C0 0C)

Fig. 3. Sample continuous-curvature maneuvers Until now we identified about thirty maneuvers. Fig. 3 shows ten of them that support continuous-curvature paths. The challenge is to set up formulas for the trajectory parameters. For the non-continuous curvature planning, the formulas usually are a result of solving a linear or quadratic equation system – for all we 12

Articles

2021

get closed solutions. For continuous-curvature planning, however, we have clothoid functions that results in approximations. We come back to this point later.

3.3. Notation of Maneuvers

In order to systematically describe the numerous maneuvers, we introduced a special notation. This is not only useful to precisely describe the maneuver’s structure to developers – is can be used to check basic properties without to know the underlying formulas. These checks cover: • Checks when constructing maneuvers, whether they are properly defined and integrated into the overall planning system. • Checks that support the developer of a robotapplication to select appropriate maneuvers for a certain situation or vehicle. • Runtime checks, whether two subsequent maneuver match, i.e., can be driven after each other, while certain driving properties are fulfilled. For this, we developed the notion of structure patterns. They are built from the primitive trajectory (A, L and C) and modifiers that set a primitive trajectory in relation to its neighbour. Table 1 shows all elements. For A we introduced the modifiers A~ and A= that relate the arc’s turning orientation to a former arc. This is useful to indicate maneuvers that first drive a right curve, then left curve or vice versa (A~) or twice right or twice left curve (A=). For clothoids we know more modifiers. First we may indicate a zero-curvature at start (0C) or termination (C0). Second, we may indicate a symmetric clothoid as described later in section ‎4.2 (C-) that also may have zero-curvature at termination (C0).

Tab. 1. Maneuver notation elements Notation

Meaning

Linear

Arc with opposite turning orientation to last A

Clothoid

Arc

Arc with same turning orientation to last A

Clothoid with zero start curvature

C-

Exact symmetric clothoid to last C

C-0 Clotho-ACLCA (A C0 L 0C A)

N° 1

Clothoid with zero end curvature

Exact symmetric clothoid to last C with zero end curvature

The maneuvers we found so far are listed in Table 2. Besides the structure patterns, the table presents start, target and free parameters. For start parameters we can distinguish the following cases: • (x, y, θ): maneuvers that do not predefine a start curvature: such maneuvers usually change the curvature at start, thus are not suitable for continuous curvature planning; • (x, y, θ, 0): maneuvers that assume a zero-curvature at start: such maneuvers can continue a trajectory that ends with zero-curvature;

Journal of Automation, Mobile Robotics and Intelligent Systems

VOLUME 15,

N° 1

2021

Tab. 2. Maneuvers Name

Start Params

Target Params

Free Params

Continuous Curvature

(x, y, θ)

(x, y)

yes

L A

(x, y, θ, 0)

(x, y, θ)

(x, y, θ, κ)

(x, y, θ, 0)

 of 1 C

yes

C C-0 L

(x, y, θ, 0)

yes

A L A

(x, y, θ)

C-Bow Clothoid-Bow J-Bow

L 0C C-0

J-Clothoid-Bow

C0 L C C-0

J-Bow2

J-Clothoid-Bow2

Arcarc

(x, y, θ, 0)

A A=

(x, y, θ)

C0 C C-0 L C C-0 0

C C-0 0C C-0

Dubins-Arcs Dubins-Clothoid-Arcs Clotho-0C Clotho-C00C Clotho-C Clotho-CL Clotho-ACL-nodir Clotho-ACL Clotho-LCA Clotho-CLCA Clotho-ACLCA

yes

 of 1 C

r1 of 1st arc, r2 of 2nd A

r1 of 1st A, r2 of 2nd A (*)

yes

(x, y, θ, κ)

(x, y, θ, 0)

 of 1 C, r1 of 1 A, r2 of 2 A

(x, y, θ, 0)

r1 of 1st A, r2min... r2max of 2nd A (*)

yes

(x, y, θ)

(x, y, θ, 0)

(x, y, θ) (x, y, θ)

(*)

r1 of 1st A, r2min... r2max of 2nd A -

(x, y, θ, 0)

yes

(x, y, θ, κ)

(x, y, θ, 0)

C C-0 0C C-0 L

(x, y, θ, 0)

r of 1st A (=r of 2nd A) (*)

yes

L 0C C-0 0C C-0

(x, y, θ, 0)

r of 1st A (=r of 2nd A) (*)

yes

(x, y, θ, 0)

r of 1st A (=r of 2nd and 3rd A) (*)

yes

(x, y, θ, κ)

(x, y)

L A L

L 0C C-0 L 0

C0 L C C-0 L A A~ L

L A A~

A A~ A~

C C-0 0C C-0 0 C C-0 0

(x, y, θ, 0) (x, y, θ)

(x, y, θ, 0)

(x, y, θ, κ)

A C0 L

(x, y, θ, κ)

C0 C C0 L

A C0 L 0

(x, y, θ)

(x, y, θ) (x, y)

(x, y)

(x, y, 0)

(x, y, θ, κ)

(x, y, θ, 0)

(x, y, θ, κ)

(x, y, θ, 0)

A C0 L 0C A

(x, y, θ, κ)

C0 L C A

(x, y, θ, 0)

(x, y, θ, κ)

L C A 0

(x, y, θ, 0)

r of A

yes

(x, y, θ, 0)

Wing-Clothoid-Arc

Snake2-Clothoid-Arcs

(x, y, θ)

yes

(x, y, θ, 0)

Wing-Arc

Snake2

A A~

C C-0 0C C-0

S-Clothoid-Arcs

Snake-Clothoid-Arcs

(x, y, θ, 0)

C C-0 L 0C C-0

S-Arcs

Snake

(x, y, θ)

(x, y, θ, 0)

Clothoid-Arcarc

C-Wing-Clothoid-Arc

C0 C C-0 L

∫-Arcs C-∫-Clothoid-Arcs

A L

(x, y, θ, 0)

(x, y)

(x, y, θ, κ)

C-J-Clothoid-Bow2 ∫-Clothoid-Arcs

(x, y, θ)

C C-

C-J-Clothoid-Bow

Suitable for Continuous Curvature Planning

Structure Pattern

r of A (*) st

 of 1 C, r of A

r of 1st A (=r of 2nd A) r of 1st A (=r of 2nd A) st

(x, y, θ, κ)

• (x, y, θ, κ): maneuvers that may continue any trajectory that ends with an arbitrary non-zero curvature. For target parameters we distinguish (x, y), (x, y, θ), (x, y, 0), (x, y, θ, 0), and (x, y, θ, κ). All are suitable for inner maneuvers (i.e., accept for the last), even for continuous-curvature planning. For the last maneuver it depends, whether the application requires that the robot holds a certain orientation and/ or curvature at the target position. E.g., there may be a charging station that only can be used with a certain robot orientation. It is not likely for an application to request a target curvature. However, it could, e.g., be required that the steering angles of the wheels must be zero when entering the charging station.

r of 1 A (=r of 2 and 3 A) -

 of 1 C

yes

 of 1 C

yes

(x, y, 0)

(x, y, θ, κ)

(*)

yes

We have manifold free parameters. Most are arc radii and clothoid lengths. Some free parameters are marked with (*): these are a result of transforming non-clothoid maneuvers as described in section 4 ‎ .2. The penultimate column of Table 2 indicates whether the connected primitive trajectories have a continuous-curvature. These are single trajectory maneuvers (e.g., only an A) or each pair of subsequent trajectories are of AC, AC0, L0C, C0L, C-0L, CC-, 0 CC-0, or C00C. For continuous-curvature planning (last column) we further require both to have a continuous-curvature maneuver and start parameters that cover the curvature (0 or κ).

Articles

Journal of Automation, Mobile Robotics and Intelligent Systems

3.4. Finding Optimal Maneuver From all valid sequences, weSequences look for an optimal se-

whereas optimality was assessed by the Ourquence, planning approach both covers non-continEvaluator component (Fig. 2) that provides an evaluauous and continuous-curvature trajectory planning. tion function costs(Mi(pi)). The problem is a typical opHere, we describeproblem: the latter We start from a contimization wecase. look for figuration (xs, ys, θs, κs) and plan to a target (xt, yt, θt, κt), whereas θt M and/or ‘any argalso min be * for costs ( M target i , pi  κt may j ( p j )) ori- (1) ( M j(resp.) ) n1 , pmay entation or curvature j P ( Mbe j ), suitable’. ( M i ( p j )) is valid Let Π denote the set of maneuvers that are suitaThe set  is finite, moreover small, thus we could conble for continuous-curvature planning as described sider to iterate over all (Mj)n-1. However, even for in Table 2. Let P(M) an M∈Π be the start, target is small n, the totalfor number of possible combinations and free parameters. M(p) denotes getting very large. E.g.,for forp∈P(M) ||=22 (Table 2) a conand n=10 route points get more than a billion possible secrete maneuver, i.e.,we a specific sequence of trajectories quences (Mj). Maneuver Consider parameters (e.g.,toorientabetween given ofconfigurations. M(p) be tion and curvature of inner routing points) areM.even a maneuver instance according to a maneuver class more crucial, as P(Mj) are continuous values with a Let large (xs, ysnumber ) = (x1, yof (x2, y2), …, (xMoreover, , yt) de1), dimensions. n, yn) = (xtthere is no note a close collision-free found byofthe relation ofroute nearby values P(MNavigation ) and the rej sulting(Fig. costs2). value, even slightest modifications component Our i.e., problem is to find a valid se- of significantly the costs. Thus, quenceparameters (Mi(pi)) of may maneuvers. Validchange means: optimization approaches as hill climbing or simulated • M1(p 1) starts with the configuration (xs, ys, θs, κs) annealing will fail. and MnOur (pn)approach terminates (xs, ys, θby s, κthe s); Viterbi algo[19]with is inspired • Mi(prithm thebasic positions [23]. The ideas:(xi, yi), (xi+1, y i+1); i) connect Of the infinite of the maneuver parameters • Mi(pi) and Mi+1 (pi+1)number match at connection point we a finite set of promising candidates. This (xi+1, y define i+1), i.e., they have the same orientation θ i+1 obviouslyκ leads to sub-optimal results, however, and curvature i+1. opens the possibility to use a discrete optimization From all valid sequences, we look for an optimal seapproach. quence, whereas optimality was assessed by the EvalWe do not try to optimize all maneuvers at once, what would to a provides huge number of variations. uator component (Fig.lead 2) that an evaluation Instead,i(pwe locally optimize two maneuvers that function costs(M i)). The problem is a typical optimiare connected a route point. zation problem: we lookbyfor

This approach is suitable, because optimal paths have Mi , pi = arg min costs( M j ( p j )) (1) a characteristic: the interference between two trajec( M j )∈Π n−1 , p j ∈P ( M j ), tories in that depends on their distance. If they ( Mi ( ppath j )) is valid a change are close, a change of one usually also causes theΠother. If they are far, we may thus change trajecTheofset is finite, moreover small, weone could tory of the sequence, without affecting the other. Vin-1 . However, consider to iterate all (Mj) ∈ Π as terbi reflects over this characteristic, it checks even all comfor small n, the total number of possible combinations binations of neighbouring (i.e., close) maneuvers to get the optimum. is getting very large. E.g., for |Π| = 22 (Table 2) and Fig.points 4 illustrates themore approach. begin with n =10 route we get thanWe a billion pos-the route points discovered by Navigation component. sible sequences of (Mj). Maneuver parameters (e.g., Step 1 varies orientation 2 and curvature 2 of route orientation and curvature of inner routing points) point (x 2, y2). For each variation, we look for the best are even more crucial, P(M arethis continuous values maneuver to get as there – j)for we vary the maneuwith a large number of dimensions. there vers and its parameters. As a Moreover, result we get |2iis||2j| best maneuvers. no close relation of nearby values of P(Mj) and the reStep 2 is the for all further steps. Now, sulting costs value, i.e.,pattern even slightest modifications of all best maneuvers that are collected so far are combined parameters may significantly change theto costs. with all matching maneuvers to get (x3, yThus, 3), meanoptimization approaches as hill climbing or simulated while varying 3 and 3. For short a time the algorithm annealing will fail.|2i||2j||3i||3j| permutations. However of deals with we only keep the best by onethe for Viterbi each 3,algo3. As a Ourthese, approach [19] is inspired result, step 2 terminates rithm [23]. The basic ideas: with |3i||3j| best maneuver sequences. • Of theWe infinite iteratenumber throughofallmaneuver inner routeparameters points, meanwe define a finite set of promising candidates. This while always keeping a set of current best maneuvers.

obviously leads to sub-optimal results, however, opens the possibility to use a discrete optimization approach. • We do not try to optimize all maneuvers at once, what would lead to a huge number of variations. Instead, we locally optimize two maneuvers that are connected by a route point. This approach is suitable, because optimal paths have a characteristic: the interference between two trajectories in that path depends on their distance.

Articles

VOLUME 15,

Route

(xs, ys, θs, s)

(xt, yt, θt, t)

(x2, y2)

Step 1 (xs, ys, θs, s) (x2, y2) best maneuver to (x2, y2, θ2i1, 2j1)

Step 2

2j2 (xs, ys, θs, s)

2j1

θ2i2 (x2, y2)

θ2i1

...

a total of |θ2i||2j| best maneuvers

θ3i2

a total of |θ3i||3j| best maneuvers

best maneuver to (x2, y2, θ2i2, 2j2)

3j1

θ3i1

3j2 best maneuver to (x2, y2, θ3i1, 3j1) extending one best of the first step

...

2021

Target

(x3, y3)

Start

N° 1

...

best maneuver to (x2, y2, θ3i2, 3j2) extending one best of the first step

cross-check all maneuvers of the previous step with all possible maneuvers to get to the respective (x3, y3, θ3i, 3j)

Last Step

Target

(xt, yt, θt, t)

best maneuver to (xt, yt, θt, t) extending one best of the previous step

a total of 1 best maneuver

cross-check all maneuvers of the previous step with all possible maneuvers to get to (xt, yt, θt, t)

Fig. 4. Idea of Viterbi optimization of trajectories If theare target configuration orientation and If they close, a change ofcontains one usually also causes curvature, the last step is different, as we do not have a change the other. If they are far, may change to vary of them. Thus, only a single bestwe maneuver reonemains. trajectory sequence, the If one of of the orientation and without curvatureaffecting are *, then the Viterbi respective candidate set as selectedasfor inner other. reflects this characteristic, it checks points is checked. all route combinations of neighbouring (i.e., close) maneuWe have to clarify the candidate sets for vers to getfinally the optimum. maneuver parameters: 4 illustrates the approach. We begin with the Fig. For target orientations we consider variations of route angles pointsfrom discovered by Navigation the previous and to the component. next route is suitable, as curvature optimal trajectories Step 1point. variesThis orientation θ2 and κ2 of route the basic route directions. point mainly (x2, y2).follow For each variation, we look for the best  For arc we consider some of the maneuver to radii get there – for this wemultipliers vary the maneuminimum curve radius rmin, e.g., {rmin, 3rmin, 5rmin}. vers parameters. As a result we getzero |θ2i|⋅|κ  and For its target curvatures we consider and2j| best maneuvers. reciprocals of candidates for arc radii. Step Candidates clothoid are computed from 2 is the for pattern forlengths all further steps. Now, all the vehicle'sthat maximum curvature change and canbest maneuvers are collected so far are combined didates for curvatures. with all matching maneuvers to get to (x3, y3), meanwhile varying θ and κ3. Forparameters short a time thedepend algorithm 3 Note that configuration that on deals with |θ |⋅|κ |⋅|θ |⋅|κ | permutations. However 2i 2j 3i 3j other maneuver parameters implicitly define candidates for certain step.the E.g., Clotho-0C not of these, wea only keep best one fordoes each θ3accept , κ3. As target step orientation and curvature. these a result, 2 terminates with |θ3iThis |⋅|κ3j|means, best maneudepend on other parameters such as start and vervalues sequences. target position. In particular, these parameters are not We iterate through all inner route points, meanwhile always keeping a set of current best maneuvers. If the target configuration contains orientation and curvature, the last step is different, as we do not have to vary them. Thus, only a single best maneuver remains. If one of orientation and curvature are *, then the respective candidate set as selected for inner route points is checked. We finally have to clarify the candidate sets for maneuver parameters:

Journal of Automation, Mobile Robotics and Intelligent Systems

VOLUME 15,

• For target orientations we consider variations of angles from the previous and to the next route point. This is suitable, as optimal trajectories mainly follow the basic route directions. • For arc radii we consider some multipliers of the minimum curve radius rmin, e.g., {rmin, 3⋅rmin, 5⋅rmin}. • For target curvatures we consider zero and reciprocals of candidates for arc radii. • Candidates for clothoid lengths are computed from the vehicle’s maximum curvature change and candidates for curvatures. Note that configuration parameters that depend on other maneuver parameters implicitly define candidates for a certain step. E.g., Clotho-0C does not accept target orientation and curvature. This means, these values depend on other parameters such as start and target position. In particular, these parameters are not free. If in one step, such maneuvers are selected as best maneuver, the respective parameters are implicit candidates for the next step, even though not mentioned in the list above.

N° 1

2021

For the normalized Euler spiral we set x(s) = C(s) and y(s) = S(s). To produce a clothoid with a certain curvature ∆κ at running length , we need a factor a Then

∆κ 2

(6)

C (a ⋅ s ) S (a ⋅ s ) , y( s ) = a a (7) 2 2 κ ( s ) = 2a ⋅ s , ϕ( s ) = a ⋅ s 2 x( s ) =

Fig. 5 shows a clothoid and its values. At a certain point we have a heading angle and a curvature. We may consider the trajectory at this point as an infinitely small arc with radius 1/κ.

4. Clothoid Computations Continuous-curvature paths require a) primitive trajectories with a continuous curvature and b) no discontinuity between two subsequent primitive trajectories. The curvature κ is defined as change of heading angle ϕ over running length s, i.e.,

dϕ (2) ds This means, for linear trajectories we have κ = 0 and for arcs κ = 1/r. To connect these trajectories with constant curvature we further need a type with changing curvature. Of the infinite possible trajectories with changing curvature, clothoids play an important role: they have a constant change of curvature over running length, i.e.,

κ=

s (3)  for a trajectory length of , curvature κ0 for s = 0 and curvature (κ0 + ∆κ) for s = . Many formulas for clothoids request κ = 0 for s = 0, i.e.,

κ ( s ) = κ 0 + ∆κ

s′ ∆κ (4)  To apply (3) instead of (2) we have to use s’ = s + s0 with s0 = κ0/∆κ. As a result, we can use the simple formula (3) and just have to increment the running length s by s0.

κ (s ′) =

4.1. Direct Clothoid Functions

Direct clothoid functions map a running length s to position (x, y), curvature κ and heading angle ϕ. Positions are based on so-called the Fresnel integrals

C( s ) = ∫ cos(t 2 )dt , S ( s ) = ∫ sin(t 2 )dt

(5)

Fig. 5. A clothoid Only the formulas for κ and φ can easily be inverted. Moreover, for C(s) and S(s) we only know approximations

s5 s9 s 13 + − + ... 5 ⋅ 2! 9 ⋅ 4! 13 ⋅ 6! (8) s3 s7 s 11 S( s ) = − + − ... 3 ⋅ 1! 7 ⋅ 3! 11 ⋅ 5! hat do not easily support inverse questions. E.g., for given x, y, it is not obvious to get s, a of a clothoid to get there. The following sections show how to construct maneuvers with clothoids and how to compute clothoid parameters. C( s ) = s −

4.2. Replacing Arcs by Symmetric Clothoids

A first idea to construct continuous-curvature maneuvers: we take a non-continuous maneuver and replace each arc by two symmetric clothoids (0C C-0). The replacement starts and terminates with zero curvature, whereas both clothoids are connected by the same non-zero curvature. As an example, we can replace the J-Bow (L A) by J-Clothoid-Bow (L 0C C-0). If the original maneuver had an arc radius as free parameter, we consider this radius also as free parameter for the new maneuver, even though no arc appeared in the maneuver. This was indicated by (*) in Table 2. We assume an arc that starts in x-direction, i.e., the arc centre resides on the y-axis. For the general case we have to roto-translate our approach to any location/orientation. We want to replace the arc with angle α by two symmetric clothoids. This means, each Articles

Journal of Automation, Mobile Robotics and Intelligent Systems

clothoid spans an angle of α/2 viewed from the arc centre. This also must be the heading angle at the end of the first clothoid (Fig. 6). Of the clothoid we do not know the respective a. However, we can benefit from a property of clothoids: they only differ in their scaling, but have, apart from this, identical shapes. Our approach to compute a is as follows: • We first construct a clothoid with a’ = 1/ 2. • We compute s’ to get to φ(s’) = α/2. • We compute (x’, y’) for s’. • We add a symmetric clothoid and get to (t’x, t’y). • We compute an f with (tx, ty)=f(t’x, t’y) for the former arc termination (tx, ty). • We finally get a=1/ 2 f . y

(cx, cy)=(0, r)

(tx, ty)



(x/2, y/2)

/2

Fig. 6. Replacement of arc by clothoids Fig. 6. Replacement of two arc by two clothoids Thetheidea behind firstthe create a ‘test’a. Of clothoid we this: do notwe know respective clothoid and the symmetric clothoid. TypicalHowever, we append can benefit from a property of clothoids: they in to their from ly, weonly willdiffer not get the scaling, desired but endhave, point.apart But from this, identical shapes. Our approach to compute a is as the nature of all clothoids, the position is only wrong follows: by a scaling factor. When we applied the scaling f, we finally We get firstthe construct a clothoid with a'=1/2. respective a.  We compute s' to get to (s')=/2.  We compute (x', y') for s'. 4.3.We add a symmetric clothoidClothoid and get to (tx', ty'). Maneuvers with Leading  We compute an f with (tx, ty)=f(tx', ty') for the former A second approach arc termination (tx, ty).to create continuous-curvature maneuvers is to add a leading clothoid of  We finally get a=1/(2f). type C0. An existing continuous-curvature maneu0 ver that with L or clothoid C) can The idea starts behind this:zero we curvature first create(i.e., a 'test' and append the symmetric clothoid. weany will be transformed to a maneuver thatTypically, starts with not get to curvature. the desiredThe endexample point. But from the nature non-zero is ∫-Clothoid-Arcs of all 0clothoids, the position is only by a scaling (0C C L 0C C-0) that is extended to wrong C-∫-Clothoid-Arcs 0 0 we applied the scaling f, we finally get factor. When (C0 C C0 L C C0). the The respective a. construction is as follows: we construct a clothoid that starts the start curvature and 4.3. Maneuvers withwith Leading Clothoid ends with zero curvature in any point. As the original A second approach to create continuous-curvature maneuver already was able to connect any point maneuvers is to add a leading clothoid of start type C 0. An with anycontinuous-curvature target point, it also is possible to choose the existing maneuver that starts 0C) can be first clothoid’s end point alternative start. Howevwith zero curvature (i.e., as L or transformed to maneuver that starts with any non-zero er, awe get an additional free parameter  – thecurvafirst ture. The example curvature’s length. is -Clothoid-Arcs (0C C-0 L 0C C-0) 0 C L 0C thatWith is extended to C--Clothoid-Arcs 0 Csection -0 the methods in section ‎4.2 and(C this Cwe -0). made use of direct clothoid computations as The in construction as follows: we construct a clotshown formula (7)isand only modified or extended hoid that starts with the start curvature and ends with existing maneuvers. However, we also wish to directzero curvature in any point. As the original maneuver ly createwas continuous-curvature already able to connect anymaneuvers. start point For withthis, any we need to invert clothoid functions. The following target point, it also is possible to choose the first clothoid's end point as alternative start. However, we get an additional free parameter  – the first curvature's 16 length. Articles With the methods in section 4.2 and this section

VOLUME 15,

N° 1

2021

sections deal with this problem. For most questions related to inverse clothoid functions, we do not know closed formulas. We thus developed numerous approximations to quickly compute such functions in the context of trajectory planning.

4.4. Running Length and Distance to Given Point

This next question is not actually related to maneuver construction, but it is heavily used during plan execution: given a point (px, py) and a clothoid defined by a. What is the running length s to get to (px, py)? As a clothoid may not exactly go through (px, py), we extend the question: what is the running length s of the nearest clothoid point? We need this function in the context of motion regulation [20] when we want to quickly compute functions toproximations check if the robot still drives on thesuch planned route in theifcontext of trajectory planning. and not, what is the current deviation. OurRunning approach Length to compute function is fol4.4. andthisDistance toasGiven lows: Point • We first only consider the standard clothoid a = 1. This next question is not actually maneuver A clothoid is infinitely twisted, related but we to stop after construction, but it is heavily used during plan execua reasonably running length in the context of tration: given a point (px, py) and a clothoid defined by a. jectory planning, e.g.,length s = √(2π). What is the running s to get to (px, py)? As a • clothoid We pre-compute a grid 7 left): grid may not exactly go(Fig. through (pxfor , py),each we extend centre we store the running length s of the nearest the question: what is the running length s of the nearA grid 20×20 sufficient. estclothoid clothoidpoint. point? We of need thisisfunction in the con• text Forofa position (px, py) we[20] look for the with the motion regulation when wecell want to check if the robotcell stillcentre drivestoon(pthe and was if not, nearest position x, pplanned y). If theroute what is thethe current deviation. outside grid, we use a projection into the grid. Ourrunning approach to compute this from function as isfol• The length s retrieved the is grid lows: only a rough approximation. We thus need a cor rection We first only consider the standard clothoid a=1. A step. clothoid is infinitely twisted, but we stop after a reasonably running length in the context of trajec(p , p ) tory planning, e.g., s=(2).  We pre-compute a grid (Fig. 7 left): for each grid centre we store the running length s of the nearest (c , c ) clothoid point. A grid of 2020 is sufficient.  For a position (px, py) we look for the cell with the nearest cell centre to (px, py). If the position was outside the grid, we use a projection into the grid.  The running length s retrieved from the grid is only a rough approximation. We thus need a correction step. x

(px, py)

Fig. 7. Getting the nearest clothoid point 

1/

(cx, cy)

To pre-compute the grid, we iterate through s in a fine-grained manner, apply formula (7) and memorize in the grid cells, whether a new clothoid point is closer than formerly computed clothoid points. This pre-computation usually costs only some seconds. When using the grid at runtime, we have to face the case when a given position is outside. For each of 7. Getting the nearest clothoid point a virthe majorFig. direction (e.g., x too large) we assume tual clothoid centre and linearly project (px, py) to borTo grid pre-compute the grid, we iterate through s in a der cell. fine-grained manner, apply and memorize For a certain cell, we askformula for the (7) respective runin the grid cells, whether a new clothoid point is ning length s’ of nearest clothoid point. To get a more closer than formerly computed clothoid points. This precise s we proceed as follows (Fig. 7 right): pre-computation usually costs only some seconds. When using the grid at runtime, we have to face the case when a given position is outside. For each of the major direction (e.g., x too large) we assume a virtual clothoid centre and linearly project (px, py) to

Journal of Automation, Mobile Robotics and Intelligent Systems

VOLUME 15,

• From s’ we compute φ(s’) and κ(s’) and construct an arc with centre (cx, cy) and radius 1/κ(s’). This arc approximates the clothoid in the area of s’. • We cut the arc with the line (cx, cy) – (px, py). From this we can compute a more precise s. • We can repeat this to improve the precision, but a single iteration usually is sufficient. Until now, we assume a=1. For any other value, we perform the steps above for (a⋅px, a⋅py) to get the respective running length s/a.

4.5. Clothoid Through a Given Point with Zero Start Curvature

The next question is: given a (px, py) what is the a of a clothoid and running length s to go to this point? We need the answer to construct the Clotho-0C maneuver (0C). Even though we have two equations and two variables, the respective equation system contains the functions C and S that prohibit common techniques to solve equations. We thus reduce the equations with a single function Q that we numerically invert. We define

Q( x ) =

S( x ) C( x )

(9)

We then are able to express the ratio of py and px as p y S (a ⋅ s ) = = Q(a ⋅ s ) px C (a ⋅ s )

From

a ⋅ s = Q −1

we get and finally

(10)

py C (a ⋅ s ) , px = px a

(11)

py   C  Q −1 px  C (a ⋅ s ) a= =  px px s=

Q −1

(12)

py px

(13) As a result, we now ‘only’ have to invert Q. A first problem: Q is periodic, thus we can only invert from 0 to the first maximum (Fig. 8). For a standard-clothoid (a = 1) we only convert up to a running length from 0 to approx. 2.04 (represents 75% of the spiral turn). This is sufficient for trajectory planning requests.

N° 1

2021

Second, we have to numerically invert Q. As Q has a simple shape, we can use simple techniques. E.g., we may sample some hundreds values of Q as pre-computed data points and interpolate to compute any value of Q-1.

4.6. Clothoid Through a Given Point with Non-Zero Start Curvature The approach in the section 4 ‎ .5 is only applicable for start curvature zero. In this section we assume a non-zero start curvature. We need this approach to construct the Clotho-C maneuver (C). We assume a clothoid as presented in formula (3) with start curvature κ0. We have two cases (Fig. 9).

Fig. 9. Two cases of clothoids with start curvature For a certain κ0 and value a we have two major ways to create a clothoid, dependent on the sign of ∆κ. It distinguishes, whether the turning point κ=0 appears for increasing or decreasing running length s. For a given (px, py) we are thus looking for value a, s and the sign of ∆κ of a clothoid that goes through the point. Our approach to compute these values is as follows: • We only consider the case κ0=1 and pre-compute a grid. • In the pre-computation phase we construct sample clothoids for different values of a and the two cases for the signs of ∆κ. For each clothoid we iterate through s in a fine-granular manner. • Each grid element holds the parameters of the clothoid that was the nearest passing the grid centre (Fig. 10).

2.0 1.8 1.6 1.4

Q (x )

1.2 1.0 0.8 0.6 0.4

Fig. 10. Look-up array

0.2 0.0 0.0

0.5

1.0

1.5

2.0

2.5

Fig. 8. Q function

3.0

3.5

4.0

4.5

For trajectory planning, the values of a from the interval [0.05; 5.0] are reasonable. This is because smaller values create very large and larger values Articles

rameters a', s' asking the cell of (pxs, pys). As this is only an approximation, we improve the precision with of Automation, Mobile Robotics and Intelligent Systems We anJournal optimization approach (e.g., downhill simplex). finally get a=a's, s=s's. (px, py)

C (xs, ys, s, s)

1/s

4.7. Clothoid Extended by a Straight Line very tight spirals. Both are not suitable in the context Through planning. a Given Values Point from s up to 25 are sufof trajectory

Also in continuous-curvature planning have creficient as larger values create more we than onetospiral ate‘round’. maneuvers that span large we distances; we For our experiments createdusually a grid with have integrate an L100×100 trajectory forWe this. A maneuver cellto size of 0.2 with cells. created approx. with L would that start orientation 1.7 single millions samplerequire clothoids to the fill the grid. exactly points to the target position. In practice, Fig.(p 10. array For a position look up clothoidthis pax, pLook-up y) we first never occurs. The most simple continuous-curvature rameters a’, s’ asking the cell of (px⋅κs, py⋅κs). As this is maneuver thatplanning, uses an Lthe trajectory and starts from an For trajectory values of a from the interonly an approximation, we improve the precision with arbitrary pose is of type C L (Clotho-CL). Note that 0 valan[0.05; 5.0] are approach reasonable. This is because smaller optimization (e.g., downhill simplex). We this pattern does not support to specify a target orienvalues create very large and larger values very tight finally get a=a’⋅κ , s=s’⋅κ . s s tation. Both are not suitable in the context of trajecspirals. actual Values problem is tos up construct a clothoid toryThe planning. from to 25 are sufficientthat as points to the target position at  =0 (Fig. 11). Our For aplarger values create more than one spiral 'round'. 4.7. Clothoid Extended a Straight proach to construct such aby is cell asLine follow: we our experiments we created aclothoid grid with size of 0.2 Through a Given Point with 100100 cells. We created approx. 1.7 millions Also in continuous-curvature planning we have to sample clothoids to fill the grid. create span large usually For a maneuvers position (px,that py) we first lookdistances; up clothoid parameters asking the of (pxs,for pythis. s). As this is we havea',tos'integrate an cell L trajectory A maneuonly approximation, werequire improve thethe precision with veran with single L would that start orientaantion optimization approach downhill simplex). We exactly points to the(e.g., target position. In practice, finally get a=a'  , s=s'  . s s this never occurs. The most simple continuous-curvature maneuver that uses an L trajectory and starts (p , py) from an arbitrary pose is ofx type C0L (Clotho-CL). Note L that this pattern does not support to specify a target orientation. The actual problem is to construct a clothoid that C 1/target s points to the position at κ = 0 (Fig. 11). Our ap(xs, ys, s, s) proach to construct such a clothoid is as follow: we first measure the angle of start point to target point. We then construct a clothoid with the required start curvature that has the respective orientation at κ = 0. This can easily be computed using the formulas for κ and φ (7). The required clothoid now has the respective angle Construction of Cposition 0L maneuvers to createFig. an 11. L trajectory, but the of κ = 0 is not the starting point, thus we miss the target. We use the 4.7. Clothoid Extended by ato Straight Line point to again measure the angle the target, which starts from a more suitable position. We again use the Through a Given Point measured angle to create a more precise The Also in continuous-curvature planning we clothoid. have to creangle and thus the clothoid position at κ = 0 quickly ate maneuvers that span large distances; usually we converges, if theantarget is not too closeAthe clothoid have to integrate L trajectory for this. maneuver turning Thus, this approach is only suitable, with singlepoint. L would require that the start orientation if targets exceed a certain distance.In This however exactly points to the target position. practice, thisis never occurs. as The most continuous-curvature reasonable, this typesimple of maneuver explicitly should maneuver that uses an L trajectory and starts from an spawn a larger distance. arbitrary pose is of type C0L (Clotho-CL). Note that this pattern does not support to specify a target orientation. The actual problem is to construct a clothoid that points to the target position at =0 (Fig. 11). Our ap18 Articles proach to construct such a clothoid is as follow: we

4.8. Clothoid Extended by a Straight Line With 4.8. Given Clothoid Extended by a Straight Line With Distance

Given Distancehow to create a maneuver that The next question: starts from a non-zero and ends with The next question: howcurvature to create a maneuver thatzero curvature, the target orientation is with given? We starts fromwhen a non-zero curvature and ends zero assume thewhen structure pattern AC0L (maneuver curvature, the target orientation is given?ClothoACL or the reverse Clotho-LCA). illustrates the We assume the structure patternFig. AC012 L (maneuver problem. Clotho-ACL or the reverse Clotho-LCA). Fig. 12 illusThe start configuration defines a circle, on which trates the problem. the clothoid must start with curvature s. The target L configuration defines a line, on which the clothoid , yt, t, 0) C curvature zero. The(xtrespective must terminate with clothoid's start and target points are not given. However, we know the distance d that is the distance of leading arc Acentre to terminating line. Let d

Fig. 11. Construction of C0L maneuvers Fig. 11. Construction of C0L maneuvers

clothoid turning point. Thus, this approach is only suitable, if targets exceed a certain distance. This however is reasonable, as this VOLUME type of15,maneuver exN° 1 2021 plicitly should spawn a larger distance.

(xs, ys, s, s)

1/s

d=s(a)

(14)

the function that computes the respective distance for given start curvature a.0LWe are able to comFig. 12. Construction ofand AC0value L maneuvers Fig. 12. Construction of AC maneuvers pute  using formulas (7), but these formulas cannot easily bestart inverted. What of westart actually need is on which first measure the angle point to target point. The configuration defines a circle, We construct a clothoid with the required start the then clothoid must start with curvature κ . The target s -1 (15) a=a (d) on which curvature that defines has the respective orientation at  =0. sline, configuration the clothoid This can easily be computed using the formulas for must terminate with curvature zero. The respective A first approach already applied in former sections: as and  (7). clothoid’s start and target points are not given. Howall The clothoids are clothoid similar apart from scaling, anwe required now has thethe respective ever, we know distance d that is the distance of onlytoinvert forLthe atrajectory, single start =1 and regle createan butcurvature the position of  =0 is s leading arc centre to terminating line. Let scale resultspoint, later.thus Moreover, not have to not thethe starting we misswe thedo target. We use consider for s=1: it should be too to 1.0 the todagain measure thenot angle to close the target, pointall d = δ κs(a) (14) as thisstarts wouldfrom mean to drive a longposition. distanceWe until we which a more suitable again the the function that computes the respective distanceclotfor use measured angle to create a more precise givenThe startangle curvature and the value a. We are able toatcomhoid. and thus clothoid position =0 quickly converges, if the is not too close the pute δ using formulas (7),target but these formulas cannot clothoid this approach easily beturning inverted.point. What Thus, we actually need is is only suitable, if targets exceed a certain distance. This 1 (15) a =asδ κ−this however is reasonable, exs (d )type of maneuver plicitly should spawn a larger distance. A first approach already applied in former sec4.8. Clothoid Extended by a Straight Line tions: as all clothoids are similar apart from theWith scaling, we only invert δ for a single start curvature κ s = 1 Given Distance and rescale the results later. Moreover, we do not have The next question: how to create a maneuver that to consider d for κs = 1: it should be with too close starts from aall non-zero curvature andnot ends zero to 1.0 as this would mean to drive a long distance curvature, when the target orientation is given? unWe til we change to L. Itpattern should AC also not be too large as assume the structure 0L (maneuver Clothothis or would requireClotho-LCA). a quick change ACL the reverse Fig. of 12curvature. illustrates For the our experiments we select d ∈ [1.1, 5.0], what results problem. start configuration defines a circle, which in The a ∈ [0.1254, 0.565]. As we effectively areonable to the clothoid must start some with curvature s. Theoftarget compute δ1, we sample hundreds values δ1 as configuration a line, which the clothoid pre-computeddefines data points andon interpolate to compute –1 with curvature zero. The respective must terminate any value of δ1 . We again improve the precision with clothoid's start and target (e.g., points are notsimplex). given. Howan optimization approach downhill We ever, we know the distance d that is the distance of finally get an approximation for all start curvatures: leading arc centre to terminating line. Let δ κ−s1 (d ) ≈ κ sδ 1−1 (d ) (16) (14) d=s(a) If the target angle θt is not given, we can construct thefunction maneuver that drives to (xrespective orientat, yt) with any the that computes the distance for tion. This can be useful for inner route points, given start curvature and value a. We are able towhen comwe only wantformulas to visit the point formulas whereas the acpute  using (7),route but these cannot tual orientation not ofwe interest (Clotho-ACL-nodir). easily be inverted.isWhat actually need is In this case, we do not have to construct a certain d, -1 but are interested, how (15) a=sfast (d) the clothoid transforms the start curvature to zero. This is expressed by the Afactor first approach in former sections: as d⋅κs. Tablealready 3 showsapplied some values. all clothoids are similar apart from the scaling, we only invert  for a single start curvature s=1 and rescale the results later. Moreover, we do not have to consider all d for s=1: it should not be too close to 1.0 as this would mean to drive a long distance until we

Journal of Automation, Mobile Robotics and Intelligent Systems

Table 3. Value a for different distance factors d⋅κs

a/κs

1.5

0.3687568411482851

2.5

0.2550963611370387

2.0

0.21431547506198895

5.0

0.12540637523181616

4.0

N° 1

2021

Table 4. Clothoid functions

0.2983815391022415

3.0

VOLUME 15,

0.1575822277624827

Here d⋅κs expresses the speed of curvature change and can be adapted to the robot’s properties.

4.9. Maneuvers Between Non-Zero Curvatures

The most complex continuous-curvature maneuver that is not a result of transforming or extending existing maneuvers has the structure pattern AC0L0CA. It connects two configurations with non-zero curvatures. The construction is similar to the AC0L maneuver of section 4 ‎ .8. In contrast to AC0L with given target orientation, the orientation of the L trajectory in the AC0L0CA is not given. We can consider the orientation as free parameter. However, it is more suitable to express the degree of freedom with the approach of fixed values of d⋅κs (Table 3).

Function

Approach

Section

Cont-curve maneuver creation

replace A by 0CC-0

4.2

append trailing C0

4.3

Nearest clothoid point

pre-computed grid, correction with arc approximation

4.4

C to given point

pre-computed grid, subsequent optimization

4.6

Cont-curve maneuver creation

C to given point

Example Maneuver J-ClothoidBow C-∫ClothoidArcs all

inversion of Q

4.5

Clotho-0C

C0L to given point

converging L angle construction

4.7

Clotho-CL

AC0L0CA to given pose

tangents of two circles, a values from table

4.9

ClothoACLCA

AC0L to given pose

closed formula for approximation of a, subsequent optimization

4.8

Clotho-C

ClothoACL

(xt, yt, θt, κt)

1/κt

A (xs, ys, θs, κs)

1/κt

1/κs

Fig. 13. Construction of AC0L0CA maneuvers

5. Experiments We implemented the approach on our Carbot robot (Fig. 14). It has a size of 35 cm x 40 cm x 27 cm and a weight of 4.9 kg. It is able to run with a speed of 31 cm/s. The wheel configuration allows to independently steer two wheels.

Fig. 13 shows the idea. We have two free parameters ds⋅κs and dt⋅κt. For a certain selection we get two circles and four tangents (Fig. 13 right) that touch these circles. From the four tangents two would change to target orientation by 180° – the choice for the remaining tangents thus is an additional free (binary) parameter.

4.10. Summary of Approaches

In addition to the well-known clothoid computations in section ‎4.1, we introduced functions that are related to continuous-curvature trajectories. Table 4 gives an overview of the last sections. Some of them reverse clothoid functions with the help of pre-computations and subsequent optimizations. The applied techniques cover a wide range of methods as there is no such best approach for all required clothoid functions.

Fig. 14. The Carbot For arcs, the different numbers of revolutions of the powered front wheels as well as the steering angles of the steered rear wheels are adapted to follow Articles

Journal of Automation, Mobile Robotics and Intelligent Systems

the respective curve geometry. The motion system is also able to drive clothoids with a single motion command – here the servos and revolutions are smoothly changed according to the clothoid’s geometry. We run the experiments in our simulation environment that simulates the robot on hardware-level [21]. It is very close to the real robot. E.g., the same binary code runs on the real robot and simulator. Motors and sensors are simulated very low-level. E.g., the simulated and real motors have the same I2C command interface. Physical effects such as slippage and sensor errors are applied. The benefit of the simulation tool: we can easily construct very large environments. E.g., we constructed a virtual maze with size of 50 m × 50 m which the robot has to discover in order to find a way out. In our experiments, we first want to know the performance of our approximation functions (Table 5). We measured the time on a i7-4790 CPU, 3.6 GHz. Most of the approximations are uncritical concerning execution time. The exception is the approach described in section 4.6. We investigate that nearly all of the time to compute the C trajectory is used by the downhill simplex approach to improve the precision of a. Fortunately, the approximation is only used to construct a single maneuver, the Clotho-C. Tab. 5. Clothoid function computation times Function

Section

Execution time

Nearest clothoid point

4.4

2.78 µs

C to given point

2.21 µs

4.6

196.1 µs

AC0L to given pose

4.8

17.7 µs

C0L to given point

4.5

C to given point

4.7

11.5 µs

We further tested the continuous-curvature planning in several scenarios. We constructed different environments. A maze generator is able to construct mazes with arbitrary size. We also created mixed environments with different shaped obstacles. In each environment the robot is able to perform two tasks: • Exploration and navigation: Here, the environment is unknown at startup and the robot should drive to a certain position. For this, the robot plans movement on an incomplete map. During movement, the robot explores formerly unknown obstacles, thus has to reschedule the movement. Driving and rescheduling are alternated, until the target is reached. • Only Navigation: here the environment is known, either from former exploration or because the ground map is provided by the caller. In this case, the robot only has to compute a single path and drive it. Fig. 15. shows screenshots of two environments (top: a large maze, bottom: a mixed environment). For each, the left images show the single plan for a known map (only navigation). The right images show some steps when the robot tried to move to the target in unknown environments (exploration and navigation). Paths for known environments show very fine-granular, smooth and rational trajectories. For Articles

VOLUME 15,

N° 1

2021

the case of unknown maps, the robot may unwittingly drive to dead-ends. In this case, the robot either may drive a U-turn or a turning maneuver. We can see both cases. Note that planning turning maneuvers usually is not trivial, in particular if we want to have continuous curvatures. The robot has to take into account the costs for backdriving compared to the costs for a U-turn. As our approach incorporates arbitrary costs functions, we are able to penalize trajectories that go backwards, as in this case the robot has to drive slower. Note that the Viterbi approach is able to deal with such situations, even though only pairs of connected trajectories are locally optimized. We finally evaluated the occurrence of our continuous-curvature maneuvers in paths. We measured: • The percentage of a specific maneuver that is selected as best to connect two subsequent (intermediate) configurations (Fig. 16 left). This was indicated as ‘best maneuver’ for each step in Fig. 4. • The percentage of a specific maneuver to appear in the finally planned trajectory sequence to a target (Fig. 16 right). The histograms are not identical, as the creation of the final trajectory is strongly influenced by the selection of near-optimal intermediate configurations. In other words: a certain maneuver may have a large portion to connect arbitrary configurations (that may turned out not to be near-optimal), but a lower portion to connect configurations that appear more often in optimal sequences. An observation: maneuvers that make use of the arc replacement (Section 4 ‎ .2) and leading clothoids (Section ‎4.2) often appear in planned sequences. On the other hand, maneuvers that connect non-zero curvatures (Section 4 ‎ .9) only rarely appear. This however is a result of the fact that many of our maneuvers terminate with L, C0 or C-0, i.e., zero curvature. A further observation: of the computations in Table 5, the ‘ C to given point’ had worse performance. But the respective maneuver Clotho-C only rarely appeared both as best maneuver in a optimization step and in the planned path. We thus could remove this maneuver from the list without serious consequences.

6. Conclusion Trajectory planning for mobile robots still is a non-trivial task. If we additionally request continuous-curvature planning that considers obstacles and arbitrary cost functions, we have to face numerous challenges, starting from the actual planning approach down to approximations that compute the respective trajectory parameters. There is an infinite number of trajectories that connect two configurations and optimal continuous-curvature paths may have an infinite number of primitive trajectories. We thus need a simplification of the overall problem. We suggest modelling the final path by maneuvers, for which we know formulas to define the respective geometric parameters. We introduced a set of about twenty continuous-curvature maneuvers and provide basic approximations, when-

Journal of Automation, Mobile Robotics and Intelligent Systems

VOLUME 15,

N° 1

2021

Fig. 15. Planned paths in different environments

Articles

Journal of Automation, Mobile Robotics and Intelligent Systems

VOLUME 15,

0 0 C-C-ò-Clothoid-Arcs -Clothoid-Arcs(C0 (C00C CC-0 C-0 LL0C CC-0) C-0)

2021

0 ò-Clothoid-Arcs (0C -Clothoid-Arcs (0CC-0 C-0 LL0C CC-0) C-0)

0 Clotho-ACLCA(A (A C C0 L 0C C A) A) Clotho-ACLCA 0L 0

N° 1

Clotho-0C (0C) Clotho-0C (0C)

ò-Clothoid-Arcs (0C -Clothoid-Arcs ( CC-0 C-0 LL0CCC-0) C-0)

0 S-Clothoid-Arcs (0C C-00C CC-0) C-0) S-Clothoid-Arcs (0CC-0

Clotho-LCA (L (L 0C C A) A) Clotho-LCA

0 0 Snake2-Clothoid-Arcs (L Snake2-Clothoid-Arcs (L0C CC-0 C-00C CC-0) C-0)

0 C-Wing-Clothoid-Arc (C (C00 L L 0C C C-0 C-0 L) L) C-Wing-Clothoid-Arc

0 Snake-Clothoid-Arcs (0C L) C-0 0C C C-0 C-0 L) Snake-Clothoid-Arcs (0CC-0

0 0 Snake2-Clothoid-Arcs (L CC-0 C-00C CC-0) C-0) Snake2-Clothoid-Arcs (L0C

0 C-0 L 0C C-C-ò-Clothoid-Arcs -Clothoid-Arcs(C0 (C00C C C-0 L 0CC-0) C-0)

0 Snake-Clothoid-Arcs (0C Snake-Clothoid-Arcs (0CC-0 C-0 0C C C-0 C-0 L) L)

Clotho-CL (C (C00 L) L) Clotho-CL

0 C-0) S-Clothoid-Arcs (0C C-00C C C-0) S-Clothoid-Arcs (0CC-0

0 Clotho-C00C (C0 C) Clotho-C00C (C0 0C)

Wing-Clothoid-Arc (L (L 0C Wing-Clothoid-Arc C C-0 C-0 L) L)

0 J-Clothoid-Bow (L CC-0) C-0) J-Clothoid-Bow (L0C

0 C-0 L) C-J-Clothoid-Bow2 (C0 C C-0 L) C-J-Clothoid-Bow2 (C00C

J-Clothoid-Bow2 (0C L) C-0 L) J-Clothoid-Bow2 (0C C-0

0 C-J-Clothoid-Bow (C0 CC-0) C-0) C-J-Clothoid-Bow (C0LL0C

Clotho-C (C) Clotho-C (C)

0 C-0) Dubins-Clothoid-Arcs (0C C-0 C-00C0CC-0 C-00C C C-0) Dubins-Clothoid-Arcs (0C

0 L 0C0C-0 L) C-Wing-Clothoid-Arc C C-0 C C-0) Dubins-Clothoid-Arcs (0C C-0(C0

0 C-0) J-Clothoid-Bow (L J-Clothoid-Bow (L0C C C-0)

C-J-Clothoid-Bow2 (C0 L 0C-0 C CL) C-J-Clothoid-Bow (C0 0C -0)

Clotho-0C (0C) Clotho-0C (0C)

C-J-Clothoid-Bow (C0 C-J-Clothoid-Bow2 (C0 L0C0CCC-0) -0 L)

J-Clothoid-Bow2 (0C C-0 L) L) J-Clothoid-Bow2 (0C C-0

Dubins-Clothoid-Arcs (0C C-0 (C 0C0 C-0 L 0C0CCC-0) C-Wing-Clothoid-Arc -0 L)

Clotho-ACL (A (A C0 Clotho-ACL C0 L) L)

Clotho-LCA(L (L 00C A) C A) Clotho-LCA

0 A) Clotho-CLCA (C (C0 L 0C C A) Clotho-CLCA 0L

Clotho-ACL-nodir (A (A C0 L) Clotho-ACL-nodir C0 L) Clotho-CLCA (C (C0 L 00C A) Clotho-CLCA 0 L C A)

Clotho-ACL-nodir (A (A C0 Clotho-ACL-nodir C0 L) L)

0 L 0C A) Clotho-ACLCA (A(LC0 Wing-Clothoid-Arc C C-0 L)

Clotho-C (C) (C) Clotho-C

0 Wing-Clothoid-Arc C-0 L) Clotho-ACLCA (A(LC0C 0 L C A)

0 Clotho-C00C (C0 C) Clotho-C00C (C00C)

Clotho-ACL Clothoid-Arcarc (0C C-0(A0CC0CL) -0)

0 C-0) Clothoid-Arcarc (0C C-00C C C-0) Clothoid-Arcarc (0CC-0

Clothoid-Arcarc (0C C-0(A 0CCC-0) Clotho-ACL 0 L)

Clotho-CL (C (C00 L) Clotho-CL L)

Fig. 16. Percentage of a maneuver in a Viterbi step (left), percentage of a maneuver in the final path (right) ever the underlying clothoid function cannot easily be reversed. As an observation: the respective questions to find parameters are different between the maneuvers and lead to different approximation – we finally discovered five methods. We successfully implemented the approach on our Carbot platform. Carbot is able to create continuous-curvature paths to, e.g., get through a large maze.

AUTHOR Jörg Roth – Faculty of Computer Science, Nuremberg Institute of Technology, Nuremberg, Germany, e-mail: joerg.roth@th-nuernberg.de.

References [1] J. Barraquand, B. Langlois and J.-C. Latombe, “Numerical potential field techniques for robot path planning”, IEEE Transactions on Systems, Man, and Cybernetics, vol. 22, no. 2, 1992, 224–241, 10.1109/21.148426.

[2] J.-D. Boissonnat, A. Cerezo and J. Leblond, “A note on shortest paths in the plane subject to a constraint on the derivative of the curvature”, Research Report, Institut National de Recherche en Informatique et an Automatique, 1994. [3] X.-N. Bui, J.-D. Boissonnat, P. Soueres and J.-P. Laumond, “Shortest path synthesis for Dubins non-holonomic robot”. In: Proceedings of the 1994 IEEE International Conference on Robotics and Automation, 1994, 2–7, 10.1109/ROBOT.1994.351019.

[4] H. Delingette, M. Hebert and K. Ikeuchi, “Trajectory generation with curvature constraint based on energy minimization”. In: Proceedings IROS ‘91: IEEE/RSJ International Workshop on Intelligent Robots and Systems ‘91, 1991, 206–211, 10.1109/IROS.1991.174451. Articles

[5] L. E. Dubins, “On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents”, American Journal of Mathematics, vol. 79, no. 3, 1957, 497–516.

[6] S. Fleury, P. Soueres, J.-P. Laumond and R. Chatila, “Primitives for smoothing mobile robot trajectories”, IEEE Transactions on Robotics and Automation, vol. 11, no. 3, 1995, 441–448, 10.1109/70.388788. [7] T. Gu and J. M. Dolan, “On-Road Motion Planning for Autonomous Vehicles”. In: C.-Y. Su, S. Rakheja and H. Liu (eds.), Intelligent Robotics and Applications, 2012, 588–597, 10.1007/978-3-64233503-7_57.

[8] S. Karaman, M. R. Walter, A. Perez, E. Frazzoli and S. Teller, “Anytime Motion Planning using the RRT*”. In: 2011 IEEE International Conference on Robotics and Automation, 2011, 1478–1483, 10.1109/ICRA.2011.5980479.

[9] J.-P. Laumond, P. E. Jacobs, M. Taïx and R. M. Murray, “A motion planner for nonholonomic mobile robots”, IEEE Transactions on Robotics and Automation, vol. 10, no. 5, 1994, 577–593, 10.1109/70.326564. [10] S. M. LaValle, “Rapidly-Exploring Random Trees: A New Tool for Path Planning” , Research Report, Computer Science Department, Iowa State University, 1998.

[11] S. M. LaValle and J. J. Kuffner, “Randomized Kinodynamic Planning”, The International Journal of Robotics Research, vol. 20, no. 5, 2001, 378–400, 10.1177/02783640122067453. [12] T.-C. Liang, J.-S. Liu, G.-T. Hung and Y.-Z. Chang, “Practical and flexible path planning for car-like mobile robot using maximal-curvature cubic spiral”, Robotics and Autonomous Systems,

Journal of Automation, Mobile Robotics and Intelligent Systems

vol. 52, no. 4, 2005, 312–335, 10.1016/j.robot.2005.05.001.

[13] D. S. Meek and D. J. Walton, “A note on finding clothoids”, Journal of Computational and Applied Mathematics, vol. 170, no. 2, 2004, 433–453, 10.1016/j.cam.2003.12.047.

[14] D. S. Meek and D. J. Walton, “An arc spline approximation to a clothoid”, Journal of Computational and Applied Mathematics, vol. 170, no. 1, 2004, 59–77, 10.1016/j.cam.2003.12.038.

[15] M. Mukadam, X. Yan and B. Boots, “Gaussian Process Motion planning”. In: 2016 IEEE International Conference on Robotics and Automation (ICRA), 2016, 9–15, 10.1109/ICRA.2016.7487091.

VOLUME 15,

N° 1

2021

[24] D. K. Wilde, “Computing clothoid segments for trajectory generation”. In: 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2009, 2440–2445, 10.1109/ IROS.2009.5354700.

[25] M. Zucker, N. Ratliff, A. D. Dragan, M. Pivtoraiko, M. Klingensmith, C. M. Dellin, J. A. Bagnell and S. S. Srinivasa, “CHOMP: Covariant Hamiltonian Optimization for Motion Planning”, The International Journal of Robotics Research, vol. 32, no. 9-10, 2013, 1164–1193, 10.1177/0278364913488805.

[16] V. Muñoz and A. Ollero, “Smooth Trajectory Planning Method for Mobile Robots”. In: Proc. of the Congress on Comp. Engineering in System Applications, Lille, France, 1995, 700–705.

[17] M. Pivtoraiko and A. Kelly, “Efficient constrained path planning via search in state lattices”. In: Proceedings of 8th International Symposium on Artificial Intelligence, Robotics and Automation in Space (iSAIRAS ‘05), 2005.

[18] J. A. Reeds and L. A. Shepp, “Optimal paths for a car that goes both forwards and backwards.”, Pacific Journal of Mathematics, vol. 145, no. 2, 1990, 367–393.

[19] J. Roth, “A Viterbi-like Approach for Trajectory Planning with Different Maneuvers”. In: M. Strand, R. Dillmann, E. Menegatti and S. Ghidoni (eds.), Intelligent Autonomous Systems 15, 2019, 3–14, 10.1007/978-3-030-01370-7_1.

[20] J. Roth, “Trajectory Regulation for Walking Multipod Robots”, International Journal on Advances in Systems and Measurements, vol. 12, no. 3 & 4, 2019, 265–278.

[21] J. Roth, “Robots in the Classroom: Mobile Robot Projects in Academic Teaching”. In: K.-H. Lüke, G. Eichler, C. Erfurth and G. Fahrnberger (eds.), Innovations for Community Services, 2019, 39– 53, 10.1007/978-3-030-22482-0_4.

[22] A. Scheuer and T. Fraichard, “Continuous-curvature path planning for car-like vehicles”. In: Proceedings of the 1997 IEEE/RSJ International Conference on Intelligent Robot and Systems (IROS ‘97), vol. 2, 1997, 997–1003, 10.1109/ IROS.1997.655130. [23] A. Viterbi, “Error bounds for convolutional codes and an asymptotically optimum decoding algorithm”, IEEE Transactions on Information Theory, vol. 13, no. 2, 1967, 260–269, 10.1109/ TIT.1967.1054010.

Articles

Journal of Automation, Mobile Robotics and Intelligent Systems Journal of Automation, Mobile Robotics and Intelligent Systems

VOLUME 15, VOLUME 15,

N° 1 2021 N° 1 2021

HUMANOID ROBOT PATH PLANNING USING RAPIDLY EXPLORED RANDOM TREE AND MOTION PRIMITIVES Submitted: 6th April 2020; accepted: 14th March 2021

Maksymilian Szumowski, Teresa Zielinska DOI: 10.14313/JAMRIS/1‐2021/3 Abstract: Path planning is an essential function of the control sy‐ stem of any mobile robot. In this article the path planner for a humanoid robot is presented. The short description of an universal control framework and the Motion Ge‐ neration System is also presented. Described path plan‐ ner utilizes a limited number of motions called the Mo‐ tion Primitives. They are generated by Motion Generation System. Four different algorithms, namely the: Informed RRT, Informed RRT with random bias, and RRT with A* like heuristics were tested. For the last one the version with biased random function was also considered. All menti‐ oned algorithms were evaluated considering three diffe‐ rent scenarios. Obtained results are described and discus‐ sed. Keywords: humanoid robot, path planning, rapidly explo‐ ring random tree

1. Introduction An universal control framework for a humanoid robots performing various task is proposed. Despite of many solutions designed for speci�ic robots, universal and simple but effective framework is still an actual re‑ search topic. Such need was demonstrated by DARPA Robotics Challenge [6] where humanoid robots were acting semi–autonomously in disaster scenario, or by Robo–Cup Soccer Humanoid League [10] where hu‑ manoids were competing with each other. Essential part of a control system of any mobile ro‑ bot is a path planner. Presented work describes deve‑ loped and tested Motion Generation System (MGS) for a humanoid robots. Joint trajectories generated by the Motion Generation System that describe one fragment of the robot’s motion are called the Motion Primitive. Motion Primitive transfers the robot from one localisa‑ tion to another while maintaining the postural balance and ful�illing the joints constrains for position, velocity and torque. MGS can either be implemented as on–line generator, or can be used off–line pre–generating the gait cycles. Off–line mode reduces the needed for com‑ putational power of a robot controller. However not every motion can be pre–determined, maneuvering in dynamic environment cannot be left as an off–line op‑ tion. Path planning can be viewed as a search in a con‑ �iguration space. �or this purpose the search trees are constructed re�lecting transformations from the initial to goal con�igurations. The Rapidly–exploring Random Trees (RRT) is very popular motion planning method applied in mobile robots. In this method �irst the graph 24

of possible paths is created and next it is determined a feasible but not necessarily optimal path between the initial and �inal localisation. RRT* is the modi�ied RRT algorithm that aims to obtain the shortest path in a term of some de�ined metric. Informed RRT* is the mo‑ di�ication of RRT* which behaves as RRT* until a �irst solution is found. Afterwards is only sampled the sub‑ set of con�igurations de�ined by an admissible heuris‑ tic for possible solution improvement. It uses the heu‑ ristics for shrinking the planning problem only to the subsets of the original domain [4]. Due to limited num‑ ber of possible movements, the Informed RRT and not Informed RRT* is used. The difference lies in post pro‑ cessing which can be applied to smooth out generated tree. In this work the RRT approach is applied for path planning considering the obstacles. The Motion Pri‑ mitives are used. Presented case assumes that the ro‑ bot walks on the 2D plane perpendicular to the gravity vector. Two different methods were analysed: classic RRT with informed tree and RRT with A*–like heuris‑ tics. Both methods are additionally studied with bia‑ sed towards the goal con�iguration the function is used to build the tree. The paper is structured as follows. After discus‑ sing the related works, the path planner description taking into account the robot tasks (section 3) is given. Section 4 describes details of path planning method. In section 5 experimental results demonstrating advan‑ tages of presented approach are presented. At the end the conclusions are given.

2. Related Work

Path planner usually makes a separate module (or layer) in a structured (usually hierarchical) control architectures. In article [11] authors presented the hierarchical control system for planning and simu‑ lating dynamic motions of a humanoid robots. The sampling–based motion planning method is here ap‑ plied. Method concerns the motion in limited con‑ �iguration space. The effectiveness of Bi–directional Rapidly–explored Random Tree approach was proven even for imposed kinematic/kinodynamic constrains. By contrast in presented publication the path planning considering different con�iguration space and localisa‑ tion of the whole robot in the global reference frame is studied. Motion planning for humanoid robots (or any wal‑ king robots) is very actual research topic [9], [1]. In ar‑ ticle [1] researchers provide analysis of use different tree building algorithms: RRT–CONNECT and RRT–

JournalofofAutomation, Automation,Mobile MobileRobotics Roboticsand andIntelligent Intelligent Systems Systems Journal

VOLUME 2021 VOLUME 15, 15, N° 1N° 1 2021

Fig. 1. Task division into motion primitives

Fig. 2. The structure of Motion Generation System

EXTEND. RRT–CONNECT tries to connect two trees, one constructed starting from the initial con�igura‑ tion and another one built starting from the goal. In each the so called sample the modi�ied con�iguration is checked according to the tree structure. Difference between RRT–CONNECT and RRT–EXTEND lies in re‑ striction on number of samples (RRT–CONNECT has no restriction). Proposed algorithms (both cases) ful‑ �illed the expectations but there is still room for im‑ provement. Presented algorithms are not performing well when initial and �inal con�iguration are close but the obstacles exist. In this work we analysed 3 diffe‑ rent cases (goal con�igurations) which are of the above type. Additionally, not only classic Euclidean norm as a distance measure (distance between the points in 2–D plane) is analysed but the converge rate (time of path evaluation) for �inding best possible motion is studied. The Rapidly‑exploring Random Trees (RRT) [7] is a very popular motion planning method applied in mobile robots. Often the methods used for mobile (wheeled) robots path planning are transferred to the legged robots. The example are the methods presen‑ ted in [12] and [2], [17]. In [12] the RRT method was applied for 6‑legged robot. Described in these publi‑ cations algorithms are providing interesting impro‑ vements however they have the drawbacks as well. In [12] the user must specify additional inter–mediate

con�igurations what is limiting the automatic path planning. As studied in [2] an on‑line motion genera‑ tor is still needed to generate additional motions that were not previously speci�ied. �owever, the algorithm is quite universal and allows to design collision–free, kinematically–feasible paths for any shape of a vehicle. In presented work the off–line pre–generated Motion Primitives are used and no additional motion genera‑ tion procedures are needed. The RRT* algorithm was used for path planning of a mobile robots [15], [3] and providing a complete and optimal solution. Unfortunately RRT* is not useful when the tasks or space is limited (limited motion set). Therefore the RRT* algorithm is not applicable in our research but RRT–A* is. In article [8] authors introdu‑ ced A–Star(A*) cost function to RRT algorithm. Similar approach is used in presented method with adapting it to the known motions set. The modi�ied RRT method is applied for path plan‑ ning of a humanoid robot in cluttered environment. The existing state–of–the–art approach is improved using the limited set of motion primitives. After the va‑ lid paths are found additional conditions are applied for excluding from the tree search the nodes without the improvement of assumed measure/criterion.

3. General Concept of Path Planning

When designing the path planner the a broader context must be taken into account. — The user de‑ �ines the task for a humanoid robot. For example the task can be ”pick an object from the table and place it on the shelf”. Figure 1 presents scenario for pick– and–place task divided into motion primitives. In this case user de�ines the task as a set of smaller sub– tasks: ”move robot to certain location”, ”pick an ob‑ ject”, ”move robot to another location”, ”place an ob‑ ject” and ”move robot to �inal location”. For simplicity, Fig.1 is not including the error handling and the optio‑ nal cases, eg. ”object is not available to pick”. The sub– tasks are further divided into motion primitives. While ”pick” and ”place” sub–tasks can be de�ined using pre– de�ined template, ”move to” sub–tasks signi�icantly differ from each other thus cannot be de�ined from template. Moving the robot from one place to another requires the work of the path planner. The path plan‑ ner should take into account not only obstacles avoi‑ dance but also kinematic constraints of the robot. It Articles

Journal of of Automation, Mobile Robotics andand Intelligent Systems Journal Automation, Mobile Robotics Intelligent Systems

VOLUME 15, 15, VOLUME

N° N°11 2021 2021

Fig. 3. Illustration of applied Motion Primitives (left picture) and example of foot placement for selected Motion Primitive (right picture) Tab. 1. Motion Primitives used in path planning algorithm. rstart = [0, 0′ ] and αstart = 0◦

1 2 3 4 5 6

rend [mm]

αend [◦ ]

[60, 0] [120, 0]′ [40, 40] [40, −40] [50, −30] [50, 30]

0 0 45 −45 30 −30

time [s] 11.5 16.5 36.5 36.5 26.5 26.5

steps 2 3 7 7 5 5

Vmean [ mm s ] 5.2174 7.2727 1.5498 1.5498 2.2004 2.2004

Fig. 4. Generated map with (left picture) from real scenario (right picture) should be also possible to modify a path in the real‑ time when an unexpected obstacle appears. Utilization of motion primitives in path planning drastically reduces the size of required on‑board me‑ mory and computational demand. Let us consider the area of 1.6m × 2m as our robot environment. The area is surrounded by walls and the robot is 55cm tall. Co‑ vering the whole environment by RRT is needed ap‑ proximately 2000 edges. Each edge takes from 8s up to 20s (approximately 14s) to be performed. The sam‑ pling rate of generated motion by MGS is 1kHz. The robot uses 20 actuators, the control signal for each mo‑ tor has resolution of 16 bits. Without optimization and not utilizing motion primitives it is needed 2000×14× 1000 × 20 × 16 = 8960000000bits ≈ 1.12T B of sto‑ red data. Not only storing but operating such amount 26

Articles

of data would be the considerable task. Of course such method of data representation, is not used in real–life applications. Such simple example illustrates the dis‑ advantage of poorly designed methods and emphasise the need for careful selection of motion primitives and demand for the ef�icient motion planning algorithms. In this work six motion primitives are applied. Just for those motion primitives 6 × 14 × 1000 × 20 × 16 = 3.36M B data are needed. If the motion primitives are generated off–line they can be stored and then used as a read–only data. In the path planning, as the Motion �rimitives the initial and �inal pose are used. These poses must be de�ined. Moreover the added around space regions used in collision detection function (a part of path planning algorithm) must be given. Such de�initions improve ef�iciency of path planner.

Journal of Automation, Mobile Robotics and Intelligent Systems Journal of Automation, Mobile Robotics and Intelligent Systems

When generating motion primitives the previously designed and evaluated Motion Generation System (MGS) [14] and [13] is used. Structure of this system is presented in �igure 2. Motion Primitives are using the motion parameters. After planning the feet placements by Motion Generation System (MGS), the feet transfer trajectories, trajectories of hands and ZMP reference trajectory are produced. Basis on reference trajectory of ZMP (Zero Moment Point) the CoM (Center of Mass) trajectory is obtained for which the ZMP criterion [16] is ful�illed. An inverse kinematic problem is solved and angular trajectories of the joints are obtained. Foot placements (footsteps) are produced by path planning algorithm in Trajectories Planner module. Applied Motion Primitives are presented in Fig. 3. �eft part of �igure presents the available motions. They are de�ined taking into account the unidirectio‑ nal on–board vision system. Such movements are pos‑ sible that the explored �ield of view (Fo�) allows to detect the obstacles. In the right side of the �igure the Motion Primitives as the sequence of steps are shown. The initial (rstart ,αstart ) and �inal position and orien‑ tation is denoted (rend ,αend ). For each Motion Primi‑ tive initial and �inal pose of the robot is the same – it is symmetrical towards the saggital plane of the robot with holding the feet parallel to each other. This allows immediately start the next Motion Primitive after the previous ends. Mean travelling distance for all Motion Primitives equals 4.13cm. Radius of acceptable tolerance towards the goal position is equal to 10cm (it is a bit more than 2 times bigger than the mean distance travelled). It must be noted that selecting too small number of Motion Primitives can cause various problems. First of all, due to the limited selection of available motions the overall robot path may become longer in terms of a distance and time than the path designed using more Motion Primitives. Secondly, in a case of narrow pas‑ sages the motion planning algorithm may not return a viable path. Finally, with poorly chosen Motion Pri‑ mitives the robot will not reach the goal moving bac‑ kward and forward with crossing but not reaching the goal.

4. Rapidly Explored Random Tree

Path planning is divided into two steps. First the tree with speci�ied number of nodes ktree is created. One node at a time is established as it is presented in �igure 5 (algorithm 1). Each node represents posi‑ tion and orientation of the robot in environment and the edge represent motion between two adjacent no‑ des (between two positions). Position is represented as the vector r which contains x and y coordinate in 2D plane. After building the tree, an optimal path is es‑ tablished by �inding the k–nearest nodes that satisfy the distance criterion given by Euclidean distance me‑ asured between qgoal and selected node being nearest than ε. In our algorithm we assumed that ε = 10cm. From set of all nodes selected in previous step, the one which has smallest execution time is selected. This time is computed as a sum of motions (edges) from any

VOLUME 15, N° 1 2021 VOLUME 15, N° 1 2021

given node to the starting node qstart . All motion exe‑ cution times are presented in table 1.

Fig. 5. Building tree T rooted in qstart by selecting qnear and adding new node in qnew

Algorithm 1 Algorithm for building tree T 1: 2: 3: 4: 5: 6: 7:

8: 9: 10: 11: 12: 13: 14: 15: 16: 17:

procedure BUILDTREE Input: initial con�iguration qstart , number of nodes ktree , region free from obstacles Qf ree , set of allowable motions ∆qmotions Output: Tree T if qstart ∈ Qf ree then T.init(qstart ) else return INIT_ERROR repeat qrand ← GET _RAN D_N ODE(Qf ree ) qnear ← N EAREST _N ODE(qrand , T ) qnew ← GET _N EXT _M OT ION (Qf ree , qnear , ∆qmotions ) if qnew ∈ Qf ree then T .add_vertex(qnew ) T .add_edge(qnear , qnew ) T.update_tree_inf o() until T .number_of_nodes() < ktree return T

The core pat of RRT algorithm is selection of three nodes: qrand , qnear and qnew . When �inding qrand node two approaches were used: biased and unbiased. Bia‑ sed approach differs from unbiased by returning qgoal node once each kbias –times function is stimulated. For large workspace this can be useful especially near the end of path search (when generated tree is few steps away from the goal con�iguration). We de�ine con�i‑ guration as the vector q which contains position and orientation of the robot. The bias reduces unnecessary random points in large workspace and focuses on goal con�iguration. For selecting qnear node two different approaches were used: Euclidean distance norm and A*–like heu‑ ristics. Euclidean distance between two con�igurations qA and qB is represented by: (1)

eucl_norm(qA , qB ) = ||rB − rA ||

where rA and rB are con�iguration positions. The are described by coordinates of the point located between two feet. This localisation is prede�ined and is speci�ic for each motion. Articles

Journal of Automation, Mobile Robotics and Intelligent Systems Journal of Automation, Mobile Robotics and Intelligent Systems

VOLUME 15, VOLUME 15,

N° 1 2021 N° 1 2021

Fig. 6. Example of generated tree with selected path for informed RRT algorithm. Left figure present tree without qgoal bias, while right presents the results obtained using the bias

Fig. 7. Example of generated tree path obtained using informed RRT algorithm with A* heuristics. Left figure presents tree without qgoal bias, while right presents it with the bias For implementing A*–like heuristics the standard form of A* algorithm is used: f (q) = g(q) + h(q)

(2)

In this equation g(q) represents the weight of the path between qstart and q. It is the time needed to exe‑ cute the path from start to the node representing con‑ �iguration q. h(q) is the weight (in terms of time) for the path segment from the given node describing con‑ �iguration q to the �inal con�iguration qend . In our ap‑ proach the weight h(q) is obtained using the following formula: h(q) = eucl_norm(q, qend )/(kmean ∗ Vmeana ll ) (3)

where Vmeana ll is the mean velocity of the robot along the executed path from qstart to the node repre‑ senting con�iguration q. kmean is the weight parameter used in f (q) function. 28

Articles

5. Experiments Presented algorithm was tested in the real scena‑ rio. Figure 4 (right side) presentes the testbed for the environment with the obstacles. Figure 4 (left side) is presenting automatically generated map obtained by vision system with ArUco markers [5]. In our ap‑ plication ArUco markers are used to localize the ob‑ stacles and goal destination. The coordinates of the point S are [210mm, 205mm]. Goal destinations are: A, B, C locations A = [521mm, 1404mm], B = [1589mm, 876mm], C = [1710mm, 1315mm]. Radius of acceptable tolerance towards the goal position is rgoal = 10cm.

List of parameters used for generating Motion Pri‑ mitives is given in Table 1. Using these parameters MGS generates the set of Motion Primitives used in path planning algorithm. Different motions are perfor‑ med with different speed. Comparing Vmean motion No.1 and No.3 from table 1 it can be seen that wal‑ king straight is faster than walking diagonally. It im‑

Journal Automation,Mobile MobileRobotics Roboticsand andIntelligent IntelligentSystems Systems Journal ofof Automation,

VOLUME 2021 VOLUME 15, 15, N° 1N° 12021

Tab. 2. Results of path time execution for analysed algorithm with specified parameters

qgoal A A A A B B B B C C C C

metrics

euclidean euclidean A* A* euclidean euclidean A* A* euclidean euclidean A* A*

kbias

tmin

tmean

tmax

nmin

nmean

nmax

0% 25% 0% 25% 0% 25% 0% 25% 0% 25% 0% 25%

534.5 528 522 478 504 489.5 461.5 484.5 501 491 473 479.5

733.22 689.23 658.75 618.55 706.24 674 641.7 627.7 673.4 659.5 625.5 616.3

1032 955 834.5 788 927.5 818 856 831 954 808 773 735

131 58 95 88 66 69 107 78 75 44 85 48

279 203 442 342 181 155 256 228 168 60 194 68

620 613 736 707 453 379 558 432 344 100 391 134

plies that ”zig–zagging” is more time consuming. Figure 6 presents the example of generated tree with selected path for informed RRT algorithm with (rigth picture) and without (left picture) the bias. In presented tree, nodes excluded from the search are marked by crosses and circles. Cross marks the nodes excluded by time limit, while circle marks nodes with dead—ends. Figure 7 presents example of generated tree with selected path using informed RRT algorithm with A* heuristics. Right picture presents version with bias while left without it. The reduced amount of nodes in the areas which are farther from the goal con�igura‑ tion can be observed. Figure 6 and �igure 7 provide the visible difference in the path curvature. Path in �igure 7 utilizes faster straight motions. The experiments are summarized in table 2. Each test (each row) was performed for ktree = 800 100 times. Goal destinations qgoal are represented in �igure 4 (left picture) as the A,B,C points. Analyzing the data given in 2 it can be concluded that the proposed algorithm provides signi�icant re‑ duction of tmin and tmean for generated path. In ma‑ jority of the cases tmax obtained for RRT–A* with bias was satisfactory. The values of nmin , nmean and nmax inform with what amount of nodes the connection with the goal was achieved. The Table is also presen‑ ting what ktree should be selected for successful obtai‑ ning of the path from the starting to �inal position. For all tests ktree = 800 provided the success. While RRT– A* reducs signi�icantly tmean it requirs bigger number of nodes in generated tree. This can lead to unsuccess‑ ful path planing result when the indicated path is not connecting the initial and �inal node. Further analysis and improvements should focus on this aspect.

are given and analyzed. Basis on it, it was concluded that the presented RRT algorithm with A* heuristics reduces the time needed for executing the path. The bias function used for generating qrand nodes reduces the size of the tree used for �inding the path (nmin ). The further work will be considering the variable ro‑ bot orientation in the path planning. It is obviously useful for the robots performing the range of tasks. Ad‑ ditionally, the set of movements for turning the robot on the spot should be generated.

This article describes the humanoid path plan‑ ning algorithm for cluttered environment. The pre– generated Motion Primitives are used. The incorpora‑ tion of of path planning algorithm to the control struc‑ ture of is shortly discussed. Key data of four algorithms

Teresa Zielinska – Warsaw University of Technology, Institute of Aeronautics and Ap‑ plied Mechanics, Nowowiejska 24, Warsaw, 00–665, e‑mail: teresaz@meil.pw.edu.pl, www: https://ztmir.meil.pw.edu.pl/web/Pracownicy/prof.‑

6. Conclusion

Although presented algorithm provides viable path for considered scenario, it is not free of limita‑ tions. First of all, with limited number of Motion Pri‑ mitives (such as in our paper) it is possible that the viable path will be not delivered e.g. a narrow corri‑ dor case. Secondly, with proposed set of Motion Pri‑ mitives it is not possible to re�ine the path as it can be done in RRT* algorithm versions. This may be over‑ come either by on‑line generation of the Motion Pri‑ mitives or by introducing more Motion Primitives. Fi‑ nally, a combination of Motion Primitives covering the short and long distances should be considered. ”Short length” Motion Primitives provide possibility for �ine adjustment of the robot position or deliver the mo‑ tion in narrow passages, while the ”long length” Mo‑ tion Primitives should be preferred globally (in open spaces) what decreases the computational effort.

AUTHORS Maksymilian Szumowski∗ – Warsaw Univer‑ sity of Technology, Institute of Aeronautics and Applied Mechanics, Nowowiejska 24, Warsaw, 00– 665, e‑mail: mszumowski@meil.pw.edu.pl, www: https://ztmir.meil.pw.edu.pl/web/Pracownicy/inz.‑ Maksymilian‑Szumowski.

Articles

Journal Mobile Robotics and Intelligent Systems JournalofofAutomation, Automation, Mobile Robotics and Intelligent Systems

Teresa‑Zielinska. ∗

Corresponding author

ACKNOWLEDGEMENTS The project was funded by POB Research Centre for Arti�icial Intelligence and Robotics of Warsaw Univer‑ sity of Technology within the Excellence Initiative Pro‑ gram ‑ Research University (ID‑UB).

REFERENCES

[1] J. Baltes, J. Bagot, S. Sadeghnejad, J. Anderson, and C.‑H. Hsu, “Full‑body motion planning for hu‑ manoid robots using rapidly exploring random trees”, KI ‑ Künstliche Intelligenz, vol. 30, no. 3‑4, 2016, 245–255, 10.1007/s13218‑016‑0450‑z.

[2] J. L. Blanco, M. Bellone, and A. Gimenez‑ Fernandez, “TP‑space RRT – kinematic path planning of non‑holonomic any‑shape vehi‑ cles”, International Journal of Advanced Robotic Systems, vol. 12, no. 5, 2015, 55, 10.5772/60463. [3] W. Chi and M. Q.‑H. Meng, “Risk‑RRT∗: A robot motion planning algorithm for the human robot coexisting environment”. In: 2017 18th Interna‑ tional Conference on Advanced Robotics (ICAR), 2017, 10.1109/icar.2017.8023670.

[4] J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot, “Informed RRT∗: Optimal sampling‑based path planning focused via direct sampling of an ad‑ missible ellipsoidal heuristic”. In: 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2014, 10.1109/iros.2014.6942976. [5] S. Garrido‑Jurado, R. Muñ oz‑Salinas, F. Madrid‑ Cuevas, and M. Marı́n‑Jimé nez, “Automatic ge‑ neration and detection of highly reliable �i‑ ducial markers under occlusion”, Pattern Re‑ cognition, vol. 47, no. 6, 2014, 2280–2292, 10.1016/j.patcog.2014.01.005. [6] E. Krotkov, D. Hackett, L. Jackel, M. Perschbacher, J. Pippine, J. Strauss, G. Pratt, and C. Orlowski, “The DARPA robotics challenge �inals: Results and perspectives”, Journal of Field Robotics, vol. 34, no. 2, 2016, 229–240, 10.1002/rob.21683.

VOLUME15, 15, N° N° 11 2021 VOLUME 2021

[10] M. Paetzel and L. Hofer, “The RoboCup humanoid league on the road to 2050 [competitions]”, IEEE Robotics & Automation Magazine, vol. 26, no. 4, 2019, 14–16, 10.1109/mra.2019.2945738.

[11] Z. Qiu, A. Escande, A. Micaelli, and T. Ro‑ bert, “A hierarchical framework for realizing dynamically‑stable motions of humanoid ro‑ bot in obstacle‑cluttered environments”. In: 2012 12th IEEE‑RAS International Conference on Humanoid Robots (Humanoids 2012), 2012, 10.1109/humanoids.2012.6651622. [12] E. Szadeczky‑Kardoss and B. Kiss, “Extension of the rapidly exploring random tree algo‑ rithm with key con�igurations for nonholono‑ mic motion planning”. In: 2006 IEEE In‑ ternational Conference on Mechatronics, 2006, 10.1109/icmech.2006.252554. [13] M. Szumowski and T. Zielinska, “Preview con‑ trol applied for humanoid robot motion ge‑ neration”, Archives of Control Sciences, 2019, 10.24425/ACS.2019.127526.

[14] M. Szumowski, M. S. Z� urawska, and T. Zieliń ska, “Simpli�ied method for humanoid robot gait ge‑ neration”. In: Advances in Mechanism and Ma‑ chine Science, 2019, 2269–2278, 10.1007/978‑ 3‑030‑20131‑9_224.

[15] Z. Tahir, A. H. Qureshi, Y. Ayaz, and R. Nawaz, “Potentially guided bidirectionalized RRT∗: for fast optimal path planning in cluttered environ‑ ments”, Robotics and Autonomous Systems, vol. 108, 2018, 13–27, 10.1016/j.robot.2018.06.013. [16] M. Vukobratović and B. Borovac, “ZERO‑ MOMENT POINT — THIRTY FIVE YEARS OF ITS LIFE”, International Journal of Huma‑ noid Robotics, vol. 01, no. 01, 2004, 157–173, 10.1142/s0219843604000083. [17] K. Yang, S. Moon, S. Yoo, J. Kang, N. L. Doh, H. B. Kim, and S. Joo, “Spline‑based RRT path planner for non‑holonomic robots”, Journal of Intelligent & Robotic Systems, vol. 73, no. 1‑4, 2013, 763– 782, 10.1007/s10846‑013‑9963‑y.

[7] S. M. LaValle, “Rapidly‑exploring random trees : a new tool for path planning”. In: Mathematics, 1998.

[8] J. Li, S. Liu, B. Zhang, and X. Zhao, “RRT‑a∗ mo‑ tion planning algorithm for non‑holonomic mobile robot”. In: 2014 Proceedings of the SICE Annual Conference (SICE), 2014, 10.1109/sice.2014.6935304.

[9] T. Nishi and T. Sugihara, “Motion planning of a humanoid robot in a complex environment using RRT and spatiotemporal post‑processing techniques”, International Journal of Huma‑ noid Robotics, vol. 11, no. 02, 2014, 1441003, 10.1142/s0219843614410035. 30

Articles

Journal of Automation, Mobile Robotics and Intelligent Systems Journal of Automation, Mobile Robotics and Intelligent Systems

VOLUME 15, N° 1 2021 VOLUME 15, N° 1 2021

HUBERO – A FRAMEWORK TO SIMULATE HUMAN BEHAVIOUR IN ROBOT RESEARCH Submitted: 21st June 2020; accepted: 14th March 2021

Jarosław Karwowski, Wojciech Dudek, Maciej Węgierek, Tomasz Winiarski DOI: 10.14313/JAMRIS/1‐2021/4 Abstract: Social robots’ software is commonly tested in a simula‐ tion due to the safety and convenience reasons as well as an environment configuration repeatability assurance. An interaction between a robot and a human requires ta‐ king a person presence and his movement abilities into consideration. The purpose of the article is to present the HuBeRo framework, which can be used to simulate hu‐ man motion behaviour. The framework allows indepen‐ dent control of each individual’s activity, which distinguis‐ hes the presented approach from state‐of‐the‐art, open‐ source solutions from the robotics domain. The article presents the framework assumptions, architecture, and an exemplary application with respect to presented sce‐ narios. Keywords: human simulation, HRI, social robotics, robot control

1. Introduction The society of the modern world is ageing. This phenomenon applies not only to countries of the old continent but also those with such a large population as China. Therefore, the number of young people who would be able to support the elderly decreases. Social robotics concerns this problem by creating solutions that can support people in carrying out their daily acti‑ vities [3, 8, 18]. One of the problems that must be solved when de‑ signing a social robot is a navigation in a dynamically changing environment. The challenge, which resear‑ chers must face, is to evolve algorithms that provide robot collision avoidance concomitantly with a mo‑ vement pattern that is convenient for surrounding pe‑ ople [9, 17]. To be able to determine if the robot meets this requirement, many tests must be performed. Modern robotic systems are essayed in simulation environments [23]. Such an approach for evaluation allows creating the entire robot system more effecti‑ vely and safely. The simulator should take into account the kinematics and dynamics of the robot as well as the existence of static and dynamic elements of the en‑ vironment. One particular example of such a dynamic element is a simulated human, which should be con‑ sidered not only as an object that is able to change its position, but also behaves in a certain way [20, 24]. The problem of human behaviour modelling is not trivial due to the enormous complexity of the impera‑ tive that guides people while moving. Since its descrip‑ tion in a strictly mathematical way is impossible [5],

a qualitative evaluation is straitened. Moreover, a hu‑ man movement manner may differ among different ethnic and age groups. To the best of the authors’ kno‑ wledge, there is no comprehensive solution to this problem. The purpose of this article is to present HuBeRo framework that allows a control of simulated indivi‑ duals on a simple movement tasks level. Commanding of a simulated human is executed according to a spe‑ ci�ic scenario, designed concerning an environment structure. Both scenario and environment, related to a speci�ic application, are designed by an end user. The aim of the HuBeRo is to assist robots’ soft‑ ware designer during the control system development process. Our work proposes an architecture of the system and presents test scenarios taking advantage of a novel, socially compatible local navigation algo‑ rithm. Although the path calculation is performed with a use of the custom method, it is out of the scope of this paper as its description is complex. Moreover, we do not evaluate the paths quantitatively in this work, because our objective is to point out the framework it‑ self. The main signatures, which distinguish HuBeRo from other frameworks for human behaviour simula‑ tion, are� possibility to create application–speci�ic sce‑ narios using the prede�ined functions, ability to cont‑ rol individuals – instead of a whole collective, and soft‑ ware modularity. The paper structure is as follows. First, state of the art solutions are depicted (sec. 2). Next, the HuBeRo framework assumptions (sec. 3) and architecture (sec. 4) are described. Then, the test scenarios are presented (sec. 5). The paper ends up with conclusions (sec. 6).

2. Related Work

State‑of‑the‑art works considering human motion behaviour simulation frameworks focus on different aspects. Pan et. al [16] proposed the MASSEgress com‑ putational framework that incorporates a pattern– based model in which agents are allowed to select the appropriate type of behaviour based on e.g., the sur‑ rounding environment and experience. Since it is de‑ rived from pattern–based model – it does not allow to control individuals separately. Berg et. al [25] pre‑ sented an approach for interactive navigation of mul‑ tiple agents in crowded scenes. The formulation em‑ ploys a pre‑computed roadmap ‑ providing macrosco‑ pic connectivity, aggregated with a navigation for each agent. Agents’ environment perception allows them to

Journal of Journal of Automation, Automation, Mobile MobileRobotics Roboticsand andIntelligent IntelligentSystems Systems

compute a collision‑free path based on i.a. extended Velocity Obstacles. Schmitz et. al [22] proposed a no‑ vel framework intended for various types of robots – SimVis3D. The authors focused on the introduction of an animated human character into the software. The movement of the person is planned using splines. The tool veri�ication is performed via an exemplary sce‑ nario. Koh et. al [10] introduced an agent–based sy‑ stem, which replicates pedestrian behaviours taking a pedestrian’s sensory, attention, memory, and navi‑ gational behaviors into consideration. They designed a two‑level navigation model in order to generate such behaviours as: overtaking, waiting, sidestepping, and lane‑forming in a crowded area. Reed et. al [21] desig‑ ned a framework that was comprehensively validated, e.g., [28], and is widely used in the research conducted by the SAE1 . The software allows to simulate physi‑ cal posture and motion. The framework’s architecture concerns from low‑level joint manipulation to a com‑ plex tasks execution. It was achieved via a coordina‑ tion of individual modules (creating a single agent) that communicate with each other. However, the soft‑ ware is available only for the commercial use. All these works are similar in regards to a con‑ ceptual design of a framework – implement systems based on agents that are aware of interactions and environment structure. However, inputs and outputs of computation blocks for human behaviour calcula‑ tion and level of knowledge about the environment usually differ. Our approach is designed from the per‑ spective of individuals. It provides pedestrian hetero‑ geneity and hence can be categorized as a microscopic simulation model. The HuBeRo framework has built‑ in interface to a popular robotic framework, which ma‑ kes it ready‑to‑use in a robotics research domain.

3. Framework Assumptions

VOLUME 2021 VOLUME 15,15, N°N°11 2021

(e) asserts the integration with various simulation software and motion controllers or planners,

(f) provides a graphical representation of the envi‑ ronment entities.

4. Framework Architecture

The HuBeRo framework description is based on the simple Environment ontology (Fig. 1) that is used as a dictionary of basic concepts. The basic entity is the Spatial Object, with its geometrical attributes, e.g., a shape and pose – expressed in the Environment coordinate system. Considering Object’s movement ability, Movable Spatial Object was distinguished. The very special Movable Spatial Object is a Human that owns two values: poseCurrent and poseGoal, stan‑ ding for operational parameters for the Human action. Four operations were de�ined – each of them is associ‑ ated with one of the basic Human movement control‑ ling functions of the HuBeRo framework.

Fig. 1. Environment ontology

The framework requirements are de�ined so as to allow the introduction of virtual people into a simula‑ tion and controlling them in such a way to re‑enact ty‑ pical scenarios for social robotics research. Moreover, the framework should support the end user in qualita‑ tive grading of a system operation and allow to adjust execution manner for a speci�ic case. As the HuBeRo framework is not a standalone soft‑ ware, two groups of requirements were demarcated. The �irst group is related to the HuBeRo itself ((a)‑(e)), whereas the second one – to the system incorporating the HuBeRo ((f)). The requirements are as follows: (a) provides an API allowing an individual Human control in a simulation,

(b) allows the creation of scenarios that incorporate Human movement activities [4], (c) allows the simulation parameters modi�ication e.g., a shape of a personal space [6], motivation to reach the goal, maitaining appetible speed or desire to avoid collisions [7],

(d) allows the collection of the human path points (for analysis of human paths, depending on the si‑ mulation parameters), Articles

Fig. 2. The composition of HuBeRo Simulation System A speci�ication of the HuBeRo framework uses the EARL language (version 1.1) [1, 27] that basis on the

Journal of of Automation, Automation, Mobile Journal Mobile Robotics Roboticsand andIntelligent IntelligentSystems Systems

Embodied Agent approach [12, 29–31]. In our appro‑ ach, the Human and Robot Simulation System can be composed of multiple Human and Robot agents that coexist in the shared Environment. The framework de�ines a group of CERT‑type Agents aa and inter Agent communication Links that are used for transferring data between aa and other Agents (Figs.2 and 3). Each aa corresponds to a Hu‑ man in the Environment.

VOLUME 2021 VOLUME 15,15, N°N° 1 1 2021

A System structured in that way allows to synchronise Agents operation, utilizing an action status feedback from both types of Agents. Currently, |aac| ≤ |aa| is required due to the in‑ ability of a single Human to schedule commands co‑ ming from more than one source. On the other hand, a single aac can send commands to more than one aa. The number of aac Agents in the System must be de‑ termined by the end user at the scenario design stage. The |aac| depends on the requirement, whether the scenario control is to be centrally located or distribu‑ ted. The aa structure is presented in Fig. 4. A Human perception (tr and mr Subsystems) is comprehensive in terms of geometrical attributes of Spatial Objects (e.g. position) and dynamic attributes of Movable Ob‑ jects (e.g., position and velocity). The cs Subsystem executes a local navigation algorithm necessary to perform the Human movements in the Environment. The control loop ends up with the act of me and te Subsystems that update the Human pose value in a si‑ mulated world.

Fig. 3. General structure and communication of the HuBeRo framework with associated Agents The framework provides communication of aa with following Agents: ‑ aac (CT‑type) – de�ines a set of tasks in a speci�ic or‑ der (scenario) to be performed by a Human, ‑ av (CET‑type) – provides a graphical representation of the Human cognition – related to, e.g., its state and attributes like position or personal zone, ‑ agp (CT‑type) – performs a global path calculation on a aa request.

Fig. 5. aa.cs.f sm definition

Fig. 4. aa internal structure definition The aac can also be interfaced with Robot’s cs [26].

The operation of aa.cs is presented in Fig. 5 by a Finite State Machine. In the idle/bb state the Human maintains its pose and waits for a new mo‑ tion command arrival. The moveToGoal/s state rea‑ lises the behaviour of reaching a certain goal (mo‑ vement from poseCurrent to poseGoal). The HuBeRo framework does not name any speci�ic algorithm of this behaviour transition function execution, as many different navigation algorithms can be adopted. The act of the behaviour associated with moveAround/s is similar to the previous one, but after reaching a certain goal, a new poseGoal is drawn randomly. The point is selected within the prede�ined Environ‑ ment bounds. As the Control Subsystem operates in followMovableSpatialObject/s state, the Human fol‑ lows another Movable Spatial Object. What distinguis‑ hes this state from previously mentioned ones is that a predicted path to the Object must be updated perio‑ dically. The second difference is that after the Human Articles

Journal of Automation, Mobile Robotics and Intelligent Systems Journal of Automation, Mobile Robotics and Intelligent Systems

VOLUME 15, N° 1 2021 VOLUME 15, N° 1 2021

has moved close enough to the Movable Spatial Object, the behaviour is not terminated, but the Human waits until the Object moves away. The HuBeRo framework is equipped with a wide range of parameters de�ined in aa.cs, e.g., a shape of the personal space or a degree of desire to avoid colli‑ sions. �e state that for a �ixed Environment con�igura‑ tion, the paths taken by a Human differ depending on the parameters’ values.

5. Test Scenarios

The structure of the exemplary HuBeRo applica‑ tion is shown in Fig. 6. Description of the application is supplemented with requirements references (sec. 3) in the R − (requirement label) form. Fig. 8. Sequence of aa.cs.f sm FSM States transitions in the parking scenario planner module [13]. Moreover, the Rviz was selected as a visualisation tool, which stands for another de‑ pendency of the HuBeRo framework. The Rviz is used for grading the framework qualitatively. Commanding of the simulated Humans is accomplished via the cu‑ stom ROS [19] framework applications with a use of the HuBeRo‑ROS interface (R‑ (a)).

Fig. 6. An exemplary application of the HuBeRo framework with its dependencies

Fig. 9. Representation of the living room Environment used in the test scenario. Screenshot obtained with a use of the Gazebo simulator Fig. 7. Sequence of aa.cs.f sm FSM States transitions in the living room scenario

As a base simulation software the Gazebo is used (R‑ (e)). The Gazebo provides all necessary Environment–related input data, along with a 3D re‑ presentation of the Environment (R‑ (f)). The HuBeRo framework itself introduces a novel local planning al‑ gorithm, which is supported by the external global Articles

To evaluate the proposed simulation framework the two speci�ic scenarios were designed (R‑ (b)). SysML activity diagrams were used to indicate the se‑ quence of aa.cs.fsm state changes for the simulated ac‑ tors. The Environment con�iguration (at the end of the scenario) along with paths executed during the opera‑ tion (R‑ (d)) and the personal space shapes are pre‑ sented in the screenshots. The desired speed of pede‑ strians in all cases was set to 1.29 m/s [15], whereas speed limits arise from object’s dynamics.

Journal of Automation, Mobile Robotics and Intelligent Systems Journal of Automation, Mobile Robotics and Intelligent Systems

Fig. 10. Representation of the parking Environment used in the test scenario. Screenshot obtained with a use of the Gazebo simulator The �irst scenario (Fig. 7) is realised in the living room with 2 Humans – actor1/aa and actor2/aa. The initial Environment con�iguration is shown in Fig. 9. At �irst, actor2/aa walks over to actor1/aa and invi‑ tes him to eat a dinner. Later on, both actor1/aa and actor2/aa go to the table surroundings. The same sce‑ nario was executed 2 times� for the �irst time Humans had a personal space modelled as a circle (Fig. 11) with a radius of 0.20 m, whereas in the second case it was an ellipse with semi–axes of 1.10 m and 0.80 m (Fig. 12). This experiment shows that the requirement (c)) has been met. The second scenario (Fig. 8) is realised in the parking Environment containing 4 Humans – actor1/aa, ..., actor4/aa (Fig. 10). Each Human starts his walk around the table and tries to get to his car. However, only 2 Humans go straight ahead to the destination, others decide to interrupt their trip. The reason behind is that actor1/aa must throw away rub‑ bish and actor2/aa meets a friend standing around the corner. In this case, during the �irst simulation, Humans had a personal space modelled as an axis‑ aligned square with side lengths of 0.25 m (Fig. 13), whereas in the second run the personal space shape was a circle with a radius of 0.70 m (Fig. 14). A vi‑ deo presentation of both scenarios performance is available online 2 . The differences in paths executed by Humans are noticeable in both scenarios. More visible differen‑ ces occurred in the parking, where the enlargement of personal space size disallowed Human to pass through the small aisle (Fig. 10). The only parameter that was changed during the execution of the scenarios was the conformations of the Humans’ personal spaces. Alternative outcomes can be achieved by the rearrangement of other lo‑ cal planner parameters. The results described above show that a wide class of scenarios can be mapped into

VOLUME 15, N° 1 2021 VOLUME 15, N° 1 2021

Fig. 11. Humans paths executed in the living room scenario – circular personal zone. Screenshot obtained with a use of the Rviz visualisation tool

Fig. 12. Humans paths executed in the living room scenario – elliptical personal zone. Screenshot obtained with a use of the Rviz visualisation tool a simulation.

6. Conclusions The HuBeRo framework introduces a detailed so‑ lution for a human behaviour simulation. It allows controlling individual humans during the execution of pre‑de�ined motion scenarios. Although the HuBeRo was designed mainly for the execution of speci�ic cases related to social robotics research, the API allows to create many sophisticated scenarios. Providing a me‑ tric map of an environment, the end user is able to �la‑ wlessly test custom scripts in different surroundings. The main restriction of the framework is a limited set of commands, de�ining simple tasks for human lo‑ comotion. Moreover, the in‑depth knowledge of para‑ Articles

Journal of Automation, Mobile Robotics and Intelligent Systems Journal of Automation, Mobile Robotics and Intelligent Systems

VOLUME 15, N° 1 2021 VOLUME 15, N° 1 2021

The HuBeRo framework is planned to be used in the INCARE project [2] as an environment for test sce‑ narios including fall prevention, transportation atten‑ dance, hazard detection, and guarding.

Notes

1 Society of Automotive Engineers https://sae.org

2 Video presentation of https://vimeo.com/397552304

the

scenarios

execution:

AUTHORS

Fig. 13. Humans paths executed in the parking scenario – rectangular personal zone. Screenshot obtained with a use of the Rviz visualisation tool

Jarosław Karwowski∗ – Warsaw University of Technology, Institute of Control and Computation Engineering, 00–665 Warszawa, ul. Nowowiejska 15/19, e‑mail: j.karwowski@stud.elka.pw.edu.pl, www: https://www.robotyka.ia.pw.edu.pl/. Wojciech Dudek – Warsaw University of Techno‑ logy, Institute of Control and Computation En‑ gineering, 00–665 Warszawa, ul. Nowowiejska 15/19, e‑mail: wojciech.dudek@pw.edu.pl, www: https://www.robotyka.ia.pw.edu.pl/. Maciej Węgierek – Warsaw University of Techno‑ logy, Institute of Control and Computation En‑ gineering, 00–665 Warszawa, ul. Nowowiejska 15/19, e‑mail: m.wegierek@elka.pw.edu.pl, www: https://www.robotyka.ia.pw.edu.pl/. Tomasz Winiarski – Warsaw University of Techno‑ logy, Institute of Control and Computation En‑ gineering, 00–665 Warszawa, ul. Nowowiejska 15/19, e‑mail: tomasz.winiarski@pw.edu.pl, www: https://www.robotyka.ia.pw.edu.pl/. ∗

Corresponding author

ACKNOWLEDGEMENTS

Fig. 14. Humans paths executed in the parking scenario – circular personal zone. Screenshot obtained with a use of the Rviz visualisation tool

meters is obligatory to achieve a certain human beha‑ viour in a speci�ic scenario. The advantage of the HuBeRo framework is that its modules are interchangeable. The local planning algorithm, integrated with the motion controller, can be replaced with another state‑of‑the‑art imple‑ mentation. The main goal of future work is to evaluate, how much the paths generated in a simulated world re�lect the ones executed by people in a real life. For compa‑ rison purposes, human localization techniques would be useful [11]. Moreover, extensions of a motion con‑ troller that would take into account complex human behaviours can be created based on human activities recognition algorithms [14]. Articles

This work was funded within the INCARE AAL‑2017‑ 059 project „Integrated Solution for Innovative Elderly Care” by the AAL JP and co‑funded by the AAL JP coun‑ tries (National Centre for Research and Development, Poland under Grant AAL2/2/INCARE/2018).

REFERENCES

[1] EARL – Embodied Agent‑Based Robot Control Sys‑ tems Modelling Language ‑ version 1.1 ‑ reference manual, March 2020. [2] “INCARE project page at WUT”, March 2020.

[3] J. Andrade, P. Santana, and A. P. Almeida, “Self‑ supervised learning of motion‑induced acoustic noise awareness in social robots”, Journal of Au‑ tomation, Mobile Robotics and Intelligent Sys‑ tems, vol. 13, no. 1, 2019, 10.14313/JAMRIS_1‑ 2019/1. [4] W. Dudek, M. Węgierek, J. Karwowski, W. Szyn‑ kiewicz, and T. Winiarski, “Task harmonisation for a single–task robot controller”. In: K. Ko‑ złowski, ed., 12th International Workshop on Ro‑ bot Motion and Control (RoMoCo), 2019, 86–91, 10.1109/RoMoCo.2019.8787385.

Journal of Automation, Mobile Robotics and Intelligent Systems Journal of Automation, Mobile Robotics and Intelligent Systems

[5] E. Frydenlund, T. Elzie, A. Collins, and R. Robin‑ son, “A hybridized approach to validation: The role of sociological research methods in pede‑ strian modeling”, Transportation Research Proce‑ dia, vol. 2, 2014, 10.1016/j.trpro.2014.09.077.

[6] E. T. Hall, The hidden dimension, Garden City, N.Y., Doubleday, 1966. [7] D. Helbing and P. Molnar, “Social force model for pedestrian dynamics”, Physical Review E, vol. 51, no. 5, 1995, 10.1103/PhysRevE.51.4282.

[8] J. Kędzierski, P. Kaczmarek, M. Dziergwa, and K. Tchoń , “Design for a robotic com‑ panion”, International Journal of Huma‑ noid Robotics, vol. 12, 2015, 1550007–1, 10.1142/S0219843615500073. [9] R. Kirby, R. Simmons, and J. Forlizzi, “Companion: A constraint‑optimizing method for person‑ acceptable navigation”. In: RO‑MAN 2009 ‑ The 18th IEEE International Symposium on Robot and Human Interactive Communication, 2009, 607– 612, 10.1109/ROMAN.2009.5326271.

[10] W. Koh and S. Zhou, “Modeling and simulation of pedestrian behaviors in crowded places”, ACM Trans. Model. Comput. Simul., vol. 21, 2011, 20, 10.1145/1921598.1921604.

[11] J. Kolakowski, V. Djaja‑Josko, M. Kolakowski, and J. Cichocki, “Localization system supporting pe‑ ople with cognitive impairment and their caregi‑ vers”, International Journal of Electronics and Te‑ lecommunications, vol. 66, no. 1, 2020, 125–131, 10.24425/ijet.2020.131853. [12] T. Kornuta, C. Zieliń ski, and T. Winiarski, “A universal architectural pattern and speci�ica‑ tion method for robot control system design”, Bulletin of the Polish Academy of Sciences: Technical Sciences, vol. 68, no. 1, 2020, 3–29, 10.24425/bpasts.2020.131827. [13] D. Lu. “A path planner library and node.”. http:// wiki.ros.org/global_planner. [Accessed on: 2021‑06‑21]. [14] I. Mocanu, D. Axinte, O. Cramariuc, and B. Cra‑ mariuc, “Human activity recognition with con‑ volution neural network using tiago robot”. In: 2018 41st International Conference on Telecom‑ munications and Signal Processing (TSP), 2018, 1–4, 10.1109/TSP.2018.8441486.

[15] M. Moussaı̈d, D. Helbing, S. Garnier, A. Johans‑ son, M. Combe, and G. Theraulaz, “Experimental study of the behavioural mechanisms underlying self‑organization in human crowds”, Proceedings of the Royal Society B, vol. 276, 2009, 2755–2762, 10.1098/rspb.2009.0405. [16] X. Pan, C. S. Han, K. Dauber, and K. H. Law, “A multi‑agent based framework for the simula‑ tion of human and social behaviors during emer‑ gency evacuations”, AI & Society, vol. 22, 2007, 113–132, 10.1007/s00146‑007‑0126‑1.

VOLUME 15, N° 1 2021 VOLUME 15, N° 1 2021

[17] N. Piçarra, J.‑C. Giger, G. Pochwatko, and J. Mo‑ zaryn, “Designing social robots for interaction at work: Socio‑cognitive factors underlying inten‑ tion to work with social robots”, Journal of Auto‑ mation, Mobile Robotics and Intelligent Systems, vol. 10, no. 4, 2016, 17–26, 10.14313/JAMRIS_4‑ 2016/28.

[18] J. Piasek and K. Wieczorowska‑Tobis, “Accep‑ tance and long‑term use of a social robot by elderly users in a domestic environment”. In: 11th International Conference on Human System Interaction (HSI), 2018, 478–482, 10.1109/HSI.2018.8431348. [19] M. Quigley, K. Conley, B. Gerkey, J. Faust, T. Foote, J. Leibs, R. Wheeler, and A. Y. Ng, “ROS: an open‑ source Robot Operating System”. In: ICRA Works‑ hop on Open Source Software, 2009.

[20] V. S. Rajpurohit and M. Manohara Pai, “Motion prediction of moving objects in a robot naviga‑ tional environment using fuzzy‑based decision tree approach”, Journal of Automation, Mobile Ro‑ botics and Intelligent Systems, vol. 4, no. 4, 2010, 11–18.

[21] M. Reed, J. Faraway, D. Chaf�in, and B. Martin, “The humosim ergonomics framework: A new approach to digital human simulation for er‑ gonomic analysis”, SAE Technical Papers, 2006, 10.4271/2006‑01‑2365. [22] N. Schmitz, J. Hirth, and K. Berns, “A simulation framework for human‑robot interaction”. 2010, 79–84, 10.1109/ACHI.2010.28.

[23] D. Seredyń ski, K. Banachowicz, and T. Wi‑ niarski, “Graph‑based potential �ield for the end‑effector control within the torque‑based task hierarchy”. In: 2016 21st International Conference on Methods and Models in Auto‑ mation and Robotics (MMAR), 2016, 645–650, 10.1109/MMAR.2016.7575212. [24] S. Sonoh, S. Aou, K. Horio, H. Tamukoh, T. Koga, and T. Yamakawa, “A human robot interaction by a model of the emotional learning in the brain”, Journal of Automation, Mobile Robotics and Intel‑ ligent Systems, vol. 4, no. 2, 2010, 48–54. [25] J. van den Berg, S. Patil, J. Sewall, D. Manocha, and M. Lin, “Interactive navigation of individual agents in crowded environments”, 2008.

[26] T. Winiarski, W. Dudek, M. Stefań czyk, L. Zie‑ liń ski, D. Giełdowski, and D. Seredyń ski, “An intent‑based approach for creating assis‑ tive robots’ control systems”, arXiv preprint arXiv:2005.12106, 2020. [27] T. Winiarski, M. Węgierek, D. Seredyń ski, W. Du‑ dek, K. Banachowicz, and C. Zieliń ski, “EARL – Embodied Agent‑Based Robot Control Systems Modelling Language”, Electronics, vol. 9, no. 2, 2020, 379, 10.3390/electronics9020379. [28] W. Zhou and M. Reed, “Validation of the hu‑ man motion simulation framework: Posture pre‑ Articles

Journal of Journal of Automation, Automation, Mobile MobileRobotics Roboticsand andIntelligent IntelligentSystems Systems

diction for standing object transfer tasks”, SAE Technical Papers, 2009, 10.4271/2009‑01‑2284.

[29] C. Zieliń ski, T. Kornuta, and T. Winiarski, “A sys‑ tematic method of designing control systems for service and �ield robots”. In: 19‑th IEEE Interna‑ tional Conference on Methods and Models in Au‑ tomation and Robotics, MMAR’2014, 2014, 1–14, 10.1109/MMAR.2014.6957317.

[30] C. Zieliń ski, M. Stefań czyk, T. Kornuta, M. Figat, W. Dudek, W. Szynkiewicz, W. Kasprzak, J. Figat, M. Szlenk, T. Winiarski, K. Banachowicz, T. Zieliń ‑ ska, E. G. Tsardoulias, A. L. Symeonidis, F. E. Pso‑ mopoulos, A. M. Kintsakis, P. A. Mitkas, A. Thal‑ las, S. E. Reppou, G. T. Karagiannis, K. Panayiotou, V. Prunet, M. Serrano, J.‑P. Merlet, S. Arampat‑ zis, A. Giokas, L. Penteridis, I. Trochidis, D. Daney, and M. Iturburu, “Variable structure robot cont‑ rol systems: The RAPP approach”, Robotics and Autonomous Systems, vol. 94, 2017, 226 – 244, 10.1016/j.robot.2017.05.002. [31] C. Zieliń ski, T. Winiarski, and T. Kornuta, “Agent‑ based structures of robot systems”. In: K. J. and et al., eds., Trends in Advanced Intelligent Cont‑ rol, Optimization and Automation, vol. 577, 2017, 493–502, 10.1007/978‑3‑319‑60699‑6_48.

Articles

VOLUME 2021 VOLUME 15,15, N°N°11 2021

JournalofofAutomation, Automation, Mobile Mobile Robotics Robotics and Journal and Intelligent IntelligentSystems Systems

VOLUME 2021 VOLUME 15, 15,N° N° 1 1 2021

A NEW APPROACH TO THE HISTOGRAM‐BASED SEGMENTATION OF SYNTHETIC APERTURE RADAR IMAGES Submitted: 6th May 2020; accepted: 14th March 2021

Barbara Siemiatkowska, Krzysztof Gromada DOI: 10.14313/JAMRIS/1‐2021/5 Abstract: Radar machine vision is an emerging research field in the mobile robotics. Because Synthetic Aperture Radars (SAR) are robust against weather and light condition, they pro‐ vide more useful and reliable information than optical images. On the other hand, the data processing is more complicated and less researched than visible light images processing. The main goal of our research is to build sim‐ ple and efficient method of SAR image analysis. In this ar‐ ticle we describe our research related to SAR image seg‐ mentation and attempts to detect elements such as the buildings, roads and forest areas. Tests were carried out for the images made available by Leonardo Airborne & Space System Company.

dangerous rescue operations, airborne surveillance, product transport, military purposes but also autono‑ mous cars. Such vehicles should be equipped with sen‑ sors that operate reliably regardless of weather con‑ ditions and time of a day. Modern autonomous vehi‑ cles, self‑driving cars in particular, usually incorporate three kinds of sensors: camera, LiDAR and radar. Li‑ DAR can accurately identify objects and their position, but it is expensive and susceptible to weather conditi‑ ons. Since 1978, Synthetic Aperture Radars (SAR) has been widely used for Earth remote sensing.

Keywords: SAR image, machine vision

Fig. 2. Geometrical explanation of shadow appearance on SAR scans. Shadow area is marked with violet color, building – orange and ground – green

Fig. 1. Sample image: a ‐ SAR image [source: Leonardo Airborne & Space System], b ‐ Google Map

1. Introduction Nowadays, autonomous vehicles: cars, planes and drones are rapidly gaining popularity in both scien‑ ti�ic research and practical applications, that involve

SARs use doppler effect to determine angle of re‑ ceived echo of electro‑magnetic pulses (usually emit‑ ted by a built‑in exciter, but can also use an external signal generator). Due to its operating principle SAR requires relative movement of a scanned object and a radar. Therefore SARs are usually mounted on mo‑ ving platforms (typically a plane or satellite). The sen‑ sor provides high‑resolution images which are inde‑ pendent of daylight and weather conditions [1]. The SAR system emits electromagnetic waves and receives altered echoes. The transmitted signal inte‑ racts with the Earth surface, and a portion of it is back‑ scattered. The amplitude and phase of the echo de‑ pend on the properties (geometry, permittivity) of the surface and its position in regard to the sensor. The amplitude of SAR image represents a measure of the scene re�lectivity. The pulse frequency depends on the application. It ranges from single Megahertz for the spaceborne sys‑ tems to hundreds of Gigahertz for the airborne appro‑ ach. In case of the satellite systems, a single SAR scan provides information on an area of up to 400 km2 . SAR has a wide range of applications: from planetary ex‑ ploration, Earth monitoring, climate change research

Journal of Journal of Automation, Automation, Mobile MobileRobotics Roboticsand andIntelligent IntelligentSystems Systems

to agriculture, and military. Radars are less expensive than LiDARs, but they are also less accurate. Many companies try to develop high‑resolution radar and research is ongoing on the use of SAR radars in driver assistance systems [4]. SAR images are robust against different weather or day‑ light conditions [6], but they are usually affected by speckle noise [12], distortion effects like shadowing and high local contrasts in intensity. SAR images are challenging to interpret, their preprocessing and high level processing are very demanding tasks [11]. A SAR image is a valuable source of information about envi‑ ronment but it is very dif�icult to analyze. Fig. 1 presents the SAR image of an area 7.33 km × 3.19 km and corresponding Google Map. Each pixel represents 1 m × 1 m square. The image is very noisy, but the distinctive shadow effects can be seen. Various image processing algorithms are used to support image analysis as well as signi�icantly expand radar capabilities. The best‑known is the Coherent Change Detection (CCD) algorithm [8], which allows us to detect minor differences in the height of the scan‑ ned area based on phase difference between two scans of the same area. CCD makes use of complex repre‑ sentation of SAR output (considering not only the am‑ plitude of re�lection, but also its phase). �ith current technology it gives resolution of up to 30 degrees of phase change (which for typical 3 GHz frequency – 10 cm wavelength – means around 30/360 · 10 cm = 8 mm) resolution. However, when analyzing an individual image, it is very dif�icult to determine the type of surface of the observed objects (especially not man‑made) or to de‑ �ine the boundaries between areas�objects. Threshol‑ ding method [9] is usually used for image segmenta‑ tion. It is easy to implement, but provides low accu‑ racy. Clustering methods [13] group pixels with simi‑ lar properties and separate pixels which are different, thus method ensures higher accuracy but requires prior knowledge on expected parameters. Some aut‑ hors propose to use deep learning neural networks [7] or SVM [14] classi�iers for object recognition. Those methods require a big set of data, but without large public databases containing SAR images they are not available for civil or scienti�ic use. The main goal of our resarch is to build ef�i‑ cient method of SAR data analyzis. In this article, we describe our �irst steps towards new approach to SAR image segmentation and object classi�ication. Our method combines adaptive thresholding and feature‑ based approach.

2. Image Analysis

2.1. Image Histogram

Unlike optical cameras, radar output image is not tightly related to its position during data gathering. Output scan is created by casting calculated amplitu‑ des (and phases) of response on a plane (or sphere). Scan is virtually always a view from above of scan‑ ned area. This introduces an unintuitive effect of vi‑ sible areas that were not scanned and thus are dark Articles

VOLUME 2021 VOLUME 15,15, N°N°11 2021

shadows in the output image.

Fig. 3. Image histogram: a ‐ raw image, b ‐ filtered image Fig. 2 presents the idea behind shadow creation. Radar needs to excite objects in area and receive their echo. Objects behind material that blocks electromag‑ netic waves do not backscatter a measurable amount of energy. For example, plastic does not block elec‑ tromagnetic pulses. It is not detected by radar and casts no shadow on the image while we can notice the shadows near a building. Knowing the scan angle α (often called look down angle) and measuring the shadow length lshadow (Fig. 2), building height hbuilding can be determined using simple equation 1. HBuilding = tan(α) · LShadow

(1)

Shadowing and high local contrasts in intensity ef‑ fects mostly appear in SAR images of urban areas, and they are essential sources of information that are not available in optical images. For example, bright lines indicate the boundaries of buildings, and by analyzing shadows, it is possible to obtain a precise measure‑ ment of the height of an object. Our goal is to perform image segmentation and to determine the area occupied by roads and buildings. Usually the process of image analysis consists of the following modules [3]: ‑ noise reduction ‑ segmentation

‑ feature extraction ‑ �inal decision

Journal of Automation, Mobile Robotics and Intelligent Systems Journal of Automation, Mobile Robotics and Intelligent Systems

VOLUME 15, N° 1 2021 VOLUME 15, N° 1 2021

formed several times. After completing this opera‑ tion, we can notice the formation of peaks that will allow further segmentation. In our approach, multi‑ thresholding Otsu [10] algorithm is applied. In a clas‑ sic method, the threshold is determined by minimizing intra‑class intensity variance. The extension to multi‑ level thresholding was described in [5]. a

Fig. 6. The result of segmentation b

Fig. 4. a ‐ a keypoints map, b ‐ L‐shape areas; red pixel ‐ represents a keypoint, white pixel ‐ represents a shadow

Fig. 5. Histograms of different areas The method described in this article uses median �ilter for noise reduction [2]. This kind of �ilter preser‑ ves edges while removing noise. This feature is crucial for proper segmentation. Low‑lying areas and areas occupied by objects are analyzed differently. The �irst stage of our algorithm is to create a histogram. If we examine the histogram (Fig. 3), we can observe peaks. If a given pixel has a value of 0, it is most likely a shadow fragment. If it is a light pixel, it is an element of a highly re�lective object: a building, a car, etc. Bright and shadow (brightness =0) pixels will be key points and will be used in the process of segmentation and classi�ication of objects. All other pixels will be treated as a part of the low‑lying surface. 2.2. Segmentation of Low‐Lying Areas

If we analyze the histogram, we can see that we cannot specify the threshold that will allow the seg‑ mentation of �lat areas of different brightness. The‑ refore, at the �irst stage of the operation of our al‑ gorithm, median �iltration with a 3�3 mask is per‑

2.3. Segmentation of the Area Occupied by the Objects We assume that the bright pixels in the image cor‑ respond to the objects. These pixels are treated as key points, and their surroundings are analyzed. Figure 4a shows key points (red pixels). We can notice that if the bright pixels are arranged in the form of the letter L, they represent a part of a building (Fig. 4b). The L‑shaped area can be easily detected using Hough transform. If the longest segments have been found, it is possible to build a parallelogram, which describes the visible part of the building. We can de‑ termine the height of the building based on the shape of the shadow, but this requires knowledge about the location of the aircraft. Areas covered with trees can be distinguished from built‑up areas by analyzing the histogram of a sharpened fragment of the image. For buildings, there are sharp edges, while for forests, the change in brig‑ htness is smooth. We can observe this difference by analyzing the histogram. Fig. 5 presents histograms of three different places: built‑up areas (5a), cars (5c), fo‑ rest (5b). In the last case, the value 0 is dominant in the histogram. Figure 6 shows the �inal result of image segmen‑ tation. Two types of �lat surface are detected (marked in black and dark green). Analyzing the shape of the areas, we can notice that the area marked in black is a fragment of the road. Light green pixels represent the forest. Detected buildings are represented as rectang‑ les. Other key points (red ‑ object and white ‑ shadow) remained classi�ied. To carry out further classi�ication, it is worth using contextual information. If the key point is surrounded by an area classi�ied as a road, it can be considered a car, and if there is a shadow near it, it is most likely a tree.

3. Conclusion

This article describes our �irst research related to SAR image segmentation and attempts to detect ele‑ ments such as buildings, roads, and forest areas. Be‑ Articles

Journal of Automation, Mobile Robotics and Intelligent Systems Journal of Automation, Mobile Robotics and Intelligent Systems

cause of the lack of public databases containing SAR images, modern methods like deep learning methods or SVM cannot be applied. We have decided to use clas‑ sic image processing methods. The algorithm combi‑ nes histogram‑based and simple rule‑based systems. Tests carried out for SAR images made available by Le‑ onardo Airborne & Space System Company are promi‑ sing. Further re�inements are possible. Contextual in‑ formation can be taken into account. The shape of a shadow can also be used to infer valuable information.

ACKNOWLEDGEMENTS

We thank Leonardo Airborne & Space System Com‑ pany for sharing SAR images and corresponding Goo‑ gle Map.

AUTHORS

Barbara Siemiatkowska∗ – Faculty of Mechatronics, Warsaw University of Technology, Warsaw, Poland, e‑mail: siemiatkowska.barbara@pw.edu.pl. Krzysztof Gromada∗ – PIT‑RADWAR, Warsaw, Po‑ land, e‑mail: krzysztof.gromada@pitradwar.com. ∗

Corresponding author

REFERENCES

[1] A. Aguasca, R. Acevo‑Herrera, A. Broquetas, J. J. Mallorqui, and X. Fabregas, “ARBRES: Light‑Weight CW/FM SAR Sensors for Small UAVs”, Sensors, vol. 13, no. 3, 2013, 3204–3216, 10.3390/s130303204. [2] G. R. Arce, Nonlinear signal processing: a statis‑ tical approach, Wiley‑Interscience: Hoboken, N.J, 2005. [3] A. Fabijanska, M. Kuzanski, D. Sankowski, and L. Jackowska‑Strumillo, “Application of image processing and analysis in selected industrial computer vision systems”. In: 2008 Interna‑ tional Conference on Perspective Technologies and Methods in MEMS Design, 2008, 27–31, 10.1109/MEMSTECH.2008.4558728.

[4] M. Farhadi, R. Feger, J. Fink, M. Gonser, J. Hasch, and A. Stelzer, “Adaption of Fast Factorized Back‑ Projection to Automotive SAR Applications”. In: 2019 16th European Radar Conference (EuRAD), 2019. [5] D.‑Y. Huang and C.‑H. Wang, “Optimal multi‑ level thresholding using a two‑stage Otsu optimization approach”, Pattern Recogni‑ tion Letters, vol. 30, no. 3, 2009, 275–284, 10.1016/j.patrec.2008.10.003.

[6] Y. Huang and Y.‑C. Huang, “Segmenting SAR Satellite Images With the Multilayer Le‑ vel Set Approach”, IEEE Journal of Selected Topics in Applied Earth Observations and Re‑ mote Sensing, vol. 4, no. 3, 2011, 632–642, 10.1109/JSTARS.2011.2158390. Articles

VOLUME 15, N° 1 2021 VOLUME 15, N° 1 2021

[7] F. Lattari, B. Gonzalez Leon, F. Asaro, A. Rucci, C. Prati, and M. Matteucci, “Deep Learning for SAR Image Despeckling”, Remote Sensing, vol. 11, no. 13, 2019, 1532, 10.3390/rs11131532.

[8] A. V. Monti‑Guarnieri, M. A. Brovelli, M. Man‑ zoni, M. Mariotti d’Alessandro, M. E. Molinari, and D. Oxoli, “Coherent Change Detection for Multi‑ pass SAR”, IEEE Transactions on Geoscience and Remote Sensing, vol. 56, no. 11, 2018, 6811– 6822, 10.1109/TGRS.2018.2843560.

[9] G. Moser and S. Serpico, “Generalized minimum‑ error thresholding for unsupervised change detection from SAR amplitude imagery”, IEEE Transactions on Geoscience and Re‑ mote Sensing, vol. 44, no. 10, 2006, 2972–2982, 10.1109/TGRS.2006.876288.

[10] M. Sezgin and B. Sankur, “Survey over image thresholding techniques and quantitative performance evaluation”, Journal of Electro‑ nic Imaging, vol. 13, no. 1, 2004, 146–165, 10.1117/1.1631315.

[11] X. Xue, H. Wang, F. Xiang, and J. Wang, “A new method of SAR Image Segmentation Based on FCM and wavelet transform”. In: 2012 5th International Congress on Image and Signal Processing, 2012, 621–624, 10.1109/CISP.2012.6469844.

[12] H. Yu, X. Zhang, S. Wang, and B. Hou, “Context‑ Based Hierarchical Unequal Merging for SAR Image Segmentation”, IEEE Transactions on Geo‑ science and Remote Sensing, vol. 51, no. 2, 2013, 995–1009, 10.1109/TGRS.2012.2203604. [13] Y. Zhong, A. Ma, and L. Zhang, “An Adaptive Me‑ metic Fuzzy Clustering Algorithm With Spatial Information for Remote Sensing Imagery”, IEEE Journal of Selected Topics in Applied Earth Obser‑ vations and Remote Sensing, vol. 7, no. 4, 2014, 1235–1248, 10.1109/JSTARS.2014.2303634.

[14] P. Zhou, G. Guo, and F. Xiong, “Research on mo‑ di�ied SVM for classi�ication of SAR images”. In: Proceedings of the 2017 International Conference on Frontiers of Manufacturing Science and Mea‑ suring Technology, 2017.

Journal of Automation, Automation,Mobile MobileRobotics Roboticsand andIntelligent IntelligentSystems Systems

VOLUME 15,15, N°N°1 1 2021 VOLUME 2021

MATLAB IMPLEMENTATION OF DIRECT AND INDIRECT SHOOTING METHODS TO SOLVE AN OPTIMAL CONTROL PROBLEM WITH STATE CONSTRAINTS Submitted: 3rd March 2020; accepted: 27th October 2020

Andrzej Karbowski DOI: 10.14313/JAMRIS/1‐2021/6 Abstract: The paper presents a general procedure to solve nume‐ rically optimal control problems with state constraints. It is used in the case, when the simple time discretization of the state equations and expressing the optimal cont‐ rol problem as a nonlinear mathematical programming problem is too coarse. It is based on using in turn two multiple shooting BVP approaches: direct and indirect. The paper is supplementary to the earlier author’s paper on direct and indirect shooting methods, presenting the theory underlying both approaches. The same example is considered here and brought to an end, that is two full listings of two MATLAB codes are shown. Keywords: optimal control, numerical methods, state constraints, shooting method, multiple shooting method, boundary value problem, MATLAB

1. Introduction We want to determine a piecewise continuous con‑ trol function u(t) ∈ R, t0 ≤ t ≤ tf , which minimizes the Mayer functional J(u) = g(x(tf ))

subject to the constraints

(1)

ẋ(t) = f (x(t), u(t), t), t0 ≤ t ≤ tf

(2)

x(t0 ) = x0

(3)

S(x(t), t) ≤ 0, t0 ≤ t ≤ tf

(4)

Here, x(t) ∈ R denotes a vector of state varia‑ bles, constraint (3) describes initial conditions, (4) is a nonstationary inequality constraint on current va‑ lues of state. It is assumed, that the function S is suf�iciently continuously differentiable. The function f (x(t), u(t), t) is allowed to be merely piecewise con‑ tinuously differentiable with respect to time variable for t ∈ [t0 , tf ]. The �inal time tf is �ixed. Problems with free �inal time or problems with integral terms in the performance index (Bolza or Lagrange) can be easily transformed into a problem of the type (1)‑(4) by me‑ ans of additional state variables. For the sake of simplicity of the presentation we do not consider constraints on control. However, the methodology presented both in [4] and here is general and may be used to solve also such problems. n

The simplest method to solve the problem (1)‑(4) consists in time discretization of the state equation (2) with equal length time stages. Then state and control trajectories are represented by vectors of real num‑ bers (i.e., they are piecewise constant) and from the differential state equations difference equations are obtained. The latter are treated as a set of equality constraints in a static nonlinear programming pro‑ blem with the performance index (1) and an additi‑ onal set of inequality constraints stemming from the constraint on the current state (4). In this way the op‑ timal control problem has been converted to a non‑ linear mathematical programming problem. Unfortu‑ nately, there is a big class of problems, e.g. [8], in the aerospace, medical apparatus domain, chemical and nuclear reactors, robot manipulators, in which a high accuracy of the solution is crucial and the above appro‑ ach is unacceptable as it delivers controls which are too coarse. In such cases we apply direct and indirect shoot‑ ing methods [8], [7]. Typically, the initial guesses of the optimal state and control trajectories are genera‑ ted by applying the direct method [2]. On the basis of them the possible intervals of the activity of con‑ straints are determined. Then the necessary conditi‑ ons of optimality are analytically derived, what leads to a boundary value problem (BVP) [9] for ordinary differential equations with state and adjoint variables [10]. In the previous paper [4] the basic theory of the shooting approaches was presented. As an illustration an example taken from Jacobson and Lele’s paper [3] was used. However, there was no space there to pre‑ sent the codes giving the presented results. In this pa‑ per MATLAB procedures which delivered the results described in [4] are shown. It should help the possible readers to understand the theory on both shooting ap‑ proaches and to write their own codes.

2. The Optimal Control Problem

Let us consider the following optimal control pro‑ blem taken from Jacobson and Lele’s paper [3]: ∫ 1 ( 2 ) min x1 (t) + x22 (t) + 0.005u2 (t) dt (5) u(.)

where for t ∈ [0, 1]

u(t) ∈ R

ẋ1 (t) = x2 (t), ẋ2 (t) = −x2 (t) + u(t),

x1 (0) = 0 x2 (0) = −1

(6)

(7) (8)

43 43

Journal Journal of of Automation, Automation,Mobile MobileRobotics Roboticsand andIntelligent IntelligentSystems Systems

VOLUME N°11 2021 2021 VOLUME 15,15, N°

x2 (t) ≤ 8(t − 0.5)2 − 0.5

boundary conditions iteration 1 iteration 2 solution

(9)

To apply shooting methods we have to rewrite this optimization problem as a Mayer problem, introdu‑ cing an additional state variable x3 governed by the state equation: ẋ3 (t) = x21 (t) + x22 (t) + 0.005u2 (t),

x3 (0) = 0 (10) The objective function (5) will be replaced with g(x(tf )) = x3 (1)

which will be minimized. So, now our problem has the following form: min g(x(tf )) = x3 (1) u(.)

where for t ∈ [0, 1]

t p-1

(14)

 0 x(t0 ) =  −1  0

...

(15) (16)

min g(χp )

(17)

u(t) = uk , t ∈ Ik = [tk , tk+1 ), for k = 0, 1, . . . , p − 1 (18) where uk ∈ R.

(20)

χk+1 − x(tk+1 ; χk , uk ) = 0, k = 0, . . . , p − 1 (21) χ 0 = x0 S(χk , tk ) ≤ 0, k = 0, . . . , p

In the direct approach a control interval is divided into a certain number of subintervals, on which a Cau‑ chy problem is solved by an ordinary differential equa‑ tion (ODE) solver. The initial conditions are genera‑ ted iteratively by an optimizer, constraints on state are checked in the discretization points of the time inter‑ val. The optimal control problem is transformed into a nonlinear programming problem [10], [4], [5] through a parametrization of control u(t) on the subintervals of the control interval. For example, u(t) may be: pie‑ cewise constant, piecewise linear or higher order po‑ lynomials, linear combination of some basis functions, e.g. B‑splines. We apply the simplest type of the para‑ metrization: piecewise constant, that is, we take:

(19)

Then the values obtained at the ends of subintervals ‑ we will denote them by x(tk+1 ; χk , uk ) ‑ are compa‑ red with the guesses χk+1 . The differential equations, initial and end points conditions and path constraints de�ine the constraints of the nonlinear programming problem, that is the problem (1)‑(4) is replaced with: χ,u

3.1. Formulation

t0 < t1 < t2 < . . . tp−1 < tp = tf

The basic idea is to simultaneously integrate nu‑ merically the state equations (14) on the subintervals Ik for guess initial points

 x2 (t)  −x2 (t) + u(t) = 2 2 2 x1 (t) + x2 (t) + 0.005u (t)



(12)

3. Direct Multiple Shooting Technique

Articles

Fig. 1. Direct shooting method

ẋ(t) = f (x(t), u(t)) =

u(t) ∈ R

S(x(t), t) = x2 (t) − 8(t − 0.5)2 + 0.5 ≤ 0,

χk = x(tk )



...

(11)

(13)

with

...

(22)

(23)

where x(tk+1 ; χk , uk ) for k = 0, . . . , p − 1 is the solu‑ tion of ODE: ẋ(t) = f (x(t), uk , t), tk ≤ t ≤ tk+1 , x(tk ) = χk , k = 0, . . . , p − 1,

(24) (25)

at t = tk+1 (see Fig. 1). This nonlinear programming problem can be sol‑ ved by any continuous constrained optimization sol‑ ver. 3.2. MATLAB Solution

The problem (12)‑(16) was solved by the direct shooting method, described in Sec. 3.1, for p = 20 time subintervals of equal length, with the help of two MATLAB functions: ode45 (ODE solver; medium order Runge‑Kutta method) and fmincon (constrained non‑ linear multivariable optimization solver). The code implementing it in MATLAB has the name shooDir.m and is shown below. There T denotes the array of time instants, U is an array with (discretized in time) cont‑ rol trajectory, X with state trajectory. The vector z of

Journal of Journal of Automation, Automation,Mobile MobileRobotics Roboticsand andIntelligent IntelligentSystems Systems

VOLUME 2021 VOLUME 15,15, N°N°1 1 2021

decision variables is a concatenation of vectors repre‑ senting, in turn, subsequent guesses of initial values of state variables at starting points of subintervals and control, that is: z = [χ1,0 , χ1,1 , . . . , χ1,p , χ2,0 , χ2,1 , . . . , χ2,p , χ3,0 , χ3,1 , . . . , χ3,p , u0 , u1 , . . . , up−1 ] .

(26)

f u n c t i o n [ T, U, X, gopt ]= s h o o D i r ( p v a l ) % % Implementation o f t h e d i r e c t % s h o o t i n g method % % PARAMETERS: % p v a l − number o f s u b i n t e r v a l s % ( e . g . 20) % T − a r r a y o f s u b s e q u e n t time % points ( i . e . , times of shoots ) % U − t r a j e c t o r y of optimal c o n t r o l % ( discretized ) % X − state trajectory % gopt − o p t i m a l v a l u e % of objective function % % CALLING : % >> [ T, U, X, gopt ]= s h o o D i r ( p v a l ) % For example : % >> [ T, U, X, gopt ]= s h o o D i r ( 2 0 ) % g l o b a l p Dt U p = p v a l ; Dt = 1/p ; % % s t a r t i n g p o i n t f o r ( time % discretized ) trajectory % optimization % xs = z e r o s ( p +1 ,1) ; us = z e r o s ( p , 1 ) ; z i n i t = [ xs ; xs ; xs ; us ] ; U= [ ] ; % % i n i t i a l conditions for state % equation % Aeq = z e r o s ( 3 , 4 ∗ p+3) ; Aeq ( 1 , 1 ) = 1 ; Aeq ( 2 , p+2) = 1 ; Aeq ( 3 , 2 ∗ p+3) = 1 ; beq = [ 0 ; − 1 ; 0 ] ; % % inequality state constraint % d i s c r e t i z e d i n time % A=z e r o s ( p+1 ,4∗p+3) ; f o r k=1:p+1 A( k , p+1+k ) = 1 ; b ( k ) = 8 ∗ ( ( k−1)∗Dt −0.5) ^2 −0.5; end tic [ zo , gopt ]= fmincon (@fun , z i n i t , ...

A, b , Aeq , beq , ... [ ] , [ ] , @nonlcon , ... o p t i m s e t ( ' D i s p l a y ' , ... ' i t e r ' , ' TolFun ' , 1 . e −4,... ' MaxFunEvals ' , 1 0 0 0 0 0 ) ) ;

toc % % The l i n e s below i n t h i s % f u n c t i o n a r e o n l y f o r output % purposes ( including p l o t s ) % T= [ ] ; f o r k=1:p+1 T( k ) = Dt ∗ ( k−1) ; end f ( 1 )=f i g u r e ( ) ; X=[ zo ( 1 : p+1) , zo ( p +2:2∗( p+1) ) , ... zo ( 2 ∗ p +3:3∗( p+1) ) ] ; h1=p l o t (T,X( : , 1 ) , ' b−− ' , ... T,X( : , 2 ) , ' g− ' , ... T, 8 ∗ ( T−0.5) . ^ 2 − 0 . 5 , 'm−. ' ) ; g r i d on f o r i =1:3 h1 ( i ) . LineWidth =3; end h1 ( 2 ) . C o l o r =[0 0 . 6 0 . 3 ] ; l e g e n d ( ' x_1 ' , ' x_2 ' , ' S ( x , t )=0 ' ) ; xlabel ( ' t ' ) ; ylabel ( 'x ' ) ; t x t={ 'INFEASIBLE ' , ' ' , ... ' x_2 REGION ' } ; t e x t ( 0 . 4 , 0 . 8 , txt , ' C o l o r ' , 'm' ) ; f ( 2 )=f i g u r e ( ) ; p l o t (T( 1 : end −1) , U, ' LineWidth ' , 3 ) ; g r i d on ; l e g e n d ( ' u o p t i m a l − d i r e c t method ' → ) ; xlabel ( ' t ' ) ; ylabel ( 'u ' ) ; end f u n c t i o n [ c , ceq ]= n o n l c o n ( z ) % % The f u n c t i o n c a l c u l a t e s % f o r t r a j e c t o r i e s on s u b s e q u e n t % s u b i n t e r v a l s the d i s c r e p a n c i e s % between g u e s s e s o f i n i t i a l p o i n t s % and t h e end p o i n t s o f s h o o t s % ( from t h e p r e v i o u s g u e s s e d p o i n t s ) % g l o b a l p Dt U f o r i =1:3 f o r k=1:p+1 xs ( i , k ) = z ( ( i −1) ∗ ( p+1)+k ) ; end end f o r k=1:p U( k ) = z ( 3 ∗ ( p+1)+k ) ; end c = [ ] ; ceq = [ ] ; j =1; f o r k=1:p [ T,X]= ode45 (@( t , x ) f ( t , x , ... U( k ) ) , [ ( k−1)∗Dt k∗Dt ] , ...

Articles

45 45

Journal Journal of of Automation, Automation,Mobile MobileRobotics Roboticsand andIntelligent IntelligentSystems Systems

1.5 x1 x2

S(x,t)=0

INFEASIBLE x 2 REGION

0.5

-0.5

-1

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

Fig. 2. Optimal state trajectories obtained from the direct shooting method

end

[ xs ( 1 , k ) ; xs ( 2 , k ) ; ... xs ( 3 , k ) ] ) ; f o r i =1:3 ceq ( j ) = X( end , i )−xs ( i , k+1) ; j=j +1; end

f u n c t i o n dxdt = f ( t , x , u ) % % state transformation function % ( rhs of the s t a t e equation ) ; % here x i n d i c e s denote c o o r d i n a t e s % dxdt = z e r o s ( 3 , 1 ) ; dxdt ( 1 ) = x ( 2 ) ; dxdt ( 2 ) = −x ( 2 )+u ; dxdt ( 3 ) = x ( 1 ) ^2+x ( 2 ) ^2+0.005∗ u ^ 2 ; end f u n c t i o n f z=fun ( z ) global p f z = z ( 3 ∗ ( p+1) ) ; end The calculations were performed on a PC with Intel Core i7‑2600K CPU@3.40 GHz processor under MAT‑ LAB R2020b. After about 50 s we obtained the perfor‑ mance index value equal 0.1708. The resulting trajec‑ tories of the state variables x1 and x2 are presented in Fig. 2, of the state variuable x3 in Fig. 6 and of the optimal control in Fig. 5. Analyzing the resulting x2 state variable trajectory we may see, that it contains one boundary arc. To �ind its precise course the indirect shooting method will be used.

4. Indirect Multiple Shooting Technique 4.1. Formulation

In the indirect approach BVP concerns not only state equations, but also the equations describing ad‑ Articles

VOLUME N°11 2021 2021 VOLUME 15,15, N°

joint variables η(t). It means, that for an optimal con‑ trol problem, before using a solver, we have to make a kind of preprocessing on the paper, based on the appropriate theory [1]. In particular, we have to de‑ termine the number of switching points s, where the state trajectory enters and leaves the constraint boun‑ dary. The optimal control at a given time instant is a function of the current value of state and adjoint va‑ riables (i.e., we provide a control law). Only general formulas should be given, their parameters: Lagrange multipliers, initial values of adjoint variables, their jumps and the concrete values of switching points (i.e., times) will be the subject of optimization. The basic idea of the numerical treatment of such problems by multiple shooting technique is to consi‑ der the switching conditions as boundary conditions to be satis�ied at some interior multiple shooting no‑ des [6]. Thus, the problem is transformed into a clas‑ sical multipoint BVP [9], [6]: Determine a piecewise smooth vector function y(t) = [x(t), η(t)], which satis�ies ẏ(t) = f (y(t), u(t), t), t0 ≤ t ≤ tf ,

(27)

u = uk (y(t), t), τk ≤ t < τk+1 , k = 0, . . . , s, (28) y(τk+ ) = hk (y(τk− ), γk ), k = 1, . . . , s,

y(t0 ) =

[

x0 η0

]

(29) (30)

ri (y(tf )) = 0, i = 1, . . . , nf ,

(31)

r̃k (τk , y(τk− )) = 0, k = 1, ..., s.

(32)

In this formulation, η0 , γ, τk , k = 1, . . . , s are unknown parameters of the problem, where the latter satisfy t0 =: τ0 < τ1 < τ2 < · · · < τs < τs+1 := tf

(33)

The trajectory may possess jumps of size given by Eq. (29). If a coordinate of y(t) is continuous (as all state variables), then its hk is identity. The boundary con‑ ditions and the switching conditions are described by Eqs. (30)‑(32). In every time stage k the numerical integration over the interval [τk , τk+1 ] is done by any conventional Cauchy problem (aka initial value problem ‑ IVP) ODE solver with stepsize control. The resulting system of nonlinear equations (29)‑(32) can be solved numeri‑ cally by a quasinewton method, e.g., from the Broyden family. The derivation of optimality conditions for our problem (12)‑(16) is presented in [4]. They lead to the following set of equations (here: s = 2, t1 τ1 , t2

Journal of Journal of Automation, Automation,Mobile MobileRobotics Roboticsand andIntelligent IntelligentSystems Systems

τ2 ):

ẋ1 (t) = x2 (t) ẋ2 (t) = −x2 (t) + u(t) x21 (t)

+ ẋ3 (t) = η̇1 (t) = 2x1 (t)

x22 (t)

+ 0.005u (t)

η˙2 (t) = 2x2 (t) − η1 (t) + η2 (t) − µS (t) where

u(t) =

{

µS (t) =

x2 (t) + 16(t − 0.5), 100η2 (t), {

t ∈ [t1 , t2 ] t∈ / [t1 , t2 ]

η2 (t) − 0.01 · u(t), 0,

with

t ∈ [t1 , t2 ] t∈ / [t1 , t2 ]

VOLUME 2021 VOLUME 15,15, N°N°1 1 2021

(34)

(35)

(36)

(37)

(38)

(39)

(40)

(41)

x1 (0) = 0

(42)

x2 (0) = −1

(43)

x3 (0) = 0

− + − + − x1 (t+ 1 ) = x1 (t1 ), x2 (t1 ) = x2 (t1 ), x3 (t1 ) = x3 (t1 ) (44) − + − + − x1 (t+ 2 ) = x1 (t2 ), x2 (t2 ) = x2 (t2 ), x3 (t2 ) = x3 (t2 ) (45) − + − η1 (t+ 2 ) = η1 (t2 ), η2 (t2 ) = η2 (t2 )

(46)

The junction (jump) conditions will be as follows: − η1 (t+ 1 ) = η1 (t1 )

η2 (t+ 1) and the �inal ones:

η2 (t− 1)

+γ

(47) (48)

η1 (1) = 0

(49)

η2 (1) = 0

(50)

The switching (tangency) condition in our problem has the form: − − − 2 S(x2 (t− 1 ), t1 ) = x2 (t1 )−8(t1 −0.5) +0.5 = 0 (51)

4.2. MATLAB Solution

Putting all these things together and expressing them in the format required by the indirect shooting method, described in [4], we get a system of nonlinear equations, with the vector of unknowns: z = [η10 , η20 , t1 , t2 , γ]

(52)

to be satis�ied at �inal and switching points, re‑ sulting from the solution of ODEs with time functions:

x1 (t), x2 (t), x3 (t), η1 (t), η2 (t), de�ined on three inter‑ vals: [0, t1 ], [t1 , t2 ], [t2 , 1], where starting points are de‑ �ined by initial and junction�jump conditions (41)‑ (48). The set of nonlinear equations to be solved was made of conditions (49)‑(51):   − S(x2 (t− 1 ), t1 ) =0 (53) F (z) = Ferr (z) =  η1 (1) η2 (1)

This problem was solved under MATLAB with the help of two MATLAB functions: ode45 (mentioned above) and fsolve (a solver of systems of nonlinear equati‑ ons of several variables). The resulting code has the name shooIndir.m and is presented below:

f u n c t i o n [ T, U, Y, gopt ]= s h o o I n d i r % % Implementation o f t h e i n d i r e c t % s h o o t i n g method % % PARAMETERS: % T − a r r a y o f s u b s e q u e n t time % points of control interval % U − t r a j e c t o r y of optimal c o n t r o l % ( discretized ) % Y − s t a t e and a d j o i n t v a r i a b l e s % trajectories ( discretized ) % gopt − o p t i m a l v a l u e % of objective function % % CALLING : % >> [ T, U, Y, gopt ]= s h o o I n d i r % g l o b a l TTot YTot

z s =[ 0 ; 0 ; 0 . 3 3 ; 0 . 6 6 ; 1 ] ; tic [ zo , Fo ] = f s o l v e (@ShooF , zs , → o p t i m s e t ( ' D i s p l a y ' , ' i t e r ' ) ) ; T=TTot ; Y=YTot ; toc % % The l i n e s below i n t h i s % f u n c t i o n a r e o n l y f o r output % purposes ( including p l o t s ) % stT=s i z e (T) ; f o r t =1: stT i f T( t ) < zo ( 3 ) | | T( t ) > zo ( 4 ) U( t )= 100∗Y( t , 5 ) ; MuS( t )= 0 . ; else U( t )= Y( t , 2 ) +16∗(T( t ) −0.5) ; MuS( t )= Y( t , 5 ) −0.01∗U( t ) ; end end gopt=Y( end , 3 ) ; f ( 1 )=f i g u r e ( ) ; h1=p l o t (T, Y( : , 1 ) , ' b−− ' , ... T, Y( : , 2 ) , ' g− ' , ... T, 8 ∗ ( T−0.5) . ^ 2 − 0 . 5 , 'm−. ' ) ; f o r i =1:3 Articles

Journal of Automation, Mobile Robotics and Intelligent Systems Journal of Automation, Mobile Robotics and Intelligent Systems

h1 ( i ) . LineWidth =3; end h1 ( 2 ) . C o l o r =[0 0 . 6 0 . 3 ] ; g r i d on ; l e g e n d ( ' x_1 ' , ' x_2 ' , ' S ( x , t )=0 ' ) ; xlabel ( ' t ' ) ; ylabel ( 'x ' ) ; t x t={ 'INFEASIBLE ' , ' ' , ... ' x_2 REGION ' } ; t e x t ( 0 . 4 , 0 . 8 , txt , ' C o l o r ' , 'm' ) ; f ( 2 )=f i g u r e ( ) ; h2=p l o t (T, Y( : , 4 ) , ' r− ' , ... T, Y( : , 5 ) , ' b−− ' , ... T, MuS' , ' g −. ' ) ; h2 ( 3 ) . C o l o r =[0 0 . 6 0 . 3 ] ; g r i d on ; l e g e n d ( ' \ eta_1 ' , ' \ eta_2 ' , ' \mu_S ' ) ; x l a b e l ( ' t ' ) ; y l a b e l ( ' \ eta , \mu_S ' ) ; f o r i =1:3 h2 ( i ) . LineWidth =3; end

end

f ( 3 )=f i g u r e ( ) ; p l o t (T, U, ' LineWidth ' , 3 ) ; g r i d on ; legend ( 'u optimal − i n d i r e c t → method ' ) ; xlabel ( ' t ' ) ; ylabel ( 'u ' ) ;

f u n c t i o n F e r r=ShooF ( z ) % % The f u n c t i o n c a l c u l a t e s l h s % of the ultimate s e t of ( s t a t i c ) % nonlinear equations : % F e r r ( z )=0 % g l o b a l TTot YTot eT10 = z ( 1 ) ; eT20 = z ( 2 ) ; t1 = z (3) ; t2 = z (4) ; gamma = z ( 5 ) ; TTot = [ ] ; YTot = [ ] ; % % Region 1 ( s t a t e c o n s t r a i n t % inactive ) % [ T,Y]= ode45 (@( t , y ) f ( t , y , 1 ) , ... [ 0 t 1 ] , ... [ 0 ; −1; 0 ; eT10 ; eT20 ] ) ; TTot = T ; YTot = Y; x11 = Y( end , 1 ) ; x21 = Y( end , 2 ) ; x31 = Y( end , 3 ) ; eT11 = Y( end , 4 ) ; eT21 = Y( end , 5 ) ; x2b1 = 8 ∗ ( t1 −0.5) ^2 −0.5; F e r r ( 1 ) = x21−x2b1 ; %

Articles

VOLUME 15, N° 1 2021 VOLUME 15, N° 1 2021

% Region 2 ( s t a t e c o n s t r a i n t % active ) % [ T,Y]= ode45 (@( t , y ) f ( t , y , 2 ) , ... [ t 1 t 2 ] , ... [ x11 ; x21 ; x31 ; ... eT11 ; eT21+gamma ] ) ; TTot =[TTot ; T ] ; YTot =[YTot ; Y ] ; x12 = Y( end , 1 ) ; x22 = Y( end , 2 ) ; x32 = Y( end , 3 ) ; eT12 = Y( end , 4 ) ; eT22 = Y( end , 5 ) ; % % Region 3 ( s t a t e c o n s t r a i n t % inactive ) % [ T,Y]= ode45 (@( t , y ) f ( t , y , 3 ) , ... [ t 2 1 ] , ... [ x12 ; x22 ; x32 ; ... eT12 ; eT22 ] ) ; TTot =[TTot ; T ] ; YTot =[YTot ; Y ] ; F e r r ( 2 ) = Y( end , 4 ) ; F e r r ( 3 ) = Y( end , 5 ) ; end f u n c t i o n dydt = f ( t , y , r e g i o n ) % % extended s t a t e t r a n s f o r m a t i o n % f u n c t i o n ( rhs of the 1 s t order % ODE i n s t a t e and a d j o i n t % v a r i a b l e s space ) ; % h e r e : y=[x1 , x2 , x3 , eta1 , e t a 2 ] % dydt = z e r o s ( 5 , 1 ) ; dydt ( 1 ) = y ( 2 ) ; switch region c a s e { 1 , 3} u = 100∗ y ( 5 ) ; muS = 0 ; case 2 u = y ( 2 ) +16∗( t −0.5) ; muS = y ( 5 ) −0.01∗u ; end dydt ( 2 ) = −y ( 2 )+u ; dydt ( 3 ) = y ( 1 ) ^2+y ( 2 ) ^2+0.005∗ u ^ 2 ; dydt ( 4 ) = 2∗ y ( 1 ) ; dydt ( 5 ) = 2∗ y ( 2 )−y ( 4 )+y ( 5 )−muS ; end The calculations took only about 1.5 s, the obtained optimal value of the performance index was 0.1698. The resulting state x1 , x2 and adjoint variables η1 , η2 trajectories are presented in Figs. 3‑4, and the optimal control u and state variable x3 trajectories obtained from both methods are depicted, respectively, in Fig. 5, and 6. One may see, that indeed, the solution delivered by the indirect shooting method is much more precise than that of the direct one, despite that the number of unknowns in the indirect method was much smaller than the dimension of the decision vector in the di‑ rect method (5 vs. 83) and the time of calculations was

Journal of Journal of Automation, Automation,Mobile MobileRobotics Roboticsand andIntelligent IntelligentSystems Systems

VOLUME 2021 VOLUME 15,15, N°N°1 1 2021

1.5

0.18 x1 x

S(x,t)=0

INFEASIBLE

x3 - direct method

0.16

x - indirect method 3

0.14

x 2 REGION

0.12

0.5

0.1 0.08

0.06 0.04

-0.5

0.02 -1

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

Fig. 3. Optimal trajectories of the state variables x1 and x2 obtained from the indirect shooting method. 0.5

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

Fig. 6. Optimal trajectories of the (artificial, representing the objective function growth) state variable x3 obtained from both direct and indirect shooting methods

1 2

0.4

0.3

0.2

0.1

-0.1

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

Fig. 4. Optimal trajectories of the adjoint variables η1 , η2 and the Lagrange multiplier function for state constraint µS obtained from the indirect shooting method.

AUTHOR Andrzej Karbowski – Institute of Cont‑ rol and Computation Engineering, Warsaw University of Technology, Warsaw, Poland, e‑mail: andrzej.karbowski@pw.edu.pl, www: http://www.ia.pw.edu.pl/~karbowsk.

16 u optimal - direct method u optimal - indirect method

14 12 10

8 6

REFERENCES

4 2 0 -2 -4

lution procedure consists in the application of two shooting approaches together: �irst the direct shoot‑ ing method to �ind an approximation of the optimal control trajectory and then ‑ on the basis of the asses‑ sment of the intervals of the activity of the state con‑ straint, obtained from the direct method and the ana‑ lysis of conditions of optimality ‑ the indirect shoot‑ ing method. The latter may seem quite complicated at the beginning and it may be dif�icult for the novices to imagine how to translate them into a computer code. The two listings of the corresponding, working codes in MATLAB, given in the paper, should help in this. Both �iles (’shooDir.m’ and ’shooIndir.m’) may be downloaded from the author’s Web page: http:// www.ia.pw.edu.pl/~karbowsk/shooting.

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

Fig. 5. Optimal control trajectories obtained from both direct and indirect shooting methods much shorter (1.5 s vs. 50 s).

5. Conclusion

In the paper an optimal control problem with one state constraint was considered. The proposed so‑

[1] A. E. Bryson, W. F. Denham, and S. E. Dreyfus, “Optimal Programming Problems with Inequa‑ lity Constraints I: Necessary Conditions for Ex‑ tremal Solutions”, AIAA Journal, vol. 1, no. 11, 1963, 2544–2550, 10.2514/3.2107. [2] M. Gerdts, “Direct Shooting Method for the Nu‑ merical Solution of Higher‑Index DAE Optimal Control Problems”, Journal of Optimization The‑ ory and Applications, vol. 117, no. 2, 2003, 267– 294, 10.1023/A:1023679622905. [3] D. Jacobson and M. Lele, “A transformation technique for optimal control problems with a state variable inequality constraint”, IEEE Tran‑ sactions on Automatic Control, vol. 14, no. 5, 1969, 457–464, 10.1109/TAC.1969.1099283. Articles

49 49

Journal Journal of of Automation, Automation,Mobile MobileRobotics Roboticsand andIntelligent IntelligentSystems Systems

[4] A. Karbowski, “Shooting Methods to Solve Op‑ timal Control Problems with State and Mixed Control‑State Constraints”. In: R. Szewczyk, C. Zieliń ski, and M. Kaliczyń ska, eds., Challen‑ ges in Automation, Robotics and Measurement Techniques, Springer: Cham, 2016, 189–205, 10.1007/978‑3‑319‑29357‑8_17.

[5] H. Maurer and W. Gillessen, “Application of mul‑ tiple shooting to the numerical solution of opti‑ mal control problems with bounded state varia‑ bles”, Computing, vol. 15, no. 2, 1975, 105–126, 10.1007/BF02252860.

[6] H. J. Oberle, “Numerical solution of minimax optimal control problems by multiple shoot‑ ing technique”, Journal of Optimization Theory and Applications, vol. 50, no. 2, 1986, 331–357, 10.1007/BF00939277. [7] R. Pytlak, J. Blaszczyk, A. Karbowski, K. Kraw‑ czyk, and T. Tarnawski, “Solvers chaining in the IDOS server for dynamic optimization”. In: 52nd IEEE Conference on Decision and Control, 2013, 7119–7124, 10.1109/CDC.2013.6761018.

[8] S. Sager, Numerical methods for mixed‑integer op‑ timal control problems, Ph.D. Thesis, University of Heidelberg, 2005. [9] J. Stoer and R. Bulirsch, Introduction to Numerical Analysis, Springer New York: New York, NY, 2002, 10.1007/978‑0‑387‑21738‑3.

[10] O. von Stryk and R. Bulirsch, “Direct and indirect methods for trajectory optimization”, Annals of Operations Research, vol. 37, no. 1, 1992, 357– 373, 10.1007/BF02071065.

Articles

VOLUME N°11 2021 2021 VOLUME 15,15, N°

Journal of Automation, Mobile Robotics and Intelligent Systems

VOLUME 15,

N° 1

2021

A Novel Merchant Optimization Algorithm for Solving Optimal Reactive Power Problem Submitted: 6th July 2019; accepted: 14th March 2021

K. Lenin

DOI: 10.14313/JAMRIS/1-2021/7 Abstract: In this paper Merchant Optimization Algorithm (MOA) is proposed to solve the optimal reactive power problem. Projected algorithm is modeled based on the behavior of merchants who gain in the market through various mode and operations. Grouping of the traders will be done based on their specific properties, and by number of candidate solution will be computed to individual merchant. First Group named as “Ruler candidate solution” afterwards its variable values are dispersed to the one more candidate solution and it named as “Serf candidate solution” In standard IEEE 14, 30, 57 bus test systems Merchant Optimization Algorithm (MOA) have been evaluated. Results show the proposed algorithm reduced power loss effectively. Keywords: Optimal reactive power, Transmission loss, Merchant Optimization Algorithm

1. Introduction Reactive power problem plays a key role in secure and economic operations of power system. Optimal reactive power problem has been solved by variety of types of methods ([1-6]) . Nevertheless numerous scientific difficulties are found while solving problem due to an assortment of constraints. Evolutionary techniques ([7-16]) are applied to solve the reactive power problem, but the main problem is many algorithms get stuck in local optimal solution & failed to balance the Exploration & Exploitation during the search of global solution. In this paper Merchant Optimization Algorithm (MOA) has been proposed to solve the optimal reactive power problem. Projected algorithm is modeled based on the behavior of merchants who gain in the market through various mode and operations. Initial point of the projected algorithm all the merchants will possess same properties and in subsequent iterations properties will be updated. Then grouping of the traders will be done based on their specific properties, and by number of candidate solution will be computed to individual merchant. Subsequent to grouping of candidate solutions, then the most excellent candidate solution of each group

named as “Ruler candidate solution” afterwards its variable values are dispersed to the one more candidate solution and it named as “Serf candidate solution”. In standard IEEE 14, 30, 57 bus test systems [17] the proposed Merchant Optimization Algorithm (MOA) is evaluated. Simulation study shows the projected algorithm reduced power loss effectively.

2. Problem Formulation True power loss reduction is main objective of the problem:

F = PL = ∑ (k∈Nbr) g k (Vi2 + Vj2 − 2Vi Vjcosij )

Voltage deviation given as follows:

F = PL + ωv × Voltage Deviation

Voltage deviation given by:

Npq

Voltage Deviation = ∑ Vi − 1

Constraint (Equality)

i =1

(1) (2) (3)

PG = PD + PL (4)

Constraints (Inequality)

min max Pgslack ≤ Pgslack ≤ Pgslack

max Q min gi ≤ Q gi ≤ Q gi ,i ∈ N g

Vimin ≤ Vi ≤ Vimax ,i ∈ N

(5) (6)

(7)

Timin ≤ Ti ≤ Timax ,i ∈ NT

(8)

Q cmin ≤ Q c ≤ Q Cmax ,i ∈ NC

(9)

3. Merchant Optimization Algorithm Merchant Optimization Algorithm (MOA) has been modeled based on the behavior of merchants who gain in the market through various mode and operations. Initialization of population through candidate solution (CS) is done through various variables and given by,

variousvariables = {vv1 , vv2 , .., vvn , G}

(10)

Journal of Automation, Mobile Robotics and Intelligent Systems

VOLUME 15,

Subsequent to the formation of the population the merit is computed through the objective function and with reference to the problem merit will be defined. Initial point of the projected algorithm all the merchants will possess same properties and in subsequent iterations properties will be updated. Then grouping of the traders will be done based on their specific properties, then by following equation the number of candidate solution will be computed to individual merchant,

 Q  Totalnumberi = 2 + round  T i × ( G − 2 × T )  (11)  ∑ j =1Q J  Subsequent to grouping of candidate solutions, then the most excellent candidate solution of each group named as “Ruler candidate solution” afterwards its variable values are dispersed to the one more candidate solution and it named as “Serf candidate solution” by, Ck



∑  ∑ ( candidate solution_Serf_j(random)(n)) = j =1  i =1

 candidate solution_Ruler_k(random)(n)))  (12)  “Serf candidate solution” is improved by,

∑ ( candidate solution Serf (R ) = i =1

candidate solution Serf ( R ) +

+ k × random(candidate solution Serf ( R ) ))

(13) To “Ruler candidate solution” most excellent candidate solution are imported which has been arbitrarily chosen, T

∑∑ ( candidate solution_Ruler_j(random(n))= j =1 i =1

= candidate solution_Ruler_k(random(n)))

(14)

= {a|a ≠ j and a is an integer random value in 1 ≤ a ≤ n}

(15) Merchant group’s properties has been updated by,

 B  propertiesi = ∑OF ( j )|candidate solution ( j,P ) = Pi   j =1  (16)

a. Initialization of parameters b. Candidate population initiated For I=1 to B For j=1 to n Candidate solution (I,j) = an arbitrary in defined level For I=1 to T Modernize the total number (I) by 52

Articles

N° 1

2021

c. Objective function called For I=1 to B Candidate solution (I). Gain = Returned value form the objective function d. Candidate solution grouping TMP =NB For I=1 to B k = random(light(TMP), Candidate solution (I,n + 1) = k, TMP(k) - TMP(k) – 1 if TMP(k) == 0 then TMP(:,k) = [ ] e. Altering the candidate solution using For I=1 to T Ruler = most excellent candidate solution of the “I” group For J=1 to B Dispense “Ruler candidate solution” values to “Serf candidate solution” only if objective function value enhanced or else disregard the alteration f. Altering the candidate solution using For I=1 to B Modernize the “Serf candidate solution” only if objective function value enhanced or else disregard the alteration g. Altering the candidate solution using Modernize the “Ruler candidate solution” only if objective function value enhanced or else disregard the alteration h. Modernization of the properties Merchant properties are updated by Stop if the end criterion is satisfied or else go to step “d” i. Output the best solution

4. Simulation Results In standard IEEE 14 bus system validity of Merchant Optimization Algorithm (MOA) has been tested, Tab. 1 gives the constraints of control variables, Tab. 2 provides the limits of reactive power generators. Tab. 3 shows the comparison results. Tab. 1. Control variables limits System

Variables

Minimum (PU)

Maximum (PU)

IEEE 14 Bus

Generator Voltage

0.95

1.1

VAR Source

0.20

Transformer Tap

0.9

Tab. 2. Reactive power generators limits

1.1

System

Variables

Q Minimum (PU)

Q Maximum (PU)

IEEE 14 Bus

-40

-6

3 8

-6

50 24 24

Journal of Automation, Mobile Robotics and Intelligent Systems

VOLUME 15,

N° 1

2021

Tab. 3. Simulation results of IEEE −14 system Control variables

Base case

VG−1

1.060

VG−2

MPSO [18]

1.045

VG−3 VG−6 VG−8

1.070

Tap 8

0.978

Tap 9

Tap 10 QC−9

0.975

1.024

0.19

14.64

272.39

QR (Mvar)

Reduction in PLoss (%) Total PLoss (Mw)

NR* - Not reported.

271.32

82.44

75.79

9.2

13.550

12.293

Then the proposed Merchant Optimization Algorithm (MOA) is evaluated in standard IEEE 30 Bus system. Tab. 4 gives the constraints of control variables, Tab. 5 shows the limits of reactive power generators and in Tab. 6 comparison of real power loss has been given. Tab. 4. Constraints of control variables System

Variables

Minimum (PU)

Maximum (PU)

IEEE 30 Bus

Generator Voltage

0.95

1.1

VAR Source

0.20

Transformer Tap

0.9

1.029

1.060

1.067

1.097

1.099

1.056

1.018

0.932

1.086

1.055 1.074

0.969

SARGA [18]

1.100

1.069

1.090

EP [18]

1.100 1.085

1.010

PSO [18]

1.060

1.019

0.988

1.008

0.185

271.32

NR*

1.016 1.053 1.04

0.94

1.03

0.18 NR*

NR*

1.024

1.036

1.037

1.078 0.95

0.95

0.96

0.06 NR*

76.79

NR*

12.315

13.346

13.216

9.1

1.5

MOA

2.5

1.038 1.023 1.035

0.940

0.942

0.946 0.131

271.90 75.91

25.40

10.108

Tab. 5. Constrains of reactive power generators System

Variables

Q Minimum (PU)

Q Maximum (PU)

IEEE 30 Bus

-40

-10

-6

1.1

-40

-6

Tab. 6. Simulation results of IEEE −30 system Control variables VG−1 VG−2 VG−5 VG−8 VG−12

Base case 1.060

1.045

1.010

VG-13

1.082

Tap 11

0.978

Tap 12

Tap 15

Tap 36 QC 10

1.071

0.969

0.932

0.968 0.19

QC 24

0.043

QC (Mvar)

133.9

PG (MW)

Reduction in PLoss (%) Total PLoss (Mw)

300.9 0

17.55

MPSO [18]

PSO [18]

EP [18]

SARGA [18]

1.072

1.097

1.094

1.048

1.033

1.059

1.101

1.100

1.047

1.038

1.086

1.057

1.048

1.068

0.983

1.023

1.020

0.988

0.077

0.119

299.54 130.83 8.4

16.07

1.058

1.080

0.987

1.015

1.020

1.012

0.077

0.128

299.54 130.94 7.4

16.25

NR*

1.049

1.092

1.091 1.01

1.03

1.07

0.99

0.19

0.04 NR* NR* 6.6

16.38

MOA

NR*

1.023

1.053

1.021

1.099

1.099 0.99

1.03

0.98

0.96

0.19

0.04 NR* NR* 8.3

16.09

1.030

1.042

1.045

1.037

0.941

0.943

0.930

0.941

0.090

0.122

297.73 131.44 18.88

14.241

Articles

Journal of Automation, Mobile Robotics and Intelligent Systems

Then the Proposed Merchant Optimization Algorithm (MOA) has been tested, in IEEE 57 Bus system. Tab. 7 shows the constraints of control variables, Tab. 8 shows the limits of reactive power generators and comparison results are presented in Tab. 9. Tab. 7. Constraints of control variables

IEEE 57 Bus

Variables type

Minimum value (PU)

Maximum value (PU)

Generator Voltage

0.95

1.1

VAR Source

0.20

Transformer Tap

0.9

VOLUME 15,

IEEE 57 Bus

Variables

Q Minimum (PU)

Q Maximum (PU)

-140

200

-10

-140

200

-150

155

1.1

-17 -8

-3

25 9

Base case

MPSO [18]

PSO [18]

CGA [18]

AGA [18]

MOA

VG 1

1.040

1.093

1.083

0.968

1.027

1.020

VG 3

0.985

1.056

1.055

1.056

1.033

1.023

Control variables

VG 2

VG 6

VG 8

VG 9

VG 12

Tap 19

Tap 20

Tap 31

Tap 35 Tap 36

Tap 37

Tap 41

Tap 46

Tap 54

Tap 58

Tap 59

Tap 65

Tap 66

Tap 71

Tap 73

Tap 76

Tap 80 QC 18

1.010

0.980

1.005

0.980

1.015

0.970

0.978

1.043

1.000 1.000

1.043

0.967

0.975

0.955

0.900

0.930

0.895

0.958

0.980

0.940 0.1

1.086

1.038

1.066

1.054

0.975

0.982

0.975

1.025 1.002

1.007

0.994

1.013

0.988

0.979

0.983

1.015

0.975

1.020

1.001

0.979

1.002

0.179

1.071

1.036

1.059

1.048

1.046

0.987

0.983

0.981

1.003 0.985

0.986

0.992

0.990

0.997

0.984

0.990

0.988

0.980

1.017

0.131

15.4

14.1

Reduction in PLoss (%) Total PLoss (Mw)

NR* - Not reported. Articles

27.8

272.27 23.51

0.920

0.970 NR*

NR*

1.057

1.030

1.020

1.060 NR*

NR*

0.980

1274.8

321.08

1.004

1.051

1.100

1274.4

QC (Mvar)

0.991

1.051

1.018

1.007

1278.6

0.141

1.022

1.001

0.990

PG (MW)

0.063

0.987

1.011

0.900

0.059

QC 53

1.049

1.009

QC 25

0.176

2021

Tab. 8. Constrains of reactive power generators

Tab. 9. Simulation results of IEEE −57 system

N° 1

0.144

0.162

0.910

0.940

0.950

1.030

1.090

0.900

1.000

0.960

1.000

0.084

0.008

0.053 1276

1.100

1.010

1.080

0.940

0.950

1.050

0.950

1.010

0.940

1.000

0.016

0.015

0.038

1.012

1.024

1.021

1.030

0.900

0.901

0.924 1.012

1.026

1.025

0.917

1.024

0.932

0.930

0.941

1.042

0.913

1.022

1.034

0.941

1.020 0.133

0.141

0.101

1275

1272.10

11.6

26.57

276.58

309.1

304.4

23.86

25.24

24.56

9.2

1.022

272.23

20.412

Journal of Automation, Mobile Robotics and Intelligent Systems

5. Conclusion In this paper Merchant Optimization Algorithm (MOA) solved the optimal reactive power problem effectively. Projected algorithm is modeled based on the behavior of merchants who gain in the market through various mode and operations. Initially all the merchants will possess the same properties and in subsequent iterations properties will be updated. Then grouping of the traders has been done based on their explicit properties. Proposed Merchant Optimization Algorithm (MOA) has been tested in standard IEEE 14, 30, 57 bus test systems and simulation results show the projected algorithm reduced the real power loss. Percentage of real power loss reduction has been improved.

AUTHOR Kanagasabai Lenin – Department of Electrical and Electronics Engineering, Prasad V. Potluri Siddhartha Institute of Technology, Vijayawada, India, e-mail: gklenin@gmail.com.

References [1] K. Y. Lee, Y. M. Park and J. L. Ortiz, “Fuel-cost minimisation for both real- and reactive-power dispatches”, IEE Proceedings C (Generation, Transmission and Distribution), vol. 131, no. 3, 1984, 85-93, 10.1049/ip-c.1984.0012.

[2] N. I. Deeb and S. M. Shahidehpour, “An Efficient Technique for Reactive Power Dispatch Using a Revised Linear Programming Approach”, Electric Power Systems Research, vol. 15, no. 2, 1988, 121-134, 10.1016/0378-7796(88)90016-8.

[3] M. Bjelogrlic, M. S. Calovic, P. Ristanovic and B. S. Babic, “Application of Newton’s optimal power flow in voltage/reactive power control”, IEEE Transactions on Power Systems, vol. 5, no. 4, 1990, 1447-1454, 10.1109/59.99399. [4] S. Granville, “Optimal reactive dispatch through interior point methods”, IEEE Transactions on Power Systems, vol. 9, no. 1, 1994, 136-146, 10.1109/59.317548.

[5] N. Grudinin, “Reactive power optimization using successive quadratic programming method”, IEEE Transactions on Power Systems, vol. 13, no. 4, 1998, 1219-1225, 10.1109/59.736232. [6] R. Ng Shin Mei, M. H. Sulaiman, Z. Mustaffa and H. Daniyal, “Optimal reactive power dispatch solution by loss minimization using moth-flame optimization technique”, Applied Soft Computing, vol. 59, 2017, 210-222, 10.1016/j. asoc.2017.05.057.

VOLUME 15,

N° 1

2021

[7] G. Chen, L. Liu, Z. Zhang and S. Huang, “Optimal reactive power dispatch by improved GSA-based algorithm with the novel strategies to handle constraints”, Applied Soft Computing, vol. 50, 2017, 58-70, 10.1016/j.asoc.2016.11.008.

[8] E. Naderi, H. Narimani, M. Fathi and M. R. Narimani, “A novel fuzzy adaptive configuration of particle swarm optimization to solve large-scale optimal reactive power dispatch”, Applied Soft Computing, vol. 53, 2017, 441-456, 10.1016/j. asoc.2017.01.012.

[9] A. A. Heidari, R. Ali Abbaspour and A. Rezaee Jordehi, “Gaussian bare-bones water cycle algorithm for optimal reactive power dispatch in electrical power systems”, Applied Soft Computing, vol. 57, 2017, 657-671, 10.1016/j. asoc.2017.04.048. [10] M. Mahaletchumi, A. Nor Rul Hasma, S. M. H., M. Mahfuzah and S. Rosdiyana, “Benchmark studies on Optimal Reactive Power Dispatch (ORPD) Based Multi-Objective Evolutionary Programming (MOEP) using Mutation Based on Adaptive Mutation Operator (AMO) and Polynomial Mutation Operator (PMO)”, Journal of Electrical Systems, vol. 12, no. 1, 2016, 121-132. [11] R. Ng Shin Mei, M. H. Sulaiman and Z. Mustaffa, “Ant lion optimizer for optimal reactive power dispatch solution”, Journal of Electrical Systems - Special Issue AMPE2015, vol. 3, 2015, 68-74.

[12] P. Anbarasan and T. Jayabarathi, “Optimal reactive power dispatch problem solved by symbiotic organism search algorithm”. In: 2017 Innovations in Power and Advanced Computing Technologies (i-PACT), 2017, 1-8, 10.1109/ IPACT.2017.8244970. [13] A. Gagliano and F. Nocera, “Analysis of the performances of electric energy storage in residential applications”, International Journal of Heat and Technology, vol. 35, no. Special Issue 1, 2017, S41-S48, 10.18280/ijht.35Sp0106. [14] M. Caldera, P. Ungaro, G. Cammarata and G. Puglisi, “Survey-based analysis of the electrical energy demand in Italian households”, Mathematical Modelling of Engineering Problems, vol. 5, no. 3, 2018, 217-224, 10.18280/mmep.050313.

[15] M. Basu, “Quasi-oppositional differential evolution for optimal reactive power dispatch”, International Journal of Electrical Power & Energy Systems, vol. 78, 2016, 29-40, 10.1016/j. ijepes.2015.11.067. [16] G.-G. Wang, “Moth search algorithm: a bio-inspired metaheuristic algorithm for global optimization problems”, Memetic Computing, vol. 10, Articles

Journal of Automation, Mobile Robotics and Intelligent Systems

no. 2, 2018, 151-164, 10.1007/s12293-0160212-3.

[17] “Power Systems Test Case Archive”. University of Washington, Electrical & Computer Engineering, https://labs.ece.uw.edu/pstca/. Accessed on: 2021-06-28.

[18] A. N. Hussain, A. A. Abdullah and O. M. Neda, “Modified Particle Swarm Optimization for Solution of Reactive Power Dispatch”, Research Journal of Applied Sciences, Engineering and Technology, vol. 15, no. 8, 2018, 316-327, 10.19026/ rjaset.15.5917.

Articles

VOLUME 15,

N° 1

2021

VOLUME 15, N° 1, 2021 www.jamris.org

Indexed in SCOPUS



Journal of Automation, Mobile Robotics and Intelligent Systems pISSN 1897-8649 (PRINT) / eISSN 2080-2145

(ONLINE)

WWW.JAMRIS.ORG • pISSN 1897-8649 (PRINT) / eISSN 2080-2145 (ONLINE) • VOLUME 15, N° 1, 2021

Ł-PIAP

logo podstawowe skrót

Publisher: Łukasiewicz – Industrial Research Institute for Automation and Measurements PIAP

logo podstawowe skrót

Łukasiewicz – Industrial Research Institute for Automation and Measurements PIAP