· pdf fileq) c . move towards path turn towards the recorded orientation θ’ mimic...

36

Upload: truonglien

Post on 07-Mar-2018

226 views

Category:

Documents


2 download

TRANSCRIPT

����������� � ������������ ����� ��������� ��! " #$��"����%�&�(' )*���

#,+-.�/! �0' �/+1�2�����%�3�465��3 �7�8���3�

9-:�;=<?>A@%BDCECDF

G/H�IEJLKDMON(P >Q> M�R ;TS J @ UO> KWV <QX�YEZ KDH >[@RTH.IEJLKDM\H�]-^_Ma`cb%JWbd`eM�P ;=<QX�YEZ KDH > ]-^_Ma`cb%JWbd`cM7P

f1gih&j k�lDm�nQopoh&qrq%jts�lEuvmxwys&l�z{m4|

}�~��4��}������d���{�����*������������{~��4�����d�1����~��������A���6���x�A�4���p�

���-�= ¢¡3£¥¤8¦§}�~��4���¨��4©��4�

��� ����������� �

���d�����4�����

� ����������������� � ��� �

� ��"!#!��%$&�('�)+*-,/.���,0!21"�3� � �('54 �687:9 ;�<>=0?/@:AB@DCE?0F7G7H77H777H77H7G77H77H77H77G7H77H77H777H7G7H77H777 I687J6 K3LNM/OQP/RB?SATC(UWVXRTY/F-ABZ0<�RT<�[�CERTY/<�YSCERB@D<\?]A^VXAB@DCE?_7H77H77H777H7G7H77H777 `687JI K0a0Mcb+@:d�@D[HABZ0<HRT<�[�CERTY/<�Y�FBAT<�<\RB@:?/efVX?/ehgD<i77G7H77H77H777H7G7H77H777 `687 j K"klMmbNC�n�<HATC(UWVXRTY/F-ABZ0<�o3VXABZ 77H77H77H77G7H77H77H777H7G7H77H777 p

687 j07:9 bN<\ABZ0C]YqCE?0<hMrY8@:RT<�[\ABg:sto/RTCEolCERBAB@DCE?3VXg�ATC ABZ0<GY8@DFBA^VX?0[�<HATC�ABZ0<�o3VXABZ p687 j07J6 bN<\ABZ0C]YfAuUvC/MrP0FT<V�wQC%CExty-Z0<�VEY�z{CE@:?|A}7H77H777H7G7H77H777 ~

687J` �WCEd�d�VX?0Yq��P0FB@DCE?�777H77H7G77H77H77H77G7H77H77H777H7G7H77H777 �687Jp �{@:?0Y8@:?/e�ABZ0<?0<�VXRT<�FBA�o�CE@:?]A�CE?�ABZ0<Ho3VXABZ 77G7H77H77H777H7G7H77H777 �

� � )].�� � ��1��5�X����)]�{�5��) �/�

� ��)].(�{!���. �/�j07:9 �WCEd�o3VXRB@DF^CE?���<\AuUv<�<\?+ABZ0<HA�UWC�Y8@���<\RT<\?|A�d�<\ABZ0C%Y/F�ATC�[�CEd�o/P/AT< K3k 777 9�I

j07:9h7:9 ��CE@DFT<�7H77H777H77H7G77H77H77H77G7H77H77H777H7G7H77H777 9�Ij07J6 O�Z0<H<>��<�[\A�CX��ABZ0<HABZ/RT<�<Y8@���<\RT<\?|A��l<\Z3V�n%@DCEP/RTF�7H77H77H777H7G7H77H777 6h6

j07J687:9 w�@:d�@:A^VXAB@DCE?0F�77H77H7G77H77H77H77G7H77H77H777H7G7H77H777 6�~j07J687J6 �c?0Y8olCE@:?|A�[�CERBRT<�[\AB@DCE? 77H77H77H77G7H77H77H777H7G7H77H777 6h�

j07JI �-�0FTA^VE[\gD<GV�n�CE@DY0VX?0[�<�77H77H7G77H77H77H77G7H77H77H777H7G7H77H777 Ih�

� � �"�5�|!#��. � ����. �Q�

� � �� Q�5�%$�!�)|�{13)]4¡)]����. ���

��)]¢�)|�X)]�5��)]. �"�

� £H,/� � ,/¤�!2)].��5.�)|� �3�

�������G��S� � �

��� �_���{�d�E�

y ?|P/dG�l<\R CX�-VXg:e�CERB@:ABZ/d�FG� CERGo3VXABZ ABR^VE[Tx%@:?/e VXRT<�Y/<�FT[\RB@:�l<�Y_@:? ABZ0<�RTCE�lCEAB@D[�F g:@:AT<\R^VXABP/RT<h7OQR^VEY8@:AB@DCE?3VXgcVXg:e�CERB@:ABZ/d�F�g:@:x�<zmP/RT<zcP/RTFBP/@:A�� �WCEP3�h6��{VX?0Y��0CEg:gDC(U ABZ0< ��VXRBRTCEA�����VXR^�89�QP0FT<o�ChFB@:AB@DCE? @:?8�#CERBd�VXAB@DCE? ATC_[�CEd�o/P/AT<SFTAT<�<\RB@:?/e�[�CEd�d�VX?0Y/F�ABZ3VXA�d�VXx�<+V n�<\Z/@D[\gD<NVXo/o/RTC� �@:d�VXAT<\g:s �#CEg:gDC(U Vfo/RT<�Y/<>=0?0<�Y�o3VXABZQ7qO�Z0<�FT<tVXg:e�CERB@:ABZ/d�F�VXRT<�Uv<\g:gWx]?0C(U�?¡ATCS[\P/A [�CERB?0<\RTFFB@:?0[�<rABZ0<\s Y/C-?0CEA�<�8o/g:@D[\@:ABg:sGA^VXx�<v@:?|ATCHVE[�[�CEP/?|AcABZ0<�VE[\ABP3VXg8[\P/RBnEVXABP/RT<�CX�0ABZ0<Wo3VXABZQ7���?�ABZ/@DFo3VXo�<\R�Uv< o/RT<�F^<\?|AGV�?0C�n�<\grVXg:e�CERB@:ABZ/d&ABZ3VXAHP0FT<�F�RT<�[�CERTY/<�Y FBAT<�<\RB@:?/e�[�CEd�d�VX?0Y/FATCtC(n�<\R��[�CEd�<ABZ/@DF-o/RTCE�/gD<\d�7�O�Z0< VXg:e�CERB@:ABZ/d�@DF�[�CE?0FBABRBP0[\AT<�YSU�@:ABZ/@:?+ABZ0<G��<\Z3V�n]@DCEP/R^VXg5o3VXR^VEY8@:ehd��VX?0Y¡@DF Y8@:n]@DY/<�Y�@:?|ATC+ABZ/RT<�<�F^<\o3VXR^VXAT<��l<\Z3V�n%@DCEP/RTF��c<�VE[TZ CE?0<�RT<�FTo�CE?0FB@:�/gD<��#CER�CE?0<�VEFBol<�[\ACX��ABZ0<�o3VXABZ¡ABR^VE[^x]@:?/eNA^VEFBxl7fO�Z0<�VXg:e�CERB@:ABZ/d @DFG@:d�o/gD<\d�<\?]AT<�Y�@:? V�FB@:d P/g�VXATCERG� CERH� CERT<�FBAd�VE[TZ/@:?0<�F�VX?0Y�ABZ0<�RT<�FBP/g:ATF VXRT<t[�CEd�o3VXRT<�Y�U�@:ABZ ABZ0<�zmP/RT<tzmP/RTFBP/@:A VX?0Y�ABZ0<t�0CEg:gDC�U ABZ0<��VXRBRTCEAWVXg:e�CERB@:ABZ/d�F�7cO�Z0<�RT<�FTP/g:ATFrFBZ0C(U VHFB@:eh?/@�=3[�VX?]Ar@:d�o/RTC�n�<\d�<\?]Av@:?�o�<\R � CERBd�VX?0[�<��|�lCEABZ�#CER@DY/<�VXgv?0CE@DFT<��#RT<�<�olChFB@:AB@DCE?�Y0VXA^V���VX?0Y�VXgDF^Cq� CERo�ChFT@:AB@DCE?¡Y0VXA^VqU�@:ABZ VEY/Y/<�Y�FB@:d P/g�VXAT<�Y?0CE@DFT<h7

� � ��������� ���� *W� �Q� � ���+� �t� ���

£ � �¢���{� ©�� �E���A�¢�

O�Z0<��� ����� ����������������cVXg:e�CERB@:ABZ/d�Z3VEF5��<�<\?�Y/<\n�<\gDCEo�<�Y�VEF5o3VXRBA{CX�0ABZ0<ro/RTC� <�[\A"!$#%�& (') �*� �#+�, ��-/.�01�(�2.3 �'546 �78�� �79�����;:<�1=>�?.�'@�A� �CB�<\g��h6 ��VXAED-d�</F5D-?/@:n�<\RTFB@:A�s�7mO�Z0<�e�C�VXg"CX��ABZ/@DFmo/RTC� B<�[\A@DFATCSY/<\n�<\gDCEo Vfo3VXABZ�ABR^VE[Tx%@:?/eS� CERT<�FBAn�<\Z/@D[\gD<h7qy Z|P/d�VX? Y8RB@:n�<\R Y8RB@:n�<�FABZ0<�o3VXABZ CE?0[�<U�Z/@:gD<HABZ0<[�CEd�o/P/AT<\R-FTs%FBAT<\d RT<�[�CERTY/F�o�ChFT@:AB@DCE? �/n�<\gDC][\@:Aus �3CERB@D<\?]A^VXAB@DCE? VX?0Y�FTAT<�<\RB@:?/e�VX?/ehgD<VXA-<\n�<\RBsqd�CEd�<\?|A�7 ��?+ABZ/@DF�U�V�sqUW<Gx%?0C�U�<�/VE[\ABg:sqZ0C(U ABZ0< n�<\Z/@D[\gD<Gd�C(n�<�YNVXA�V�FBol<�[\@�=3[o�CE@:?]A��]VX?0Y�[�VX?�P0F^<�ABZ/@DFc@:?8�#CERBd�VXAB@DCE?t<�VE[TZ�AB@:d�<�ABZ0<�n�<\Z/@D[\gD<-VXP/ATCE?0CEd�CEP0FBg:s ABR^V�n�<\gDFWVXgDCE?/eABZ0<�o3VXABZQ7+OQR^VEY8@:AB@DCE?3VXg�VXg:e�CERB@:ABZ/d�F g:@:x�<G�� ����H ��I�����KJL�(7A79 (� ����VXR^�89��VX?0YM�8#%7N�O�8#%7P��#+.2�� �WCEP3�h6���P0F^<�olChFB@:AB@DCE?�@:?8� CERBd�VXAB@DCE?qCE?/g:s �8VX?0Y�FTCEd�<\AB@:d�<�FWRBP/?�@:?]ATCGo/RTCE�/gD<\d�F�ABZ3VXA�[�VX?��l<V�n�CE@DY/<�Y��|s A^VXx]@:?/eS@:?|ATC+VE[�[�CEP/?]AGABZ0<�VEY/Y8@:AB@DCE?3VXgW@:?8� CERBd�VXAB@DCE?_�#RTCEd ABZ0<�Z|P/d�VX? Y8RB@:n�<\R�7� �WABZ0<�n�<\Z/@D[\gD<�e�<\ATFGCX� [�CEP/RTFT<��Q�#CER<�0VXd�o/gD<�U�Z0<\?�V�n�CE@DY8@:?/eNVX?_CE�0FBA^VE[\gD<�CERG��<�[�VXP0FT<�CX�?0CE@DFT<�@:?fABZ0<-o�ChFT@:AB@DCE?/@:?/e F^<\?0FTCERTF��]ABZ0<5�� ����H ��Q���R�S�����>��VXg:e�CERB@:ABZ/d FTAT<�<\RTFvg:@:x�<�ABZ0<�Y8RB@:n�<\R��o/g:P0F�VX? VEY/Y8@:AB@DCE?3VXg�VX?/ehgD<��3VEFT<�Y¡CE?�ABZ0<�Y8@DFBA^VX?0[�<�ATC�ABZ0<�o3VXABZQ7qO�Z/@DF RT<\olCERBA Y/<�FT[\RB@:�l<�FABZ0<�Y/<\RB@:nEVXAB@DCE?�CX�WABZ0<�VXg:e�CERB@:ABZ/d VX?0Y o/RT<�FT<\?|ATF FB@:dGP/g�VXAT<�Y¡RBP/?0FGVX?0Y_[�CEd�o3VXRB@DFTCE?0FGU�@:ABZABR^VEY8@:AB@DCE?3VXg|o3VXABZABR^VE[^x]@:?/e�VXg:e�CERB@:ABZ/d�F�75y-g:g�FB@:d P/g�VXAB@DCE?0F{VXRT<cRBP/? CE?GV�Y/<�Y8@D[�VXAT<�YGFB@:d P/g�VXATCERY/<\n�<\gDCEo�<�Yq�]sT�@:?/e�Y0VXZ/g �CT�@:?3�hI ��7

U V �SWNWA�.¨ �YX �[Z1�d� ���\WA�d���y���)X�~

;-P/RB@:?/e ABZ0<-d�VX?|P3VXg�Y8RB@:n%@:?/e�CX��ABZ0<-o3VXABZ �%ABZ0<-CERB@D<\?]A^VXAB@DCE?SVX?0Y�FTAT<�<\RB@:?/e�VX?/ehgD<�VXRT<�FBATCERT<�YATCEe�<\ABZ0<\R�U�@:ABZ ABZ0<qolChFB@:AB@DCE? VXA�<\n�<\RBs d�CEd�<\?|A�7 y�g:g�ABZ/@DF�@:?8� CERBd�VXAB@DCE? @DF�P0FT<�Y �|s ABZ0<�] ����H (�^�����_�����>�Wd�<\ABZ0C]Y �/U�Z/@D[^ZS@DF�[�CEd�olChFT<�YqCX��ABZ/RT<�<H@:?0Y/<\ol<\?0Y/<\?|A���<\Z3V�n]@DCEP/RTF�M

K0L�MiO�P/RB?SATC�U�VXRTY/F�ABZ0<�RT<�[�CERTY/<�Y+CERB@D<\?|A^VXAB@DCE?Q7K a M}b+@:d�@D[HABZ0<HRT<�[�CERTY/<�Y�FTAT<�<\RB@:?/e�VX?/ehgD<h7K k M bNC�n�<HATC(UWVXRTY/F-ABZ0<Ho3VXABZQ7

�rVE[TZ ��<\Z3V�n]@DCEP/RfFBP/ehe�<�FBAfV_FBAT<�<\RB@:?/e VX?/ehgD<NVX?0Y @DF�RT<�VE[\AB@:n�<���@ 7 <+CEo�<\R^VXAT<�FtCE? ABZ0<N[\P/R��RT<\?|A�@:?/o/P/A�nEVXg:P0<�F/`lCERB@D<\?]A^VXAB@DCE? �QFBAT<�<\RB@:?/e�VX?/ehgD<�VX?0Y+FBZ0CERBAT<�FTA�Y8@DFBA^VX?0[�<�ATC�ABZ0<Go3VXABZQ7GK kP0FT<�F�RT<�[�CERTY/<�Y [\gDChFT<�FBA�o�ChFT@:AB@DCE?0Fba�c)d2e9f+dhg�VX?0Y VE[\ABP3VXg�olChFB@:AB@DCE?Qa�c�e9fig�VEF-@:?/o/P/ATF(7 K3LqP0FT<�FRT<�[�CERTY/<�Y CERB@D<\?]A^VXAB@DCE?kj(d�VX?0Y VE[\ABP3VXg�CERB@D<\?|A^VXAB@DCE?kj�VEF�@:?/o/P/ATF�7 K a P0FT<�F�ABZ0<+RT<�[�CERTY/<�YFBAT<�<\RB@:?/e VX?/ehgD<�Kld"VEFm@:?/o/P/ATF�7cO�Z0<�ABZ/RT<�<��l<\Z3V�n%@DCEP/RTF�VXRT<��#P0FT<�Y�@:?]ATC CE?0<�VE[\AB@DCE? �|ABZ0<-[�CEd �d�VX?0Y/<�YNFTAT<�<\RB@:?/eqVX?/ehgD<�Klm ��VEF�FBZ0C�U�? @:? �5@:ehP/RT<f9h7�O�Z0< =0ehP/RT<�VXgDFTC�FTZ0C�U�FVX? CE�0FBA^VE[\gD<V�n�CE@DY0VX?0[�<GFTs%FBAT<\d VX?0Y�Z0C�U @:A�@DF�@:?|AT<\ehR^VXAT<�Y�U�@:ABZSABZ0<Ho3VXABZ�ABR^VE[Tx%@:?/e07

���#� �q)��c� � � � ����. �

Move towards path

Turn towards the recorded orientation θ’

Mimic the recorded steering Φ’

+

(x, y)

θGPS/INS

θ’

(x’, y’)

Φ’

Φt

w1

w2

w3

Φα

Φβ

Φγ

Recorded path

w4

Safe speedv’vt

Avoid obstacles

Occupancy grid

svs

vo

turn

s

Φα

Φsum

� 1"�5��) ��� z{VXABZ+ABR^VE[Tx%@:?/e�U�@:ABZqRT<�VE[\AB@:n�<G[�CE?]ABRTCEg5CX�cFBAT<�<\RB@:?/e�VX?/ehgD< KlmrVX?0Y�FTo�<�<�Y���m

B ` � P � X%< R < I X M

� @:?0[�<mABZ0<c� CERT<�FBA5n�<\Z/@D[\gD<mP0FT<�F{VXRBAB@D[\P/g�VXAT<�Y�FTAT<�<\RB@:?/e ��ABZ0<vY/<>=0?/@:AB@DCE?�CX�/Z0<�VEY8@:?/e �hCERB@D<\?]A^VXAB@DCE?VX?0Y FTAT<�<\RB@:?/e VX?/ehgD<Nd�V�s ?0CEA��l<NCE�|n%@DCEP0FfATC¡ABZ0<+RT<�VEY/<\R�7 ��? ABZ/@DFto3VXo�<\RtUv<NP0FT<+ABZ0<�#CEg:gDC(U�@:?/e�Y/<>=0?/@:AB@DCE?0F�M

O�Z0<�FBAT<�<\RB@:?/eSVX?/ehgD<�K¡@DFHY/<>=0?0<�Y_VEF�ABZ0<�VX?/ehgD< ��<\AuUv<�<\?_ABZ0<��#RTCE?|AGVX?0Y RT<�VXRFT<�[\AB@DCE?0FHCX�ABZ0<Hn�<\Z/@D[\gD<��"VEF�FTZ0C�U�?�@:?�ABZ0<gD<>��A�o3VXRBA-CX���{@:ehP/RT<G687 �uA�@DF�[�CE?]n�<\?/@D<\?|A�ATC�@DY/<\?|AB@��#sfZ3VXg���CX�ABZ/@DF-VX?/ehgD<��0@ 7 <h7vABZ0< VX?/ehgD<��<\AuUv<�<\?NABZ0<�3VEFT<\g:@:?0<�VX?0Y�<�VE[TZ+n�<\Z/@D[\gD<GFT<�[\AB@DCE? ��VEF�FBZ0C(U�?S@:?ABZ0<HRB@:ehZ|A�o3VXRBA-CX���{@:ehP/RT<G687

B-<�VEY8@:?/e��q@DF-Y/<>=0?0<�Y VEF�ABZ0< Y8@:RT<�[\AB@DCE? CX�cABZ0<�#RTCE?|A�o3VXRBA�CX�mABZ0<n�<\Z/@D[\gD<��lY/<>=0?0<�YS@:?NABZ0<ehgDCE�3VXg�[�C]CERTY8@:?3VXAT< FBs8FBAT<\d VEF�FBZ0C(U�?�@:?��{@:ehP/RT<GI87

O�Z0<-CERB@D<\?]A^VXAB@DCE? j CX�QABZ0<-n�<\Z/@D[\gD<��8Y/<>=0?0<�Y�@:?tABZ0<-ehgDCE�3VXg�[�C%CERTY8@:?3VXAT<�FBs%FTAT<\d VEFWFBZ0C�U�?f@:?�5@:ehP/RT<�I��/@DF�ABZ0<GY8@:RT<�[\AB@DCE?S@:?�U�Z/@D[^Z+ABZ0<Hn�<\Z/@D[\gD<GUvCEP/gDYSABR^V�n�<\g�@��cABZ0<FTAT<�<\RB@:?/etVX?/ehgD<U�VEF� <\RTCfVX?0YNABZ0< �3VEFT<\g:@:?0<�UWVEF�d�VX@:?]A^VX@:?0<�Y �5VEF�ABZ0< Y0VEFTZ0<�Y+n�<\Z/@D[\gD< @:? �{@:ehP/RT<�I87 ��A�[�VX?+�l<[�CEd�o/P/AT<�Y�VEFvABZ0<�Z0<�VEY8@:?/e�CX��ABZ0<�n�<\Z/@D[\gD<�d�@:?|P0F�Z3VXg���ABZ0<�FBAT<�<\RB@:?/e�VX?/ehgD<��%@ 7 <h7Lj���������l7

� � ��������� ���� *W� �Q� � ���+� �t� ���

Φ/2

Φ/2

Φ

Φ = steering angle

� 1����X) ��� ;�<>=0?/@:AB@DCE? CX��ABZ0<tFBAT<�<\RB@:?/e VX?/ehgD<fK�� CER�Vq�#CERT<�FTAGd�VE[TZ/@:?0<tU�@:ABZ VXRBAB@D[\P/g�VXAT<�YFBAT<�<\RB@:?/e07

Φ/2

θ η

Φ = Steering angleθ = Orientationη = Heading

θ 0°

90°

180°

360°

� 1"�5�X)S� � ;�<>=0?/@:AB@DCE?+CX��ABZ0<�n�<\Z/@D[\gD< � F-CERB@D<\?]A^VXAB@DCE? j �0VX?0YfZ0<�VEY8@:?/e ��7

����� K3L � � �5�E� ���%$ ,/�X�5.��('�)+��)]���3�X�5)]� �"� � )]����,8� � ��� �

B ` B � L�� G�b ;=X R I�� K ;=Z MORTH.P ; P ^ I ;=Z P Z I ;=< P X R\K{R < I X

O�Z0<WVX?/ehgD<�j d @DF�Y/<>=0?0<�Y�VEF{ABZ0<vRT<�[�CERTY/<�Y CERB@D<\?]A^VXAB@DCE?�VXA5ABZ0<W[\gDChFT<�FBA{olCE@:?|A�CE? ABZ0<vRT<�[�CERTY/<�Yo3VXABZQ7�O�Z/@DF�olCE@:?|A�@DF�[�VXg:gD<�Y+ABZ0<����(������ �.�'l� 7�K3Lf@DF-[�CEd�o/P/AT<�Y VEF-ABZ0<GY8@���<\RT<\?0[�<G�l<\A�UW<�<\?ABZ0<[\P/RBRT<\?|A�CERB@D<\?|A^VXAB@DCE? j�VX?0YfABZ0<HRT<�[�CERTY/<�YSCERB@D<\?]A^VXAB@DCE? j(dLa�RT<>�#<\R�ATC��5@:ehP/RT<j+g>M

K3L �kj d � j� a 687:9 g

� @:?0[�< ABZ0<+�l<\Z3V�n%@DCEP/R+K3L CE?/g:s @DFt[�CE?0[�<\RB?0<�Y U�@:ABZ ABZ0<N<\RBRTCERq@:? CERB@D<\?]A^VXAB@DCE? ��Uv<Nd�V�s@:d�VXeh@:?0<�ABZ0<�n�<\Z/@D[\gD<�� CEg:gDC�U�@:?/e V�n%@:RBABP3VXg0o3VXABZ�o3VXR^VXg:gD<\g0ATC�ABZ0<�RT<�[�CERTY/<�Y�o3VXABZtVX?0Y o3VEFTFB@:?/eABZ/RTCEP/ehZSABZ0<HFBAT<�<\RB@:?/e5 BCE@:?|A-CX�{ABZ0<Hn�<\Z/@D[\gD< VEF�FBZ0C�U�?S@:?S�{@:ehP/RT<Hj07

θ = Current orientation

θ

Path

Φβ

VirtualPath

θ’

Φ β = Orientation differenceθ’ = Recorded orientation

Path Point

� 1"�5��)S� � O�Z0<Hn�<\Z/@D[\gD<�� CEg:gDC�U�F�V n]@:RBABP3VXg�o3VXABZQ7

B `� � a � � < J < ^�RTH.P ; P ^ I ;=Z P Z M�R P P ;=<?X.Y K X.YE> P

O�Z/@DFv�l<\Z3V�n%@DCEP/R�FB@:d�o/g:s�RT<\ABP/RB?0FWABZ0<�RT<�[�CERTY/<�YfFBAT<�<\RB@:?/e�VX?/ehgD<�K d VXAvABZ0<�ABZ0<�[\gDChFT<�FBAvo�CE@:?]ACE?�ABZ0<�RT<�[�CERTY/<�Y�o3VXABZ a�RT<>� <\R�ATC��{@:ehP/RT<G`1g>7

K a � K d a 687J61g

�vs P0FT@:?/e�ABZ0<SRT<�[�CERTY/<�Y FTAT<�<\RB@:?/e VX?/ehgD<���ABZ0<N[\P/RBnhVXABP/RT<NCX�ABZ0<So3VXABZ @DFfVXP/ATCEd�VXAB@D[�VXg:g:s@:?0[\g:P0Y/<�Y @:? ABZ0<+=0?3VXg�FTAT<�<\RB@:?/e [�CEd�d�VX?0Y�7 O�Z/@DFt@DFfV_ehRT<�VXA�VEY8nhVX?|A^VXe�< [�CEd�o3VXRT<�Y ATCd�<\ABZ0C]Y/F�g:@:x�<��8#+7N���)#%7P��#+.2� � �WCEP3�h6 �5VX?0Y �� ����H �� �����GJL��7�7 (� ����VXR^�89���7

� � ��������� ���� *W� �Q� � ���+� �t� ���

Path

δθ’

Φγ

θ’ = Recorded orientationΦ γ = Recorded steering angle

Path Point

� 1����X)+� � ��VXgD[\P/g�VXAB@DCE?NCX�mK0a37

B ` F � k � � I � P R I�� K ;=Z MORTH.P : K{RTH

O�Z/@DF���<\Z3V�n]@DCEP/R�@DF�RT<�FTo�CE?0FB@:�/gD<-� CER��/RB@:?/eh@:?/e�ABZ0<�n�<\Z/@D[\gD<H�3VE[TxtATC�ABZ0<�o3VXABZq@���ABZ0<�n�<\Z/@D[\gD<�#CER�FTCEd�< RT<�VEFTCE? e�<\ATF�CEP/ATFT@DY/<GABZ0<�o3VXABZQ7�O�Z/@DF�[�VX?+�l< [�VXP0F^<�Y+�]s�?0CE@DFT< @:?+ABZ0<�olChFB@:AB@DCE?FB@:eh?3VXgDF���CERf�|s ABZ0< CE�0FBA^VE[\gD<¡V�n�CE@DY0VX?0[�<�FBs8FBAT<\d���U�Z/@D[^Z @DFSV FT<\o3VXR^VXAT< �l<\Z3V�n%@DCEP/R�?0CEAY/<�FT[\RB@:��<�Y�@:?qABZ/@DF�RT<\olCERBA�7

K k [�VX?q�l<�@:d�o/gD<\d�<\?]AT<�Y�@:?fd�VX?|s�UWV�s8F�7��_<�Z3V�n�<�@:d�o/gD<\d�<\?|AT<�YqAuUvC�Y8@���<\RT<\?|A�d�<\ABZ0C%Y/FVEF�Y/<�FT[\RB@:�l<�Yq��<\gDC(U7

���:� �#� )��('5��� ����) �� � �X)|�h�(!�� �{���"�m�3��� � �"�5,0!c���_�('5)S� � .���,0�5��)S���¡�('�)+�5,8�('

O�Z0<rFB@:d�o/gD<�FBA5UWV�sH@DF�ATC�[�VXgD[\P/g�VXAT<�V�nhVXg:P0<��XY8@:RT<�[\ABg:sHo/RTCEo�CERBAB@DCE?3VXg%ATC�ABZ0<vY8@DFBA^VX?0[�<m�l<\A�UW<�<\?ABZ0<�n�<\Z/@D[\gD<�VX?0Y ABZ0<�[\gDChFT<�FBAHolCE@:?|A CE? ABZ0<�RT<�[�CERTY/<�Y_o3VXABZ^a�RT<>� <\RATC��{@:ehP/RT<�p1g>7�O�Z/@DFH@DFY/CE?0<H�|sfdGP/g:AB@:o/g:s%@:?/e�ABZ0<HY8@DFTA^VX?0[�<ATC ABZ0<Ho3VXABZ�U�@:ABZNV [�CE?0FBA^VX?|A�M

K k ����� a 687JI1gU�@:ABZSABZ0<[�CE?0Y8@:AB@DCE?QM

� K3k �� ������

U�Z0<\RT<�lM V [�CE?0FBA^VX?|A�� �������� �3M ABZ0<FB@:eh?0<�Yfol<\RBo�<\?0Y8@D[\P/g�VXR�Y8@DFBA^VX?0[�<H�l<\A�Uv<�<\?+ABZ0<Hn�<\Z/@D[\gD<GVX?0YqABZ0<�o3VXABZ

���:� K k � ���")+���%$ ,/�X�5.��('�)+�5,8�(' �

�-CEAT< ABZ3VXA t@DF-?0<\e|VXAB@:n�< CER�olChFB@:AB@:n�< Y/<\ol<\?0Y8@:?/eqCE?SU�Z/@D[^Z FT@DY/<GCX�mABZ0< o3VXABZ+ABZ0< n�<\Z/@D[\gD<@DF�7�y Aus]o/@D[�VXg5nEVXg:P0<�#CER �f@:?SABZ0< [\P/RBRT<\?|AHVXo/o/g:@D[�VXAB@DCE?+@DF��87J��~%7�O�C�<\?0FBP/RT<ABZ3VXAHK k Y/ChFT<�F?0CEA��l<�[�CEd�<�ATC]C�g�VXRBe�<�U�Z0<\? ABZ0<�Y8@DFTA^VX?0[�< ATC�ABZ0<�o3VXABZ @DF-g�VXRBe�<��lUW<�?0<�<�Y V�[�CE?0FBABR^VX@:?]A�7O�Z0<�A�UWC��l<\Z3V�n%@DCEP/RTF K3L VX?0Y K a FT<�<�ATCqABZ3VXAGABZ0<�n�<\Z/@D[\gD<�@DFo3VXR^VXg:gD<\gvATC�ABZ0<�o3VXABZ �cVX?0YABZ0<\RT<>�#CERT<HABZ0< VX?/ehgD< K k dGP0FBA��l<�U�@:ABZ/@:? � � ��� � e ��� � � �/@ 7 < � K k � ��� � 7

Path

Path Point d

d = Distance to path

� 1"�5�X)+� � ��VXgD[\P/g�VXAB@DCE?NCX�mK k @:?SbN<\ABZ0C]YqCE?0<h7

���:� ��� )��('5��� � $ � �G��.�)+, � ���"  ��'5)|,/� *-� � ���

� @:ABZ b+<\ABZ0C%Y CE?0<���ABZ0<+[�VXgD[\P/g�VXAB@DCE? CX�ABZ0< VX?/ehgD< K"k @DF�g:@:?0<�VXR�7 O�Z/@DFtd�V�s RT<�FBP/g:At@:?ChFT[\@:g:g�VXAB@DCE?0F�VX�lCEP/AABZ0<�o3VXABZ_U�Z0<\?¡ABZ0<�n�<\Z/@D[\gD<�@DF[\gDChFT<�ATCq@:A�7GT�<�Y8P0[\@:?/e�ABZ0<�nEVXg:P0< � CER�SU�@:g:g�RT<�FBP/g:A�@:? VtFBgDC�U�RT<�FBolCE?0FT< �#CER�g�VXRBe�< Y �l@ 7 <h7�ABZ0<�n�<\Z/@D[\gD< U�@:g:g{A^VXx�<�gDCE?/e�<\RHAB@:d�<�ATCRT<�VE[TZ_ABZ0< o3VXABZQ7 bN<\ABZ0C]YNA�UWCSC(n�<\RT[�CEd�<�FABZ/@DF�o/RTCE�/gD<\d �]s+P0FB@:?/eSV��� / �� !S���9��� �� �.�'l�VX?0Y [�VXgD[\P/g�VXAT< ABZ0<�VX?/ehgD< ��<\AuUv<�<\?_ABZ0<�n�<\Z/@D[\gD<�VX?0Y ABZ0< wQC%CExNy-Z0<�VEY z5CE@:?]A���FT<�< �{@:ehP/RT<~%7mO�Z0<VXg:e�CERB@:ABZ/d � CER�[�CEd�o/P/AB@:?/efK k @DF�M

9h7�;�<\AT<\RBd�@:?0<ABZ0<[\gDChF^<�FBA�o�CE@:?]A�CE?�ABZ0<�RT<�[�CERTY/<�YSo3VXABZ a�@ 7 <h7cABZ0<�o3VXABZ�olCE@:?|AAg>7

687��WCEd�o/P/AT<HV�] ��5!S���N������ �.�'l�mVXAWV�] / ��_!S���9��� �5.����&��'Y=P�����#RTCEd ABZ0<�o3VXABZ�o�CE@:?]A��@:?�ABZ0<�Y8@:RT<�[\AB@DCE?��-@:?��{@:ehP/RT<-~%7�O�Z/@DFvVX?/ehgD<�@DFcABZ0<�FBP/d CX�lABZ0<�RT<�[�CERTY/<�Y�CERB@D<\?]A^VXAB@DCE?j(d�VX?0Y�FBAT<�<\RB@:?/e�VX?/ehgD<�Kid�VXA�ABZ0<Ho3VXABZSo�CE@:?]A�7

I87���VXgD[\P/g�VXAT<�V��� / ��$!S���9���5!$'+0(����� ��Y/<>=0?0<�Y�VEF�ABZ0<�VX?/ehgD<�CX��ABZ0<�n�<�[\ATCERc�l<\A�UW<�<\?�ABZ0<n�<\Z/@D[\gD< � F�[\P/RBRT<\?|A�[�C%CERTY8@:?3VXAT<�F�VX?0Y�ABZ0<�wQC%CEx�y�Z0<�VEYtz5CE@:?]A��8@:?�ehgDCE�3VXgl[�C%CERTY8@:?3VXAT<�F�7

� � ��������� ���� *W� �Q� � ���+� �t� ���

j07��WCEd�o/P/AT<GK k VEFWABZ0<�Y8@��l<\RT<\?0[�<��l<\A�UW<�<\?qABZ0<�w�C]CEx�y-Z0<�VEYfy�?/ehgD<�VX?0YtABZ0<�VX?/ehgD< �]7

� @:ABZ�ABZ0<�?0CEA^VXAB@DCE?+@:?��{@:ehP/RT< ~�Uv<e�<\A�M

K k � � � � a 687 j+g��� �����

��� K d�� j d � K3L � K d�� j a 687J`1g

U�Z0<\RT<�-M @DF�ABZ0<w�C]CExfy�Z0<�VEYSy�?/ehgD<j(d2M @DF�ABZ0<HRT<�[�CERTY/<�YSCERB@D<\?|A^VXAB@DCE? VXA�o3VXABZ�o�CE@:?]Aj0M @DF�ABZ0<[\P/RBRT<\?]A-CERB@D<\?]A^VXAB@DCE?K d M @DF�ABZ0<HRT<�[�CERTY/<�YSFBAT<�<\RB@:?/e�VX?/ehgD<GVXA�o3VXABZSo�CE@:?]A

Φα = Error angle

Path

δ = θ ’ + Φ’

δ

Look AheadPoint

δ Ψ

Φα

Look Ahead Distance

Ψ = Look Ahead Angle

Path Point

� 1"�5��) � � ��VXgD[\P/g�VXAB@DCE?+CX�rK k @:?�bN<\ABZ0C%YqA�UWC/7

B `�� /IEJ JLK X%Z�� b%M < I X

O�Z0<fABZ/RT<�<q��<\Z3V�n]@DCEP/RTFtK k e K3L VX?0Y K a VXg:g�RT<\ABP/RB? V FBP/ehe�<�FTAT<�Y FBAT<�<\RB@:?/e�VX?/ehgD<h7 O�Z0<�FT<ABZ/RT<�<�nEVXg:P0<�FWVXRT<��#P0FT<�Y @:?|ATC CE?0<�nEVXg:P0<�Kim��]s�FB@:d�o/gD<�VEY/Y8@:AB@DCE?�VEFvFBZ0C�U�?�@:?��5@:ehP/RT<G9h7��>7 <h7:M

Klm � K3L � K a � K k � a 687Jp1g

���D� � ��� � ��1 �('5) ��)],/��)].����m� � �����"� �('5)S�{,8�(' �

�0CER�bN<\ABZ0C]YS�-?0<GVX�lC�n�<��3ABZ/@DF�<�8o/RT<�FTFB@DCE?qU�@:g:gQ�l<hM

Klm � j d ��j � K d � � � a 687 ~(g

U�Z0<\RT<j d @DF�ABZ0<HRT<�[�CERTY/<�YSCERB@D<\?|A^VXAB@DCE? VXA�ABZ0<Ho3VXABZ�olCE@:?|Aj @DF�ABZ0<[\P/RBRT<\?]A-CERB@D<\?]A^VXAB@DCE?K d @DF�ABZ0<HRT<�[�CERTY/<�YSFBAT<�<\RB@:?/e�VX?/ehgD<GVXA�ABZ0<o3VXABZqo�CE@:?]A� @DF�V [�CE?0FBA^VX?]A @DF�ABZ0<FTZ0CERBAT<�FBA�FB@:eh?0<�Y�Y8@DFBA^VX?0[�<��<\AuUv<�<\?SABZ0<n�<\Z/@D[\gD<GVX?0YqABZ0<�o3VXABZ�olCE@:?|A

O�Z0<�o3VXABZ olCE@:?|A @DF ABZ0<f[\gDChFT<�FBA o�CE@:?]A �#RTCEd ABZ0<�FBAT<�<\RB@:?/eG BCE@:?|A CX��ABZ0<�n�<\Z/@D[\gD<�ATC+ABZ0<RT<�[�CERTY/<�Y�o3VXABZ �3VEF�@:g:g:P0FTABR^VXAT<�Yq@:?S�{@:ehP/RT< ~%7

�0CERWbN<\ABZ0C%Y�OQUWC�VX�lC�n�<��]ABZ0<�<�8o/RT<�FTFB@DCE?��#CERvABZ0<��#P0FT<�YtKlmQU�@:g:g��l<�a�RT<>�#<\RvATCG�{@:ehP/RT<��GVX?0Y���|P3VXAB@DCE?0F�687J6��3687 j�VX?0Y�687J`1g>M

Klm � K k � K3L � K a �� � � � � K3L � K a �� � ��� K3L � K d � j�� � K3L � K d � a 687J�1g� � ��j

U�Z0<\RT<� @DF�ABZ0<w�C]CExfy�Z0<�VEYSy�?/ehgD<� @DF�ABZ0<GVX?/ehgD<H��RTCEd ���]P3VXAB@DCE?N687J`j(d @DF�ABZ0<HRT<�[�CERTY/<�YSCERB@D<\?|A^VXAB@DCE? VXA�ABZ0<Ho3VXABZ�olCE@:?|Aj @DF�ABZ0<[\P/RBRT<\?]A-CERB@D<\?]A^VXAB@DCE?Kid�@DF�ABZ0<HRT<�[�CERTY/<�YSFBAT<�<\RB@:?/e�VX?/ehgD<GVXA�ABZ0<o3VXABZqo�CE@:?]A

B `�� <QX�Z%<QX�Y RTH.P X P K ; P M�R : I <?X R I X RTH.P : K{RTH

��?qCERTY/<\R�ATC [�VXgD[\P/g�VXAT<HABZ0<�gDC][�VXAB@DCE?�CX��ABZ0<Hw�C]CEx�y-Z0<�VEYqz{CE@:?|A��/ABZ0<�?0<�VXRT<�FBA�olCE@:?|A�CE?qABZ0<o3VXABZ a#Y/<\?0CEAT<�YNABZ0<o3VXABZNolCE@:?|AAg�dGP0FBA-=0RTFBA���< Y/<\AT<\RBd�@:?0<�Y ��F^<�<G�{@:ehP/RT<��87-O�Ct[�VXgD[\P/g�VXAT<ABZ/@DF��{ABZ0<�Y8@DFBA^VX?0[�<���<\AuUv<�<\? VqolCE@:?|A�VX?0Y Vqg:@:?0<�VXR FT<\ehd�<\?|A�Z3VEFGATC+�l<�[�VXgD[\P/g�VXAT<�Y�7Sy-?VXg:e�CERB@:ABZ/d �#RTCEd��<�CEd�<\ABRBs%y-g:e�CERB@:ABZ/d�F�7 [�CEd � FTCX� �hI �Q@DF�P0FT<�Yf� CER�ABZ/@DF�M

Y8@DFBA^VX?0[�<1acz{CE@:?|A � � � <\ehd�<\?|A���� �����$g�

� ����� ������ ��� �����@�������� � � �"!#� ��$ � � �&%�%('�)+*-,+.0/21�'"/�34'0.�'�)+*�5�*768'9.86�:<;

RT<\ABP/RB? =����e>���2�

��� � ��������� ���� *W� �Q� � ���+� �t� ���

Look AheadPoint

Look Ahead Distance

Φt = Resulting steering angleθ = Orientation

θ

Φ’ = Recorded steering angle

Ψ

Ψ = Look Ahead Angle

Path

Φt

δ = θ ’ + Φ’

δPath Point

� 1"�5��) � � �WCEd�d�VX?0Yq�#P0FB@DCE?f�#CER�bN<\ABZ0C%YqA�UWC

P0

Pb

P

P1V

W

V = A path segment

P = Center point of the vehicle

Pb = Nearest point on the path

P0, P1 = Endpoints of the path segment

d(P,Pb)

� 1����X) � � ;�@DFBA^VX?0[�<��l<\A�UW<�<\? ABZ0<�n�<\Z/@D[\gD< � F [�<\?|AT<\R�o�CE@:?]A � VX?0Y_ABZ0<�[\gDChFT<�FBAGolCE@:?|A CE?ABZ0<Ho3VXABZ � ����7

���D� � ��� � ��1 �('5) ��)],/��)].����m� � �����"� �('5)S�{,8�(' �"�

@�������� � � � ! � � $ ��� � �&%�%7'�)+*�,0.+/21�' /�3 '+.�'�)0*�� /���)�'9.76�:��RT<\ABP/RB? =����e>� � g

� ��� � � � �� � � ��� � � �RT<\ABP/RB? =����e>� � �&%�%7'�)0*-,0.+/ 1�' /83 .(1�'�)0* 37*���� *71�'

U�Z0<\RT<� @DF�ABZ0<H[�<\?|AT<\R�olCE@:?|A-CX�{ABZ0<Hn�<\Z/@D[\gD<����e>��� @DF�ABZ0<H<\?0Y8o�CE@:?]ATF-CX�cV o3VXABZSFT<\ehd�<\?]A� � @DF�ABZ0<�o�CE@:?]A�CE?�ABZ0<�o3VXABZS[\gDChFT<�FTA�ATC ABZ0<Hn�<\Z/@D[\gD< ��� � e>� � � @DF�ABZ0<HY8@DFBA^VX?0[�<H�l<\A�Uv<�<\?+A�UWC�olCE@:?|ATF

O�CfY/<�[\@DY/<GU�Z/@D[^Z FT<\ehd�<\?|AHCX�rABZ0< o3VXABZ+ABZ0<�n�<\Z/@D[\gD< @DF�[\gDChFT<�FBA�ATC ��gDC%CEo+ABZ/RTCEP/ehZ_VXg:g�FT<\e �d�<\?|ATF�VX?0Y FT<\gD<�[\A�ABZ0<tF^<\ehd�<\?|A�[\gDChFT<�FBA�ATC+ABZ0<�n�<\Z/@D[\gD<h7 �u? CERTY/<\R�ATC FBol<�<�Y�P/o�ABZ0<qVXg �e�CERB@:ABZ/d���ABZ0<NFT<�VXRT[TZ ��<\eh@:?0FqV �#<\U FT<\ehd�<\?|ATFt�l<\Z/@:?0Y ABZ0<+g�VEFBAfFT<\gD<�[\AT<�Y FT<\ehd�<\?|A�VX?0Y[�CE?|AB@:?|P0<�F�ATCNVt�#<\U FT<\ehd�<\?|ATF @:? �#RTCE?|A CX��@:A�7 ��?¡ABZ/@DFUWV�s�uFT<�VXRT[TZ U�@:?0Y/C�U ��@DF [\RT<�VXAT<�YVXRTCEP/?0YSABZ0<Go/RT<\n]@DCEP0F�o3VXABZ+olCE@:?|A�7�O�Z0<Go/RTCE�3VX�/@:g:@:A�s�ABZ3VXA-ABZ0<G[\gDChFT<�FBA�o�CE@:?]A�CE?�ABZ0<Go3VXABZU�@:g:gc��<�?0<�VXRABZ0<�[\gDChFT<�FTA�olCE@:?|A�V�d�CEd�<\?]A VXe�C �Q@DF�n�<\RBs+Z/@:ehZQ7 �-CERBd�VXg:g:sNABZ0< ?0<\U o3VXABZo�CE@:?]AGFBZ0CEP/gDY �l< P0FTA�@:?_��RTCE?]AGCX�WABZ0<�CEgDY¡CE?0<�@��vABZ0<�n�<\Z/@D[\gD<�d�C�n�<�FH� CERBUWVXRTY VXgDCE?/e�ABZ0<o3VXABZQ7 ���vABZ0<�olChFB@:AB@DCE? eh@:n�<\? �|sNABZ0< �z � [�CE?|A^VX@:?0F�V�gDCEACX�v?0CE@DFT<��lABZ/@DFHU�@:?0Y/C(U Z3VEF�ATC��<�g�VXRBe�<\R�FB@:?0[�<HABZ0<�o/RT<\n%@DCEP0F�o3VXABZ�olCE@:?|A-d�V�sf��<�@:?0[�CERBRT<�[\A�7 �WP/A�<\n�<\?S@:?qABZ/@DF-[�VEFT<��/ABZ0<n�<\Z/@D[\gD<tFTZ0CEP/gDY�FBAB@:g:gv��<�[\gDChFT<�ATCSABZ3VXA�o�ChFT@:AB@DCE? VX?0Y�ABZ0<\RT<\�|s¡ABZ0<�?0<�VXRT<�FBA�o�CE@:?]A�CE? ABZ0<o3VXABZ�U�@:g:gQ�l<H[\gDChFT<�ATC�ABZ0<[�CERBRT<�[\A�CE?0<h7

� @:?0[�<fVSFT<\ehd�<\?|A @DF =0?/@:AT<���ABZ0<�n�<\Z/@D[\gD<�d�V�s_�l<�CEP/ATFB@DY/<�ABZ0<�o�<\RBol<\?0Y8@D[\P/g�VXR g:@:?0<�FG�#RTCEdABZ0<�<\?0Y8olCE@:?|ATF�CX��ABZ0<�FT<\ehd�<\?|A��cVEF@:g:g:P0FTABR^VXAT<�Y¡@:?¡�{@:ehP/RT<+9��87 �u?_ABZ/@DF�[�VEFT<��QABZ0<�[\gDChFT<�FBAo�CE@:?]Av@DFvCE?0<-CX��ABZ0<�<\?0Y8olCE@:?|ATF�VX?0Y�Uv<�dGP0FBA�Y/<\AT<\RBd�@:?0<�U�Z/@D[^ZqCE?0<h7c�-?0<-UWV�s�ATCGY/CGABZ/@DF��Z0C�Uv<\n�<\R�?0CEA-V n�<\RBsq<���[\@D<\?]A�CE?0<��/@DF�ATC�[�CEd�o/P/AT<�lCEABZSY8@DFBA^VX?0[�<�F�VX?0YqP0FT<HABZ0<GFBZ0CERBAT<�FBA�7�_< d P0FBA��lZ0C�UW<\n�<\R��QFBAB@:g:g�Y/<\AT<\RBd�@:?0< ABZ3VXA�ABZ0< n�<\Z/@D[\gD<�@DFHVE[\ABP3VXg:g:s+CEP/ATFT@DY/< ABZ0< FT<\ehd�<\?]A�7O�C [�CEdG�/@:?0<qABZ0<�F^<�AuUvC [�CEd�o/P/A^VXAB@DCE?0F��rUv<f[�CE?0FB@DY/<\R�ABZ0<qVX?/ehgD<�F �l<\A�Uv<�<\? ABZ0<fFT<\ehd�<\?]A� � ������ ����VX?0Y�ABZ0<�n�<�[\ATCERTF � ������ � VX?0Y � ������>� �|@ 7 <h7{�#RTCEd ABZ0<-FT<\ehd�<\?]A�<\?0Y8o�CE@:?]ATFvATC ABZ0<-n�<\Z/@D[\gD< � F[�<\?|AT<\R�olCE@:?|A��WFT<�<q�{@:ehP/RT<S�87 ����<\@:ABZ0<\R�CX��ABZ0<�FT<�VX?/ehgD<�FtVXRT< ��� � �mABZ0<�[\gDChF^<�FBA�o�CE@:?]A�CE?ABZ0<�o3VXABZ � �����Q@DFHABZ0<�[�CERBRT<�FBo�CE?0Y8@:?/e+<\?0Y8o�CE@:?]A�7 ���vABZ0<�VX?/ehgD<�@DFH?0CEA ��� � �QABZ0<�o�CE@:?]Ag:@D<�FCE? <\@:ABZ0<\RHFB@DY/<�CX�rABZ0<�<\?0Y8o�CE@:?]A��QY/<\ol<\?0Y8@:?/efCE? U�Z0<\ABZ0<\R�ABZ0<�VX?/ehgD<G@DFHVE[\P/AT<a $ ��� � g�CERCE�/ABP0FT<<a�� ��� � g>7�O�Z0<�FT<�[�CE?0Y8@:AB@DCE?0F�VXRT<�<�VEFB@:g:s AT<�FTAT<�Y_�|s�[�CEd�o/P/AB@:?/e+ABZ0<tY/CEAGo/RTC%Y8P0[\ACX�vABZ0<�n�<�[\ATCERTFH@:?|n�CEg:n�<�Y�7 O�Z0< FB@:eh? CX�vABZ/@DF�o/RTC%Y8P0[\AY/<\AT<\RBd�@:?0<�FH@��WABZ0<�Y8@DFBA^VX?0[�<�FBZ0CEP/gDY��< [�CEd�o/P/AT<�Y+ATC�CE?0< CX�mABZ0<Go�CE@:?]ATF ��� CER ��� �"CER�VEF�ABZ0< o�<\RBol<\?0Y8@D[\P/g�VXR�Y8@DFBA^VX?0[�<GATC�ABZ0<FT<\ehd�<\?|AH@:ATFT<\g��B7 �u?_VtFB@:ABP3VXAB@DCE?_FBP0[TZ¡VEF�@:? �{@:ehP/RT<q9�����ABZ0<�VXg:e�CERB@:ABZ/d U�@:g:g�?0CEA�=0?0Y_VX?|sFT<\ehd�<\?|A�ABZ3VXA�ABZ0<Hn�<\Z/@D[\gD< @DF�[\gDChF^<�FBA�ATC �"VX?0YSABZ0<\?SUW<Z3V�n�< ATC�P0FT<GABZ0<[\gDChFT<�FBA�<\?0Y8o�CE@:?]A@:?0FBAT<�VEY�7

�8� � � G��� ��� �

P1 P0

S1, S2 = Path segmentsP0, P1, P2 = Endpoints of the path segments

P2

S1

S2

P∈ P1P∈ S1

P∈ S2

P = Center point of the vehicle

P

� 1����X)�� � � ;�<\AT<\RBd�@:?3VXAB@DCE?�CX�3[\gDChFT<�FBA�olCE@:?|A{U�Z0<\?�ABZ0<rn�<\Z/@D[\gD<v@DF��l<\A�UW<�<\? AuUvC�FT<\ehd�<\?]ATF�7

� � �4�_�{�A� � � �y� �p�4©�� �{�

O�Z0<�Y/<\n�<\gDCEol<�Yf�0CEg:gDC�U ABZ0<�z�VEFBA�VXg:e�CERB@:ABZ/d @DFv@:d�o/gD<\d�<\?]AT<�Yq@:?qV FT@:dGP/g�VXATCER��0VX?0YtU�@:g:g"�l<[�CEd�o3VXRT<�Y+ATC�ABZ0< VXg:RT<�VEY8sS<�8@DFBAB@:?/e�@:d�o/gD<\d�<\?|A^VXAB@DCE?0FCX�mABZ0< �0CEg:gDC�U�ABZ0<���VXRBRTCEA ����VXR^�89�VX?0Y�zcP/RT<zcP/RTFTP/@:A � �WCEP3�h6��Qd�<\ABZ0C%Y/F�7rO�Z0<n�<\Z/@D[\gD< � F�o3VXABZ�ABR^VE[^x]@:?/e�@DF�o/RT<�FT<\?|AT<�YSehR^VXo/Z��@D[�VXg:g:sfVX?0YfU�@:ABZq?]P/d�<\RB@D[�VXg�<\RBRTCER�d�<�VEFBP/RT<�F�[�CE?0FB@DFTAB@:?/e�CX�5ABZ0<�n�<\Z/@D[\gD< � F�d�<�VX?SVX?0Yfd�V 8@ �dGP/d Y/<\n%@�VXAB@DCE?q�#RTCEd ABZ0<Ho3VXABZQ7

� Z0<\RT<f?0CEABZ/@:?/e_<\gDFT<f@DF FBA^VXAT<�Y �WV wQC%CEx�y-Z0<�VEY ;-@DFTA^VX?0[�< �tCX� 9�6Nd�<\AT<\RTF�VX?0Y � � nhVXg:P0<�87J��~�VXRT<GP0FT<�Y�7�O�Z0<�FT<GnEVXg:P0<�FHVXRT< Y/<\AT<\RBd�@:?0<�YN<�8ol<\RB@:d�<\?|A^VXg:g:sNVX?0Y+<>�l<�[\ATF�ABZ0<Gn�<\Z/@D[\gD< � FVX�/@:g:@:A�sfATC �]P/@D[Tx%g:s�RT<\ABP/RB?+ATC ABZ0<Ho3VXABZ �8� CER�<�/VXd�o/gD<HU�Z0<\?NV�n�CE@DY8@:?/e�CE�0FTA^VE[\gD<�F�7

� � �4� � W����

y-F�FBZ0C(U�?q@:?��5@:ehP/RT< 9h9��8ABZ0<�n�<\Z/@D[\gD<�@DF�VX�/gD<�ATC �#CEg:gDC(U V�o3VXABZfo�<\R � <�[\ABg:s�U�@:ABZqABZ0<��] (���� �������$�\�����vd�<\ABZ0C]YtU�@:ABZq?0C�Y/<\n%@�VXAB@DCE?f��RTCEd ABZ0<�o3VXABZQ7 �u?fABZ/@DF�AT<�FBA��8ABZ0<�n�<\Z/@D[\gD<HFBA^VXRBATF�CE?ABZ0<�o3VXABZSVX?0Y�?0C ?0CE@DF^<-@:?qABZ0<�olChFB@:AB@DCE?/@:?/e�FT<\?0FTCERTF�@DF�VEF^FBP/d�<�Y�7cy-F�V RT<>� <\RT<\?0[�<��0�{@:ehP/RT<�F9�6 VX?0Y 9�I FBZ0C�U Z0C�U ABZ0<-n�<\Z/@D[\gD<���<\Z3V�n�<�FWU�Z0<\?tP0FT@:?/e ABZ0<��] (���� ��Q�����OJL��7A79 (�rVX?0Y �8#%7N��8#+7A�A#%.2��d�<\ABZ0C]Y/F-RT<�FBol<�[\AB@:n�<\g:sqP/?0Y/<\R�ABZ0< F^VXd�<G[�CE?0Y8@:AB@DCE?0F�7��0CER�ABZ/@DF-o3VXABZ �3ABZ0<Gn�<\Z/@D[\gD<

� �#� � �"4¡�5,/� � .��"� ¤m)�� $ )|)%� �('�)S�^$ ��� ��� )]��)%����4_)|�('����{. ���¡����4_���Q��) K k �8�

Z3VEF-ATC�P0FT< d�V %@:d P/d FTAT<�<\RB@:?/efVX?/ehgD< @:?NCERTY/<\R�ATC��#CEg:gDC(U @:A�7-O�Z/@DF-d�VXx�<�F�@:A�Y8@���[\P/g:A-� CER��CEABZQ�] ����� ��I����� JL��7�7 1�GVX?0Y ��#+7N�O�8#%7A�A#%.��GATC�� CEg:gDC�U �rVEF ABZ0<\s_AT<\?0Y�ATC u[\P/A�[�CERB?0<\RTF��@:?0FBAT<�VEY�CX�{�#CEg:gDC(U�@:?/e�ABZ0<Ho3VXABZ+VXgDCE?/e�V�n�<\RBsf[\P/RBn�<�Yqo3VXABZQ7"D-?0Y/<\R�@DY/<�VXg�[�CE?0Y8@:AB@DCE?0F �/ABZ/@DFFBZ0CERBAT[�CEd�@:?/e�@DF�F^CEg:n�<�Yq�|stP0FB@:?/e�ABZ0<H�0CEg:gDC(U ABZ0<z�VEFBA-VXg:e�CERB@:ABZ/d�7

−60 −50 −40 −30 −20 −10 0−10

0

10

20

30

40

50

[m]

[m]

� 1����X)+�3� � O�Z0<�n�<\Z/@D[\gD<�� CEg:gDC�U�FWABZ0<�o3VXABZtol<\R �#<�[\ABg:s U�@:ABZtABZ0<��0CEg:gDC(U O�Z0<�z�VEFBArd�<\ABZ0C%Y

F ` /IEJ : K ;=< M�I X�� P_R � P P X RTH.P(R �0I Z%<�� P ; P X RtJLP_RTH�I Z M0R I�^ IEJ : b.R\P � k

��? � <�[\AB@DCE?0Fc687 j07:9WVX?0Y 687 j07J6��XA�UWC�d�<\ABZ0C%Y/F�ATC�[�CEd�o/P/AT<rABZ0<r��<\Z3V�n]@DCEP/RmK k ��RT<�FBolCE?0FB@:�/gD<c� CERd�C�n%@:?/e�ABZ0< n�<\Z/@D[\gD<GATC(UWVXRTY/F�ABZ0< o3VXABZ �3UW<\RT<�@:?|ABRTC%Y8P0[�<�Y�7 ��CEABZSd�<\ABZ0C%Y/FHVXRT<�3VEFT<�YNCE?ABZ0<\@:RrRT<�FBolCE?0FT<�ATCHABZ0<�Y8@DFBA^VX?0[�<��l<\A�UW<�<\?�ABZ0<�n�<\Z/@D[\gD<�VX?0Y�ABZ0<�?0<�VXRT<�FTArolCE@:?|AvCE?�ABZ0<�o3VXABZQ7y-FWFBZ0C�U�?f@:?f�{@:ehP/RT< 9\j �%ABZ0<�Y8@��l<\RT<\?0[�<��l<\A�Uv<�<\?qABZ0<-A�UWC�K k d�<\ABZ0C%Y/Fv@DFv?0CEAWn�<\RBs�g�VXRBe�<h7� @:ABZ_ABZ0<�[\P/RBRT<\?|A FT<\ABAB@:?/e�F a � � � � ��� e ����� � g��5bN<\ABZ0C%Y_CE?0<�ABP/RB?0F FBg:@:ehZ|ABg:s_FBZ3VXRBol<\RATC�UWVXRTY/FWABZ0<�o3VXABZ�U�Z/@:gD<�b+<\ABZ0C%Y�A�UWCGZ3VEF�V�d�CERT<�FTCX�#AvVXo/o/RTC�VE[TZ �%�/P/ArABZ0<-Y8@��l<\RT<\?0[�<�[�VX?��<-<\g:@:d�@:?3VXAT<�Y�U�@:ABZqY8@���<\RT<\?|A�FT<\ABAB@:?/e�Fv�#CERvABZ0< � � nhVXg:P0<�VX?0Y�ABZ0<�wQC%CEx�y�Z0<�VEYt;�@DFBA^VX?0[�<��h7O�Z0<�<>��<�[\A�CX�vVXo/o/g:s]@:?/e�V�FBd�VXg:gD<\R �SCERHV�g�VXRBe�<\R �RT<�FBol<�[\AB@:n�<\g:s �l@DF�ABZ3VXA-ABZ0<�n�<\Z/@D[\gD<�U�@:g:gABP/RB?_FBgDC�UW<\RATC�U�VXRTY/FABZ0< o3VXABZ �5VEFH[�VX? �l< FT<�<�@:?_�{@:ehP/RT<q9�`87�y�F�U�@:g:gc��<�FTZ0C�U�? @:? ABZ0<?0<�%AGF^<�[\AB@DCE? � � � � � � � VX?0Y � � ��� eh@:n�<�FG��<\ABAT<\Rol<\R � CERBd�VX?0[�<�U�@:ABZ Vt?0CE@DFBs olChFB@:AB@DCE?FB@:eh?3VXg �/�/P/A�ABZ0<\sqVXRT<[\gD<�VXRBg:sf?0CEA�CEo/AB@:d�VXgQ� CER�V �|P/@D[^xfRT<\ABP/RB?�ATC ABZ0<Ho3VXABZQ7

� �#� �#� � � � .()

��?�RT<�VXgvg:@�� <��{o�ChFB@:AB@DCE? @:?8� CERBd�VXAB@DCE? @DF�FT<\gDY/CEd o�<\R � <�[\A�7Sy Hz � FT@:eh?3VXgv@DF��5� CER <�0VXd�o/gD<��FBP/�� <�[\A ATC+��CEABZ FBs8FBAT<\d�VXAB@D[qVX?0Y o/P/RT<\g:s_R^VX?0Y/CEd Y8@DFBABP/RB�3VX?0[�<�F�7 O�Z0<\RT<>� CERT<��mABZ0<fVXg:e�C��RB@:ABZ/d � F ��<\Z3V�n]@DCEP/R U�Z0<\? ABZ0<�olChFB@:AB@DCE? @:?8� CERBd�VXAB@DCE? @DF�?0CEA [�CEd�o/gD<\AT<\g:s�RT<\g:@�VX�/gD<tZ3VEF�ATC��<�AT<�FBAT<�Y�7 �u?�CERTY/<\R�ATCNY/C�ABZ/@DF���ABZ0<�o�<\R � CERBd�VX?0[�<tCX��ABZ0<�n�<\Z/@D[\gD<�@DF CE�0FT<\RBn�<�Y¡U�Z0<\? V

�]� � � G��� ��� �

−60 −50 −40 −30 −20 −10 0−10

0

10

20

30

40

50

[m]

[m]

� 1����X)��8� � � @:ABZABZ0<v�0CEg:gDC�U ABZ0<v��VXRBRTCEAbN<\ABZ0C%Y ��ABZ0< n�<\Z/@D[\gD< Y8@:n�<\RBe�<�FSVXAfd�ChFBAI87J6�d�<\AT<\RTF��#RTCEd ABZ0<Ho3VXABZQ7WO�Z0<GV�n�<\R^VXe�<Y8@DFTA^VX?0[�<<\RBRTCER�@DF��87J�h� d�<\AT<\R�7

−60 −50 −40 −30 −20 −10 0−10

0

10

20

30

40

50

[m][m

]

� 1"�5�X) �%� � � @:ABZ ABZ0< zcP/RT< zcP/RTFBP/@:Ad�<\ABZ0C%Y�ABZ0<vn�<\Z/@D[\gD<�Y8@:n�<\RBe�<�FmVXA�d�ChFBAm687 ~d�<\AT<\RTFm��RTCEd ABZ0<�o3VXABZQ7cO�Z0<�V�n�<\R^VXe�<�Y8@DF��A^VX?0[�<<\RBRTCER�@DF��87 ~Ep d�<\AT<\R�7

−70 −60 −50 −40 −30 −20 −10 0 10

20

25

30

35

40

45

50

55

60

65

70

[m]

[m]

������������ ��� �������������� ���

−70 −60 −50 −40 −30 −20 −10 0 10

20

25

30

35

40

45

50

55

60

65

70

[m]

[m]

��� �!���"��� ���$#% &�('��*),+.-

� 1����X) �%� � b+<\ABZ0C%YqCE?0<HABP/RB?0F-FBg:@:ehZ|ABg:sqFBZ3VXRBol<\R�ABZ3VX?SbN<\ABZ0C]YfAuUvC�ATC(UWVXRTY/F-ABZ0<Ho3VXABZQ7

� �#� � �"4¡�5,/� � .��"� ¤m)�� $ )|)%� �('�)S�^$ ��� ��� )]��)%����4_)|�('����{. ���¡����4_���Q��) K k �/�

−70 −60 −50 −40 −30 −20 −10 0 10

20

25

30

35

40

45

50

55

60

65

70

[m]

[m]

������������ ��� �������������� ���

−70 −60 −50 −40 −30 −20 −10 0 10

20

25

30

35

40

45

50

55

60

65

70

[m]

[m]

��� �!���"��� ���$#% &�('�������-

� 1����X)��/��� O�Z0<�n�<\Z/@D[\gD<�ABP/RB?0FvFTgDC�Uv<\RWATC�U�VXRTY/FWABZ0<�o3VXABZ�U�@:ABZtFBd�VXg:gD<\R � � nhVXg:P0<�CERvg�VXRBe�<\RwQC]CExfy-Z0<�VEY�;�@DFBA^VX?0[�< ��RT<�FBo�<�[\AB@:n�<\g:s�7

?0CERBd�VXg:g:sSY8@DFBABRB@:�/P/AT<�Y+?0CE@DFT<������4�Le�� ��@DF�VEY/Y/<�YSATC�ABZ0<olChFB@:AB@DCE?+Y0VXA^V87��0P/RBABZ0<\RBd�CERT<��FB@:?0[�<�ABZ0<�?0CE@DFT<��#RTCEd V �z � Y/C]<�FG?0CEAG� CEg:gDC�U Vq?0CERBd�VXg�Y8@DFBABRB@:�/P/AB@DCE? U�@:ABZ � <\RTCSd�<�VX?nEVXg:P0<���VNFB@:?0<t��P/?0[\AB@DCE? @DF�VEY/Y/<�Y ATC ABZ0<fd�<�VX? nEVXg:P0<qCX��ABZ0<f?0CE@DFT<��mABZ]P0F�FB@:d P/g�VXAB@:?/e�Vo�<\RB@DC%Y8@D[ Y8RB@��#A�@:?SABZ0< �z � FB@:eh?3VXg 7-O�Z/@DF�d�<�VX?0F�ABZ3VXA�ABZ0<GolChFB@:AB@DCE? ��c e9f0��U�@:g:g�[�CE?|A^VX@:?_V?0CE@DFT<�[�CEd�o�CE?0<\?]AU�@:ABZ¡VX?_VXd�o/g:@:ABP0Y/<�CX��VXAHd�ChFTA � VX?0Y_VtFBA^VX?0Y0VXRTY Y/<\n%@�VXAB@DCE?��c7 �0CERRT<�VEFTCE?0F�CX���/RB@D<>��?0<�F^F��/?0CE@DFT<H[�CEd�olCE?0<\?|ATF-U�@:g:gQ@:?�ABZ/@DF�RT<\olCERBA��l<Y/<�FT[\RB@:�l<�Yq�|sfFBol<�[\@���s%@:?/eABZ0<�� CE?/g:s�7mO�Z0<�� CEg:gDC�U�@:?/et< �|P3VXAB@DCE?0F�VXRT<P0FT<�Y�M

���>� ��������� �����

� ��� a�j07:9 g

c d ��>� � c���>� � ��� ���� a�j07J61gf d ��>� � f ���� � ���<��>� a�j07JI1g

U�Z0<\RT<��� ����Ae����+��>��� �4�Le�� ������>M ABZ0<Hd�<�VX?�nhVXg:P0<CX��ABZ0<�?0CERBd�VXg:g:sqY8@DFBABRB@:�/P/AT<�Y�?0CE@DFT<VXA�AB@:d�<HA�>M AB@:d�<FB@:?0[�<GFBA^VXRBA� M ABZ0<Hol<\RB@DC]Y8@D[\@:AusqCX�{ABZ0<H?0CE@DF^<�a�nEVXg:P0<HP0F^<�Y�Mm6h�EFAg�cM ABZ0<FBA^VX?0Y0VXRTYSY/<\n]@�VXAB@DCE?SCX��ABZ0<H?0CE@DFT<c d ����Ae9f d ����>M ABZ0<H�l<\g:@D<\n�<�YqolChFB@:AB@DCE?SCX��ABZ0<�n�<\Z/@D[\gD< VXA�AB@:d�<HAc���>�Ae9f ��>�#� ABZ0<GVE[\ABP3VXgQolChFB@:AB@DCE?SCX��ABZ0<�n�<\Z/@D[\gD< VXA�AB@:d�<A

y-F[�VX?¡��<�F^<�<\? @:?¡�{@:ehP/RT<S9�p��QU�Z0<\? VEY/Y8@:?/eS?0CE@DFT<�U�@:ABZ�FBA^VX?0Y0VXRTY¡Y/<\n%@�VXAB@DCE?�� � ��� �ABZ0< �] ����� �� �����b��������d�<\ABZ0C]Y¡FBAB@:g:gmol<\R �#CERBd�FG��<\ABAT<\RABZ3VX?¡��CEABZ_ABZ0<��0CEg:gDC�U ABZ0<���VXRBRTCEAVX?0Y�zcP/RT<tzcP/RTFBP/@:A�d�<\ABZ0C%Y/F�Y/CSU�@:ABZ0CEP/A ?0CE@DFT<h7��0CER [�CEd�o3VXRB@DFTCE? ��ABZ0<�o�<\R � CERBd�VX?0[�<�CX��0CEg:gDC�U ABZ0<���VXRBRTCEA�VX?0Y zcP/RT<�zmP/RTFBP/@:AU�@:ABZ0CEP/A�VX?]s+?0CE@DF^<�@DFHFBZ0C�U�?¡@:?¡�{@:ehP/RT<�F�9�6�VX?0Y9�I87�O�Z0<\RT<G@DF-?0C�FB@:eh?/@�=3[�VX?]A�Y8@���<\RT<\?0[�< ��<\AuUv<�<\? ABZ0<AuUvCqK k n�<\RTFB@DCE?0F-CX�mABZ0<b�] ����� ��k������������Wd�<\ABZ0C%YSVXA�ABZ/@DF�?0CE@DF^<�gD<\n�<\g 7

�%� � � G��� ��� �

−60 −50 −40 −30 −20 −10 0 10−10

0

10

20

30

40

50

[m]

[m]

�����������& ��� ��&�

−60 −50 −40 −30 −20 −10 0 10−10

0

10

20

30

40

50

[m]

[m]

�����!������ �� #%

� 1����X)t�%� � ��CEABZ�d�<\ABZ0C]Y/F�UWCERBx%F�UW<\g:g/U�@:ABZ�FBd�VXg:g8?0CE@DFT<h75O�Z0<W?0CE@DFT<�FBA^VX?0Y0VXRTY Y/<\n%@�VXAB@DCE?� @DF�Z0<\RT<CE?0<Hd�<\AT<\R�7mO�Z0<d�V %@:d P/d Y/<\n%@�VXAB@DCE?q��RTCEd ABZ0<�o3VXABZ�@DF�VX�lCEP/A-687J`�d�<\AT<\RTF�7

� Z0<\?+?0CE@DFT<U�@:ABZNFBA^VX?0Y0VXRTY+Y/<\n%@�VXAB@DCE?�� ��� � @DF�VEY/Y/<�Y �3ABZ0<Gn�<\Z/@D[\gD< Y/<\n%@�VXAT<�F�VXA-d�ChFBA P0FBA�C(n�<\R�� CEP/R�d�<\AT<\RTF��#RTCEd ABZ0<Ho3VXABZ �/FT<�<H�{@:ehP/RT<�9(~ �/U�Z/@D[^Z�@DF P0FBA�FBg:@:ehZ|ABg:stUvCERTF^<HABZ3VX?ABZ0<��3CEg:gDC�U ABZ0<���VXRBRTCEAHd�<\ABZ0C%Y+UWCERBx]@:?/eqU�@:ABZ0CEP/AH?0CE@DF^<h7O�Z0<�o�<\R � CERBd�VX?0[�<�CX�rABZ0< A�UWCK k d�<\ABZ0C%Y/F��/ABZ0CEP/ehZSo�C%CER��/FBAB@:g:gQY/C�?0CEA�Y8@���<\R�dGP0[^Z��#RTCEd <�VE[TZ+CEABZ0<\R�7

−60 −50 −40 −30 −20 −10 0 10−10

0

10

20

30

40

50

[m]

[m]

�����������& ��� ��&�

−60 −50 −40 −30 −20 −10 0 10−10

0

10

20

30

40

50

[m]

[m]

�����!������ �� #%

� 1����X) � ��� � @:ABZ FBA^VX?0Y0VXRTY Y/<\n%@�VXAB@DCE? � ��� � ��ABZ0< ol<\R �#CERBd�VX?0[�<¡Y/<\ehR^VEY/<�Fq�/P/Aq@DFFB@:d�@:g�VXR��#CERHABZ0< AuUvCSd�<\ABZ0C]Y/F�7 O�Z0< d�V 8@:dGP/d Y/<\n]@�VXAB@DCE? �#RTCEd&ABZ0< o3VXABZ @DF� P0FBAVX��C(n�<�#CEP/R�d�<\AT<\RTF�7

� @:ABZ�FBA^VX?0Y0VXRTY�Y/<\n]@�VXAB@DCE? � � � � ��bN<\ABZ0C]Y�CE?0<�FBA^VXRBATFmATCHo�<\R � CERBd ?0CEA^VX�/g:s�o�C%CERT<\RcABZ3VX?b+<\ABZ0C%Y A�UWC ��FT<�< �{@:ehP/RT<q9�����U�Z/@D[TZ UWCEP/gDY_FBP/ehe�<�FBAHABZ3VXAH@:A�@DF�?0CEAGVEF�ATCEgD<\R^VX?]AATCf?0CE@DFT<VEF�bN<\ABZ0C]Y A�UvC/7 O�Z/@DF�@DF�[�CE?8=0RBd�<�Y U�Z0<\? VEY/Y8@:?/e V_?0CE@DFT<qU�@:ABZ FBA^VX?0Y0VXRTY Y/<\n%@�VXAB@DCE?

� �#� � �"4¡�5,/� � .��"� ¤m)�� $ )|)%� �('�)S�^$ ��� ��� )]��)%����4_)|�('����{. ���¡����4_���Q��) K k � �

� � � � �_7-b+<\ABZ0C%YNCE?0<G�/RB@:?/e�F-ABZ0<Gn�<\Z/@D[\gD< [�CEd�o/gD<\AT<\g:sSCX� [�CEP/RTFT<��"F^<�<G�{@:ehP/RT<�9��87�O�Z0<[�VXP0FT<�CX�lABZ/@DFr<\RBRTCERm@DFmY8@DF^[\P0FTFT<�Y �#P/RBABZ0<\Rm@:? � <�[\AB@DCE?�j07J687:9h7c�{@:ehP/RT<�6h�FBZ0C�U�FrABZ3VXArb+<\ABZ0C%YA�UvC�Z3VEF�?0C�o/RTCE�/gD<\d�F�U�Z0<\?SY/<�VXg:@:?/e�U�@:ABZ�Z/@:ehZ�?0CE@DF^<HgD<\n�<\gDF�7

��?�V�� CERT<�FBA�<\?|n%@:RTCE?/d�<\?|AcU�@:ABZ�gDCEATF{CX�0ABRT<�<�FcVX?0Y CEABZ0<\R�CE�0FBA^VE[\gD<�F{n�<\RBsG[\gDChFT<vATC�ABZ0<vn�<\Z/@D[\gD<��V Hz � Y/<\g:@:n�<\RB@:?/e¡VSolChFB@:AB@DCE? U�@:ABZ VX? <\RBRTCER CX��FT<\n�<\R^VXg�d�<\AT<\RTF�[�VX? ?0CEA��l<tATCEgD<\R^VXAT<�Y�7O�Z0<�FT@:ABP3VXAB@DCE? VX�lC�n�< @DFHV�ABZ0<\RT<>� CERT<��/@:AH<�%ABRT<\d�<���ABZ0<�d�V %@:d P/d o�ChFB@:AB@DCE? <\RBRTCERHY8P/RB@:?/eFB@:dGP/g�VXAB@DCE?�@DFcVX�lCEP/A�9�Ih�Xd�7 � AB@:g:g �hABZ0<Wo3VXABZ�ABR^VE[Tx%@:?/eGVXg:e�CERB@:ABZ/d FBZ0CEP/gDY ��<�VX�/gD<�ATC�Z3VX?0Y8gD<ABZ/@DF�x]@:?0YSCX�{?0CE@DFT<�U�@:ABZ0CEP/A-��<�[�CEd�@:?/e�P/?0FBA^VX�/gD<h7

−60 −50 −40 −30 −20 −10 0 10−10

0

10

20

30

40

50

[m]

[m]

�����������& ��� ��&�

−60 −50 −40 −30 −20 −10 0 10−10

0

10

20

30

40

50

[m]

[m]

�����!������ �� #%

� 1����X)�� � � � @:ABZ FTA^VX?0Y0VXRTY Y/<\n%@�VXAB@DCE? � � � � ��bN<\ABZ0C]Y CE?0<�ol<\R �#CERBd�F�UvCERTFT<SABZ3VX?b+<\ABZ0C%YSAuUvC/7O�Z0< d�V NVX?0YNd�<�VX? Y8@DFBA^VX?0[�< ATC�o3VXABZ+� CER�bN<\ABZ0C]YNCE?0<�@:?+ABZ/@DF�<�0VXd�o/gD<VXRT<�9�I�VX?0Y�p87JI�d�<\AT<\RTF�7{�0CER�b+<\ABZ0C%YAuUvC �EABZ0<rnEVXg:P0<�F{VXRT<W�87Jp-VX?0Yj07J6�d�<\AT<\RTF5RT<�FBol<�[\AB@:n�<\g:s�7

O�Z0<�o�<\R � CERBd�VX?0[�<�CX�lABZ0<�A�UvCd�<\ABZ0C]Y/Fc@DFmZ/@:ehZ/g:s�Y/<\ol<\?0Y/<\?|ArCX�lABZ0<�nhVXg:P0<�CX��ABZ0<�[�CE?0FBA^VX?]A��VX?0Y�wQC%CEx�y�Z0<�VEY�;�@DFBA^VX?0[�< �WRT<�FBol<�[\AB@:n�<\g:s�7 �u?�ABZ0<�RT<�FBP/g:ATFco/RT<�F^<\?|AT<�Y�VX�lC�n�<��]ABZ0<�nhVXg:P0<�F�87J��~+VX?0Y 9�6Xd VXRT<�P0F^<�Y�7N;�@���<\RT<\?|A nhVXg:P0<�FG� CER ��VX?0Y �tVXA Y8@��l<\RT<\?]A ?0CE@DFT<tgD<\n�<\gDF Z3V�n�<��<�<\? AT<�FBAT<�Y ��ATC¡F^<�<�Z0C�U�ABZ0< VXg:e�CERB@:ABZ/d�F�RT<�VE[\A�7 �5@:ehP/RT< 689SFTZ0C�U�FtABZ0<��l<\Z3V�n%@DCEP/RtCX�ABZ0<HA�UWC�VXg:e�CERB@:ABZ/d�F-U�@:ABZ+Y8@���<\RT<\?|A-F^<\ABAB@:?/e�F�� CER�ABZ0<x � nEVXg:P0<VX?0Y+wQC]CExfy-Z0<�VEY�;�@DFBA^VX?0[�<RT<�FBo�<�[\AB@:n�<\g:s ��VX?0Y VXA�Y8@���<\RT<\?|A�?0CE@DFT<fgD<\n�<\gDF(7�O{VX�/gD<_9qFBZ0C�U�F�ABZ0<fgDC�Uv<�FTA�nEVXg:P0<�F�CX��ABZ0<d�V fVX?0Ytd�<�VX?qY8@DFBA^VX?0[�<�ATC ABZ0<�o3VXABZ�VXA�Y8@��l<\RT<\?]A�?0CE@DFT<�gD<\n�<\gDF�7cO�Z0<�AT<�FBATFWUv<\RT<HY/CE?0<�CE?ABZ0<Go3VXABZ @:?+�{@:ehP/RT<�6hIRa V1g>7��-CEAT< ABZ3VXA�ABZ0< Y8@���<\RT<\?0[�<G@:?Nol<\R �#CERBd�VX?0[�<��l<\A�Uv<�<\? ABZ0<�A�UWCd�<\ABZ0C]Y/F�VXRT<�?0CEAn�<\RBs g�VXRBe�<�U�Z0<\?�ABP/?0<�Y¡[�CERBRT<�[\ABg:s�7fy-?0CEABZ0<\R@:?|AT<\RT<�FBAB@:?/e CE�0FT<\RBnhVXAB@DCE?@DF�ABZ3VXAtABZ0<+Z/@:ehZ0<\RtABZ0<+?0CE@DFT<���ABZ0<NFBd�VXg:gD<\RfnhVXg:P0<�� CER � eh@:n�<�FtABZ0<+�l<�FBA�o�<\R � CERBd�VX?0[�<h7O�Z/@DFHeh@:n�<�FHRB@DFT<�ATC�Vt[�CE?��0@D[\A�l<\A�Uv<�<\?¡��<\Z3V�n]@DCEP/RTF�7 ��CERBd�VXg:g:s �QU�Z0<\?¡Vtn�<\Z/@D[\gD<�e�<\ATFHCX�[�CEP/RTFT<��v�#CER�<�0VXd�o/gD<qU�Z0<\? V�n�CE@DY8@:?/e VX? CE�0FBA^VE[\gD<��WUW<qUWVX?]A�ATC e�<\A��3VE[^x ATC ABZ0<fo3VXABZVEF�� VEFBAtVEF�o�ChF^FB@:�/gD<h7 O�Z/@DF�@DF�Y/CE?0<+�|s VXo/o/g:s%@:?/e V_g�VXRBe�<Sx � nhVXg:P0<h7 �WP/A�@��Uv<+Z3V�n�< Vg�VXRBe�< x � nhVXg:P0<��lUv<�e�<\A�o/RTCE�/gD<\d�FHU�Z0<\? ABZ0<�o�ChFT@:AB@DCE?N@DF�?0CE@DFBs�7 O�Z0<�o/RTCE�/gD<\d&@:g:g:P0FBABR^VXAT<�Y@:?¡�{@:ehP/RT<S9����QU�Z0<\?�ABZ0<�n�<\Z/@D[\gD<�e�<\ATF [�CEd�o/gD<\AT<\g:s_CX� [�CEP/RTFT<���[�VX? C][�[\P/RG<\n�<\? VXAgDC�UW<\R

� � � � G��� ��� �

−40 −20 0 20 40 6010

20

30

40

50

60

70

80

90

100

110

[m]

[m]

� 1����X)N�<� � bN<\ABZ0C]YtCE?0<-Y/C%<�FW?0CEAvUWCERBxUW<\g:g�U�@:ABZ+g�VXRBe�< ?0CE@DFT<h7���CE@DFT<GU�@:ABZNFBA^VX?��Y0VXRTY Y/<\n%@�VXAB@DCE? � � � � � @DF VXo/o/g:@D<�Y @:?ABZ/@DF�<�/VXd�o/gD<h7

−60 −40 −20 030

40

50

60

70

80

90

100

110

120

130

[m]

[m]

� 1"�5�X)S� � � bN<\ABZ0C]YtAuUvC�Y/C%<�FW?0CEA�Z3V�n�<ABZ0<�F^VXd�<�Y8@���[\P/g:AB@D<�F�Z3VX?0Y8g:@:?/e�?0CE@DFTs�o�C��FB@:AB@DCE?�FB@:eh?3VXgDFmVEFcbN<\ABZ0C%Y�CE?0<h7{O�Z0<�FBA^VX?��Y0VXRTY�Y/<\n%@�VXAB@DCE?�@DFcABZ0<�F^VXd�<�VEFm@:?�ABZ0<�gD<>��A=0ehP/RT<��3Ih`Xd�7

?0CE@DFT<�gD<\n�<\gDF�U�Z0<\? VXo/o/g:s%@:?/efZ/@:ehZ x � nhVXg:P0<�F�7HO�Z/@DF�d�<�VX?0F�ABZ3VXA�ABZ0<\RT< @DFHV�o/RTCE�/gD<\d U�@:ABZABP/?/@:?/e+bN<\ABZ0C]Y�CE?0<�� CERGCEo/AB@:d�VXgvol<\R � CERBd�VX?0[�<��5U�@:ABZ�RT<\e|VXRTY_ATCS��CEABZ_?0CE@DF^<�FT<\?0FB@:AB@:n%@:A�sVX?0YNVX�/@:g:@:AusSATC�e�<\A��3VE[^x�ATC�ABZ0<Go3VXABZ �]P/@D[Tx%g:s+V���AT<\RHV�n�CE@DY8@:?/e�VX? CE�0FBA^VE[\gD<h7��0CERHb+<\ABZ0C%YA�UvC ��V [�CERBRT<�FBolCE?0Y8@:?/e�o/RTCE�/gD<\d <�8@DFBATF�7 ���5Uv<@:?0[\RT<�VEFT<HABZ0<wQC%CExfy�Z0<�VEYq;�@DFBA^VX?0[�< ���8ABZ0<VXg:e�CERB@:ABZ/d @DF�gD<�FTF�F^<\?0FB@:AB@:n�<HATC ?0CE@DFT<h7 �WP/A�VEF�UW<[�VX?�FT<�<�@:?S�{@:ehP/RT<�9�`�ABZ0<n�<\Z/@D[\gD<HABP/RB?0Fn�<\RBsNFBgDC(U�g:s+�3VE[^x ATC�UWVXRTY/FGABZ0< o3VXABZ � CER�<�0VXd�o/gD<�U�Z0<\?¡V�n�CE@DY8@:?/e+CE�0FBA^VE[\gD<�F�7bB�C�UW<\n�<\R��VEF�CEo/o�ChF^<�YqATC�b+<\ABZ0C%Y�CE?0<��0bN<\ABZ0C%YfA�UvC�Y/C]<�F�?0CEA���<�[�CEd�<P/?0FBA^VX�/gD<HU�Z0<\?+VXo/o/g:s%@:?/e�VZ/@:ehZ a#CER�gDC�U$g�wQC]CExfy-Z0<�VEY�;�@DFBA^VX?0[�<h7

�uFrABZ0<�ol<\R �#CERBd�VX?0[�<��#CERvABZ0<�A�UWC�VXg:e�CERB@:ABZ/d�F�FB@:d�@:g�VXRr�#CERvY8@��l<\RT<\?]Avo3VXABZ0F���O�C�VX?0FBUv<\RWABZ/@DF�|P0<�FBAB@DCE? �lABZ0<�VXg:e�CERB@:ABZ/d�FGVXRT<�VXo/o/g:@D<�Y CE? ABZ/RT<�< Y8@��l<\RT<\?]AHo3VXABZ0F�a#FBZ0C�U�? @:? �{@:ehP/RT<�6hI1g>7O�Z0<�RT<�FBP/g:A��8FBZ0C�U�?q@:?q�{@:ehP/RT<6h6��%@:?0Y8@D[�VXAT<�F�ABZ3VXA�Uv<�FBZ0CEP/gDYf�l<�VX�/gD<�ATC =0?0YtnhVXg:P0<�FW�#CER �VX?0Y �-ABZ3VXA�UvCERBxtUv<\g:gQCE?SV nhVXRB@D<\A�sfCX�5o3VXABZ0F�7rO5VX�/gD<6 FBZ0C(U�F�ABZ0<HFT<\ABAB@:?/e�F�� CERWU�Z/@D[^Z�ABZ0<A�UvCfd�<\ABZ0C%Y/F�ol<\R �#CERBd�<�YN�l<�FBA-U�@:ABZ ABZ0< Y8@���<\RT<\?|AHo3VXABZ0F�7H�mn�<\? @��rABZ0< CEo/AB@:d�VXgmFT<\ABAB@:?/e�Fd�V�sqY8@��l<\R��l<\A�UW<�<\?�o3VXABZ0F��/ABZ0<�e�<\?0<\R^VXg5[�CE?0[\g:P0FB@DCE?�@DF�ABZ3VXA�U�@:ABZqg�VXRBe�<\R�?0CE@DF^<��3V�FBd�VXg:gD<\Rx � nhVXg:P0<�VX?0Y�V�gDCE?/e�<\R wQC%CEx y�Z0<�VEY�;�@DFBA^VX?0[�<�eh@:n�<�FGABZ0<��l<�FBAol<\R �#CERBd�VX?0[�<h7f��CEAT<�ABZ3VXAABZ0<GVX�/@:g:@:A�sfATC �]P/@D[Tx%g:s�RT<\ABP/RB?SATC�ABZ0<�o3VXABZ�@DF�?0CEA�RT<\e|VXRTY/<�YS@:?qABZ/@DF�AT<�FBA�7

� �#� � �"4¡�5,/� � .��"� ¤m)�� $ )|)%� �('�)S�^$ ��� ��� )]��)%����4_)|�('����{. ���¡����4_���Q��) K k �+�

0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.50

0.5

1

1.5

2

2.5

3

3.5

Dis

tanc

e fr

om p

ath

[m]

k

maxmean

������������ ��� ���� #�� ���� ���(� ��-��

0 10 20 30 40 50 60 70 80 90 1000

0.5

1

1.5

2

2.5

3

3.5

Dis

tanc

e fr

om p

ath

[m]

Look Ahead Distance [m]

maxmean

��� �!���"��� �� $#% #�� ���� ���(� ��-��

0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.51

2

3

4

5

6

7

8

9

Dis

tanc

e fr

om p

ath

[m]

k

maxmean

��� �������& ��� ��&� #�� ���� ����-��

0 10 20 30 40 50 60 70 80 90 1000

1

2

3

4

5

6

7

8

9

Dis

tanc

e fr

om p

ath

[m]

Look Ahead Distance [m]

maxmean

�����!������ ���$#% #�� ���� ����-��

0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.51

2

3

4

5

6

7

8

9

10

11

Dis

tanc

e fr

om p

ath

[m]

k

maxmean

��� �������& ��� ��&� #�� ���� �.-��

0 10 20 30 40 50 60 70 80 90 1001

2

3

4

5

6

7

8

9

10

11

Dis

tanc

e fr

om p

ath

[m]

Look Ahead Distance [m]

maxmean

��� �������� �� #% #�� �� � �.-��

� 1����X) ��� � bNV VX?0Y d�<�VX? Y8@DFTA^VX?0[�<��#RTCEd ABZ0<to3VXABZ U�@:ABZ Y8@���<\RT<\?|A�F^<\ABAB@:?/e�F��#CER ABZ0<� � nEVXg:P0<qVX?0Y wQC%CEx¡y�Z0<�VEY ;�@DFBA^VX?0[�<�� � �mVX?0Y U�@:ABZ Y8@��l<\RT<\?]A�?0CE@DFT<tFT<\ABAB@:?/e�F�7 O�Z0<to3VXABZFBZ0C�U�?�@:?+�5@:ehP/RT< 6hIRa V1gWUWVEF�P0FT<�Yf@:?qABZ/@DF�AT<�FBA�7

� � � � G��� ��� �

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.80.5

1

1.5

2

2.5

3

3.5

Dis

tanc

e fr

om p

ath

[m]

k

maxmean

�����!������ ��� ���� � ��� � ��� ��&�

0 10 20 30 40 50 60 70 80 90 1000.5

1

1.5

2

2.5

3

3.5

Dis

tanc

e fr

om p

ath

[m]

Look Ahead Distance [m]

maxmean

�����!������ ���$#% � ��� � �� ��&�

0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.50

2

4

6

8

10

12

Dis

tanc

e fr

om p

ath

[m]

k

maxmean

���,�!������ ��� ���� � ��� � ��� #

0 10 20 30 40 50 60 70 80 90 1000

2

4

6

8

10

12D

ista

nce

from

pat

h [m

]

Look Ahead Distance [m]

maxmean

�����!�����& ��� # � ��� �.�� #%

0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.50.5

1

1.5

2

2.5

3

3.5

4

4.5

5

5.5

Dis

tanc

e fr

om p

ath

[m]

k

maxmean

��� �������� �� ���� � ��� � ����� � ��

0 10 20 30 40 50 60 70 80 90 1000.5

1

1.5

2

2.5

3

3.5

4

4.5

5

5.5

Dis

tanc

e fr

om p

ath

[m]

Look Ahead Distance [m]

maxmean

����� ���"��� �� $#% � ��� � ����� � ��

� 1����X) �"��� bNV VX?0Y d�<�VX? Y8@DFTA^VX?0[�<��#RTCEd ABZ0<to3VXABZ U�@:ABZ Y8@���<\RT<\?|A�F^<\ABAB@:?/e�F��#CER ABZ0<� � nEVXg:P0<tVX?0Y_wQC%CEx y-Z0<�VEY_;�@DFBA^VX?0[�< ��� CERY8@���<\RT<\?|A�o3VXABZ0F VX?0Y U�@:ABZ � � ���_7�y-g:gmABZ0<o3VXABZ0F�FBZ0C(U�?�@:?��{@:ehP/RT<G6hI Uv<\RT<P0FT<�Yq@:?qABZ/@DF�AT<�FTA�7

� �#� � �"4¡�5,/� � .��"� ¤m)�� $ )|)%� �('�)S�^$ ��� ��� )]��)%����4_)|�('����{. ���¡����4_���Q��) K k ���

−60 −50 −40 −30 −20 −10 0−10

0

10

20

30

40

50

[m]

[m]

������� � ����

−100 −80 −60 −40 −20 0

−20

−10

0

10

20

30

[m][m

]

������� �.����

−30 −20 −10 0 10

−25

−20

−15

−10

−5

0

5

10

15

20

25

[m]

[m]

��� ��� � ����

� 1"�5��)N�0� � O�Z0<HABZ/RT<�<GY8@��l<\RT<\?]A�o3VXABZ0F�P0FT<�Y�@:?��5@:ehP/RT< 6h6�VX?0YqO5VX�/gD<�687

)��('5��� ����) )��('5��� � $ �� � � � d�V � d �8a�xig d�<�VX? � d �8a�xig d�V a � g�� d � d�<�VX? a � g�� d ��87JI �87JphI a �87J�h�h`1g �87:9�� a �87J��~E`1g �87JpXj a ~E�1g �87:9(~a �h�1g9 9h7J�h� a �1g �87 ~%9Oa �87J�h`1g 9h7J�h� a 9��h�1g �87J`h� a ~E�1gI 687J`h� a �1g 9h7J6h� a �1g 687JIh� a ~E�1g �87J�h` a 9��h�1g` I87Jph� a �87J�h�h`1g 9h7J�h� a �1g I87J6h� a �h`1g 9h7JIh� a 9��h�1g

� ,/¤�!�) � � O�Z0<�gDC�UW<�FBA�nEVXg:P0<�F�CX�{ABZ0<-d�V fVX?0Y�d�<�VX?�Y8@DFTA^VX?0[�<-ATC�o3VXABZ�VXA�Y8@��l<\RT<\?]A�?0CE@DFT<FBA^VX?0Y0VXRTY+Y/<\n%@�VXAB@DCE?��c7�O�Z0<GnhVXg:P0<�F�U�@:ABZ/@:?No3VXRT<\?|ABZ0<�FT@DF�VXRT<GABZ0< [�CERBRT<�FTo�CE?0Y8@:?/efCEo/AB@:d�VXg� � nEVXg:P0<GVX?0YSwQC]CExfy-Z0<�VEY�;�@DFBA^VX?0[�<Oa � gvRT<�FBo�<�[\AB@:n�<\g:s�7$a2T�<>� <\R�ATC��{@:ehP/RT<G689 g>7

)��('5��� ����) )��('5��� � $ �z{VXABZ d�V � d �8a�xig d�<�VX? � d �8a�xig d�V a � g�� d � d�<�VX? a � g�� d �9 9h7J�h� a �1g �87 ~%9Oa �87J�h`1g 9h7J�h� a 9��h�1g �87J`h�a ~E�1g6 �87 j|� a �1g �87J6hp a �1g 9h7:9�� a 9�`1g �87 jhj a `h�1gI 9h7JIh� a �87J�h`1g �87Jph` a �87:9��1g 9h7JIh� a `h�1g �87 ~E6a 9��1g

� ,/¤�!�)���� O�Z0<cgDC(Uv<�FBA{nhVXg:P0<�F�� CERQABZ0<cd�V GVX?0YHd�<�VX? Y8@DFTA^VX?0[�<mATC�o3VXABZG�#CERQY8@���<\RT<\?|A{o3VXABZ0F�7�-CE@DFT<�FBA^VX?0Y0VXRTY�Y/<\n%@�VXAB@DCE? � � �h7�O�Z0<WnEVXg:P0<�F�U�@:ABZ/@:?�o3VXRT<\?]ABZ0<�FB@DFrVXRT<WABZ0<�[�CERBRT<�FBolCE?0Y8@:?/eCEo/AB@:d�VXg � � nhVXg:P0<VX?0Y�wQC%CExty-Z0<�VEY�;�@DFBA^VX?0[�<Oa � gvRT<�FBol<�[\AB@:n�<\g:s�7$a2T�<>� <\R�ATC��{@:ehP/RT<G6h61g

�3� � � G��� ��� �

F ` B G/H�P P � P ^aR I � R H�P(RTH ; P P Z�<�� P ; P X R � P H�K � < IEb ; M

��?tABZ/@DF�FT<�[\AB@DCE?fUv<�gDC%CExtVXAWZ0C�U ABZ0<�ABZ/RT<�<�Y8@��l<\RT<\?]A���<\Z3V�n]@DCEP/RTF�V���<�[\A�ABZ0<-o3VXABZt� CEg:gDC�UW<\R@:?�Y8@���<\RT<\?|A FB@:ABP3VXAB@DCE?0F�7 �_<�P0FT<�wQC%CEx_y-Z0<�VEY�;�@DFBA^VX?0[�<+9�6Xd VX?0Y¡x � nhVXg:P0<��87J��~%7���CEAT<ABZ3VXAvABZ0<-ehR^VXo/Z0F�FBZ0C�U�@:?/e ABZ0<�Y8@DFTA^VX?0[�<-ATCGABZ0<�o3VXABZt@:?tABZ0<�=0ehP/RT<�F�VXRT<�ABZ0<-RT<�VXglY8@DFBA^VX?0[�<��VX?0Yq?0CEA��3VEFT<�YqCE?�ABZ0<�?0CE@DFTs�olChFB@:AB@DCE?SFB@:eh?3VXg 7

�5@:ehP/RT<�6Xj�VX?0Y��{@:ehP/RT< 6h`�FBZ0C�U ABZ0< FB@:ABP3VXAB@DCE?�U�Z0<\?+ABZ0<Hn�<\Z/@D[\gD<@DF�gDC][�VXAT<�Y�CE?SABZ0<Ho3VXABZVXg:RT<�VEY8s���RTCEd ABZ0<-��<\eh@:?/?/@:?/e07 �u?�ABZ/@DF�[�VEFT<���CEABZ�K k VX?0YfK3L�VXRT<�<�FTFT<\?]AB@�VXg:g:s � <\RTC�VX?0Y�ABZ0<RT<�FBP/g:AB@:?/e�FBAT<�<\RB@:?/e�VX?/ehgD<G< �|P3VXgDF�ABZ0<HY8RB@:n�<\R � F�FBAT<�<\RB@:?/e�VX?/ehgD<�K a 7

�5@:ehP/RT<f6hpNVX?0Y��{@:ehP/RT<q6�~qFBZ0C(U ABZ0<�F^VXd�<�FB@:ABP3VXAB@DCE? �/P/A U�@:ABZ V+FB@:dGP/g�VXAT<�Y FB@:?]P0FTCE@DY0VXgo�ChFB@:AB@DCE? ?0CE@DFT<NU�@:ABZ � � � � � � a#FT<�<NFT<�[\AB@DCE? j07:9h7:9 g>7 O�Z0<Sn�<\Z/@D[\gD<N@DFtgDC%[�VXAT<�Y CE? ABZ0<o3VXABZ¡VXg:RT<�VEY8sS�#RTCEd&ABZ0<���<\eh@:?/?/@:?/e07 y-o3VXRBA���RTCEd VtFBo/@:x�<�CE?¡K k �#CERHbN<\ABZ0C]Y AuUvC �Q�lCEABZd�<\ABZ0C]Y/Fc�l<\Z3V�n�<�@:?�ABZ0<�F VXd�<�d�VX?/?0<\R�7{O�Z0<�RT<�VEF^CE? �#CERmCX��ABZ/@DFcFBo/@:x�<�@DFcY8@DFT[\P0FTFT<�Y �#P/RBABZ0<\R@:? � <�[\AB@DCE?tj07J687J687mO�Z0<-?0CE@DFT<�eh@:n�<�FWeh@:n�<�FWR^VX@DFT<-ATCGY8@DFBABP/RB�3VX?0[�<�F�@:?t��CEABZqK3L�VX?0YfK k 7�O�Z/@DFY8@DFBABP/RB�3VX?0[�<W@DF�o/RTCEo3VXe|VXAT<�Y�ATC-ABZ0<rRT<�FBP/g:AB@:?/e�FBAT<�<\RB@:?/eVX?/ehgD<h7{O�Z0<rRT<�FBP/g:AB@:?/e�o3VXABZ�ABR^VE[Tx%@:?/eVXgDFTC�FBZ0C�U�F�V�d�@:?0CER-ChFT[\@:g:g�VXAB@DCE?NVX��CEP/A�ABZ0<�o3VXABZNVEF�V RT<�FBP/g:A�CX��ABZ0<H?0CE@DFT<h7

��? �5@:ehP/RT<f6h�NVX?0Y��{@:ehP/RT<f6h���5ABZ0<�n�<\Z/@D[\gD<tFBA^VXRBATF�9�`Sd�<\AT<\RTF�V�U�V�s_�#RTCEd ABZ0<�o3VXABZ �mVXo��o/RTC�VE[TZ0<�F�@:AWVX?0Y�ABR^VE[Tx8FW@:Ar?/@D[�<\g:s�ATCGABZ0<-<\?0YtCX�QABZ0<�o3VXABZQ7c�{@:ehP/RT<�6h�GFTZ0C�U�FWABZ0<��l<\Z3V�n%@DCER�#CERHbN<\ABZ0C]Y¡�-?0<�VX?0Y �{@:ehP/RT<�6h��� CER�bN<\ABZ0C%Y OQUvC/7 ��CqFB@:eh?/@�=3[�VX?|A Y8@��l<\RT<\?0[�< �l<\A�UW<�<\?ABZ0<A�UWC�d�<\ABZ0C%Y/F�[�VX?+��<GFT<�<\?�@:?SABZ/@DF-<�/VXd�o/gD<h7�O�Z0< T�<�FBP/g:AB@:?/e�FTAT<�<\RB@:?/etVX?/ehgD<@DF-[�CEd �o�ChFT<�YqCX�mVXg:g�ABZ/RT<�<H�l<\Z3V�n%@DCERTF-VEF�gDCE?/e�VEF�ABZ0<Hn�<\Z/@D[\gD<H@DF�� VXR�V�U�V�st��RTCEd ABZ0<�o3VXABZQ7my��lCEP/Aph`HFT<�[�CE?0Y/F�� CERBd FBA^VXRBA��|��CEABZ�K k VX?0Y�K3LG�l<�[�CEd�<�Y/CEd�@:?3VXAT<�Y��|s ABZ0<�?0CE@DFT<��%VX?0Y�ABZ0<�ATCEA^VXg��<\Z3V�n]@DCER-VXg:d�ChFTA�<\?|AB@:RT<\g:s�[�CE?0FB@DFTATF�CX�5d�@:d�@D[^x]@:?/e�ABZ0<FBAT<�<\RB@:?/e�VX?/ehgD<Oa K a g>7

�0CERfRT<>�#<\RT<\?0[�<����{@:ehP/RT< Ih� VX?0Y I89 FBZ0C�U�FfZ0C(U �3CEg:gDC�U&ABZ0<_��VXRBRTCEA�VX?0Y zmP/RT<NzmP/RTFBP/@:A��<\Z3V�n�<�F�U�@:ABZ�?0CE@DFT<h7

� ��� � '5)S) � )|�����3¢��('5)��('5�X)|)S� ��� )|�X)]����¤r)]'5,�� � �"�5��. �3�

0 50 100 150 200−5

0

5

10x 10

−4 φβ (orientation difference)

Time [s]

[deg

]

0 50 100 150 200−50

0

50

φγ (mimic steering angle)

Time [s]

[deg

]

0 50 100 150 200−5

0

5x 10

−4 φα (move towards path)

Time [s]

[deg

]

0 50 100 150 200−60

−40

−20

0

20

40Resulting steering angle

Time [s]

[deg

]

0 50 100 150 200−1.5

−1

−0.5

0

0.5

1

1.5x 10

−4 Distance to path

Time [s]

[m]

−60 −50 −40 −30 −20 −10 00

10

20

30

40The path

[m]

[m]

PathVehicle

� 1"�5�X)N�/� � O�Z0<�ABZ/RT<�<Y8@��l<\RT<\?]A��l<\Z3V�n%@DCEP/RTF�U�@:ABZSbN<\ABZ0C%YqCE?0<GVX?0Yq?0C ?0CE@DFT<h7

0 50 100 150 200−5

0

5

10x 10

−4 φβ (orientation difference)

Time [s]

[deg

]

0 50 100 150 200−50

0

50

φγ (mimic steering angle)

Time [s]

[deg

]

0 50 100 150 200−5

0

5x 10

−4 φα (move towards path)

Time [s]

[deg

]

0 50 100 150 200−60

−40

−20

0

20

40Resulting steering angle

Time [s]

[deg

]

0 50 100 150 200−1.5

−1

−0.5

0

0.5

1

1.5x 10

−4 Distance to path

Time [s]

[m]

−60 −50 −40 −30 −20 −10 00

10

20

30

40The path

[m]

[m]

PathVehicle

� 1"�5��)+�"��� O�Z0<HABZ/RT<�<Y8@���<\RT<\?|A-��<\Z3V�n]@DCEP/RTF�U�@:ABZ�b+<\ABZ0C%YqAuUvC�VX?0Yf?0C�?0CE@DFT<h7

�/� � � G��� ��� �

0 50 100 150 200−10

−5

0

5

10

φβ (orientation difference)

Time [s]

[deg

]

0 50 100 150 200−50

0

50

φγ (mimic steering angle)

Time [s]

[deg

]

0 50 100 150 200−6

−4

−2

0

2

4

6

φα (move towards path)

Time [s]

[deg

]

0 50 100 150 200−50

0

50Resulting steering angle

Time [s]

[deg

]

0 50 100 150 200−0.8

−0.6

−0.4

−0.2

0

0.2Distance to path

Time [s]

[m]

−60 −40 −20 00

10

20

30

40

50The path

[m]

[m]

PathVehicle

� 1����X)��3� � O�Z0<mABZ/RT<�<WY8@���<\RT<\?|A{��<\Z3V�n]@DCEP/RTF{U�@:ABZ b+<\ABZ0C%YGCE?0<WVX?0YG?0CE@DFT<rU�@:ABZ �87JI�d�<\AT<\RFBA^VX?0Y0VXRTY�Y/<\n%@�VXAB@DCE?Q7

0 50 100 150 200−10

−5

0

5

10

φβ (orientation difference)

Time [s]

[deg

]

0 50 100 150 200−50

0

50

φγ (mimic steering angle)

Time [s]

[deg

]

0 50 100 150 200−10

0

10

20

30

φα (move towards path)

Time [s]

[deg

]

0 50 100 150 200−50

0

50Resulting steering angle

Time [s]

[deg

]

0 50 100 150 200−0.8

−0.6

−0.4

−0.2

0

0.2Distance to path

Time [s]

[m]

−60 −40 −20 00

10

20

30

40

50The path

[m]

[m]

PathVehicle

� 1����X)�� ��� O�Z0<mABZ/RT<�<WY8@��l<\RT<\?]A5�l<\Z3V�n%@DCEP/RTF5U�@:ABZ bN<\ABZ0C]YGA�UvC�VX?0Y?0CE@DF^<mU�@:ABZ��87JI�d�<\AT<\RFBA^VX?0Y0VXRTY�Y/<\n%@�VXAB@DCE?Q7

� ��� � '5)S) � )|�����3¢��('5)��('5�X)|)S� ��� )|�X)]����¤r)]'5,�� � �"�5��. �"�

0 50 100 150 200 250−50

−40

−30

−20

−10

0

10

φβ (orientation difference)

Time [s]

[deg

]

0 50 100 150 200 250−50

0

50

φγ (mimic steering angle)

Time [s]

[deg

]

0 50 100 150 200 250−20

0

20

40

60

80

φα (move towards path)

Time [s]

[deg

]

0 50 100 150 200 250−60

−40

−20

0

20

40

60Resulting steering angle

Time [s]

[deg

]

0 50 100 150 200 250−5

0

5

10

15Distance to path

Time [s]

[m]

−60 −40 −20 00

10

20

30

40

50

60The path

[m][m

]

PathVehicle

� 1����X) � � � O�Z0<cABZ/RT<�<rY8@���<\RT<\?|A{�l<\Z3V�n%@DCEP/RTFQU�@:ABZ bN<\ABZ0C%YCE?0<��XU�Z0<\?GFBA^VXRBAB@:?/e 9�`�d�<\AT<\RTF��RTCEd ABZ0<�o3VXABZNVX?0YqU�@:ABZ�?0CE@DFT<�U�@:ABZ+�87JI�d�<\AT<\R�FBA^VX?0Y0VXRTY�Y/<\n%@�VXAB@DCE?Q7

0 50 100 150 200 250−40

−30

−20

−10

0

10

φβ (orientation difference)

Time [s]

[deg

]

0 50 100 150 200 250−50

0

50

φγ (mimic steering angle)

Time [s]

[deg

]

0 50 100 150 200 250−20

0

20

40

60

φα (move towards path)

Time [s]

[deg

]

0 50 100 150 200 250−60

−40

−20

0

20

40

60Resulting steering angle

Time [s]

[deg

]

0 50 100 150 200 250−5

0

5

10

15Distance to path

Time [s]

[m]

−60 −40 −20 00

10

20

30

40

50

60The path

[m]

[m]

PathVehicle

� 1����X)G� � � O�Z0<cABZ/RT<�<rY8@���<\RT<\?|A5��<\Z3V�n]@DCEP/RTF�U�@:ABZGb+<\ABZ0C%YHAuUvC �XU�Z0<\?GFBA^VXRBAB@:?/e�9�`�d�<\AT<\RTF��RTCEd ABZ0<�o3VXABZNVX?0Yq?0CE@DFT<�U�@:ABZ+�87JI d�<\AT<\R-FBA^VX?0Y0VXRTY�Y/<\n%@�VXAB@DCE?�7

�0� � � G��� ��� �

0 50 100 150 200−50

0

50Resulting steering angle

Time [s]

[deg

]

0 50 100 150 200−2

−1

0

1

2

3

4Distance to path

Time [s]

[m]

−60 −50 −40 −30 −20 −10 00

5

10

15

20

25

30

35

40

45The path

[m]

[m]

PathVehicle

� 1"�5��)+� � � �0CEg:gDC(U ABZ0< ��VXRBRTCEA�U�@:ABZ+�87JI d�<\AT<\R�FBA^VX?0Y0VXRTYSY/<\n]@�VXAB@DCE?�?0CE@DF^<h7

0 50 100 150 200−50

0

50Resulting steering angle

Time [s]

[deg

]

0 50 100 150 200−1.5

−1

−0.5

0

0.5

1

1.5

2

2.5

3Distance to path

Time [s]

[m]

−60 −50 −40 −30 −20 −10 00

10

20

30

40

50The path

[m]

[m]

PathVehicle

� 1"�5�X)+����� zmP/RT<o/P/RTFBP/@:A�U�@:ABZS�87JI�d�<\AT<\R�FBA^VX?0Y0VXRTY�Y/<\n%@�VXAB@DCE?S?0CE@DFT<h7

� ��� � '5)S) � )|�����3¢��('5)��('5�X)|)S� ��� )|�X)]����¤r)]'5,�� � �"�5��. � �

� �����#� �r� 4 � ��,8� � ����.

y-F�[�VX? �l<�FT<�<\? @:? �5@:ehP/RT<�9�����b+<\ABZ0C%Y CE?0<+Y/C]<�F�?0CEA�Z3VX?0Y8gD<+g�VXRBe�<�?0CE@DFT<qn�<\RBs UW<\g:g 7��?_ABZ/@DF�FT<�[\AB@DCE? �{Uv<�U�@:g:grgDC%CEx¡VXAU�Z3VXA�ABZ/@DF@DFG[�VXP0F^<�Y¡�]s ��VX?0Y¡ABZ0<�Y8@���<\RT<\?0[�<��l<\A�UW<�<\?ABZ0<�A�UWCqd�<\ABZ0C]Y/FU�Z0<\?�VXo/o/g:s]@:?/eNVtg�VXRBe�<�?0CE@DFT<h7 �u?_�5@:ehP/RT<�Ih6���ABZ0<�olChFB@:AB@DCE? eh@:n�<\?_�|sABZ0<�o�ChFT@:AB@DCE?_FT<\?0FTCERTF�nhVXRB@D<�FHn�<\RBs dGP0[TZ¡��<\AuUv<�<\?�<�VE[^Z¡F^VXd�o/gD<�VX?0Y U�Z0<\? Vt?0CE@DFT< U�@:ABZFBA^VX?0Y0VXRTY Y/<\n]@�VXAB@DCE?�� � � � � @DFcVXo/o/g:@D<�Y �EABZ0<Wd�<�VEFBP/RT<�Y�o�ChFB@:AB@DCE? [�VX?���<WC�n�<\R�9��h�-d�<\AT<\RTF��RTCEd U�Z0<\RT<�ABZ0<�n�<\Z/@D[\gD<qVE[\ABP3VXg:g:s_@DF�7+O�Z/@DF CX�-[�CEP/RTF^<�d�VXx�<�F�@:AGn�<\RBs�Y8@���[\P/g:A�ATCSx]?0C(U<�/VE[\ABg:s+U�Z0<\RT< s�CEP_VXRT<���VX?0Y <\n�<\? U�Z/@D[TZ_FB@DY/<�CX�rABZ0<�o3VXABZ s�CEP VXRT<�CE?_VX?0YNABZ0<\RT<>�#CERT<�V�3VEY�ol<\R �#CERBd�VX?0[�<�[�VX?S�l<G<�8o�<�[\AT<�Y�7�y�F�d�<\?]AB@DCE?0<�Y <�VXRBg:@D<\R��"ABZ/@DF�@DF�?0CEA�V�RT<�VXg:@DFBAB@D[GgD<\n�<\gCX�l?0CE@DF^<�U�Z0<\?�[�CE?]ABRTCEg:g:@:?/e VH�#CERT<�FBArd�VE[TZ/@:?0<ba�@:?�ABZ0<�FT<\?0F^<�ABZ3VXAr@:Ar[�VX?�?0CEAm��<�ATCEgD<\R^VXAT<�Ylg���/P/A�@:? CERTY/<\R�ATCqVX?3VXg:s � <�U�Z|sNb+<\ABZ0C%YNCE?0<�Y/C%<�F�?0CEA-UWCERBx �l@:A�@DF-@:?|AT<\RT<�FBAB@:?/eqATCtgDC%CEx+VXAU�Z3VXA�C][�[\P/RTF-VXA�ABZ/@DF�?0CE@DFT<HgD<\n�<\g 7

0 10 20 30 40 50 60 70

−100

−80

−60

−40

−20

0

20

40

60

80

100

Position difference

Time [s]

[m]

� 1����X)������ O�Z0<�Y8@��l<\RT<\?0[�<���<\AuUv<�<\?fABZ0<�RT<�VXglVX?0Y�ABZ0<���<\g:@D<\n�<�Y<a�?0CE@DFBsRgco�ChFT@:AB@DCE?�[�VX?��l<n�<\RBs�g�VXRBe�<h7v�-CE@DFT<�U�@:ABZ+FBA^VX?0Y0VXRTY�Y/<\n%@�VXAB@DCE? � � � � � @DF�VXo/o/g:@D<�YS@:?qABZ/@DF�<�0VXd�o/gD<

O�Z0<�CE?/g:stY8@��l<\RT<\?0[�<��l<\A�UW<�<\?qABZ0<-AuUvC d�<\ABZ0C%Y/F�@DFWABZ0<-U�V�sqK k @DF�[�VXgD[\P/g�VXAT<�Y�a�@ 7 <�Z0C�U ATCd�C�n�<��3VE[Tx�ATCGABZ0<�o3VXABZ)g��8FTC ABZ0<-�l<\Z3V�n%@DCEP/R�CX�5bN<\ABZ0C]YtCE?0<�d P0FBA���<�Y8P0<�ATCGABZ0<-UWV�sqK k@DF�[�CEd�o/P/AT<�Y�7 ���{UW<GY/C�?0CEA�g:@:d�@:A�K k @:?�d�<\ABZ0C%YSCE?0<GATC���<U�@:ABZ/@:?�� ��� � a#FT<�<���]P3VXAB@DCE?687JI1g���ABZ0<S<\RBRTCERtVXRB@DF^<�F�VXA�d P0[TZ gDC�UW<\R�?0CE@DFT<SgD<\n�<\gDF�ABZ3VX? U�@:ABZ ABZ0<Sg:@:d�@:A^VXAB@DCE?Q7 � �HABZ0<x � nhVXg:P0<�@DFvg�VXRBe�<��|ABZ0<�<\RBRTCERWVXo/ol<�VXRTF�VXArgDC(Uv<\R�?0CE@DFT<�gD<\n�<\gDFvABZ3VX?tU�@:ABZtVGgDC�Uv<\RWx � nEVXg:P0<��%FT<�<�5@:ehP/RT< IhI87�O�Z/@DF-UvCEP/gDY FBP/ehe�<�FBA-ABZ3VXA�K k @:?Nb+<\ABZ0C%YSCE?0<G��<�[�CEd�<�F-ATC]C�g�VXRBe�<G@:?N[�<\RBA^VX@:?FB@:ABP3VXAB@DCE?0F�7��WCE?0FB@DY/<\R-�{@:ehP/RT<�IXj07 �u?�ABZ/@DF-<�/VXd�o/gD<GUv<o/RTC(n�CEx�<GABZ0< <\RBRTCER��]stgD<\ABAB@:?/etABZ0<n�<\Z/@D[\gD<�FBA^VXRBAGCEP/ATFT@DY/<�CX��VX?0Y¡VXo/o/RTC�8@:d�VXAT<\g:s o�<\RBol<\?0Y8@D[\P/g�VXRGATC�ABZ0<�o3VXABZ ���/P/AGU�@:ABZ¡?0C?0CE@DFT<qVXo/o/g:@D<�Y�7�O�Z0<fFBP/d CX�-ABZ0<tABZ/RT<�<f�l<\Z3V�n%@DCEP/RTF @DF gD<�FTF�ABZ3VX? � ��� � � �cU�Z/@D[^Z d�<�VX?0FABZ3VXArABZ0<�RT<�FBP/g:AB@:?/e�FBAT<�<\RB@:?/e�VX?/ehgD<�KlmQ[TZ3VX?/e�<�F�FB@:eh?�VX?0Y��l<�[�CEd�<�Fvo�ChFB@:AB@:n�<��|U�Z/@D[TZ�@:?�ABP/RB?d�<�VX?0FHABZ3VXAABZ0< n�<\Z/@D[\gD< ABP/RB?0FH@:? ABZ0< U�RTCE?/eSY8@:RT<�[\AB@DCE?Q7 ���vUW< UvCEP/gDY_[^Z0C]ChF^<�VtFBd�VXg:gD<\R

� � � � G��� ��� �

x � nhVXg:P0< @:? ABZ/@DF<�0VXd�o/gD<��{K k UvCEP/gDY_�l<�[�CEd�<�FBd�VXg:gD<\R���Kim�UWCEP/gDY_�l< g�VXRBe�<\RGABZ3VX? � ��� � �VX?0YqABZ0<Hn�<\Z/@D[\gD<HUWCEP/gDY�ABP/RB?q@:?�ABZ0<[�CERBRT<�[\A-Y8@:RT<�[\AB@DCE? �0RT<>� <\R�ATC��{@:ehP/RT<GIh`87

y�[�[�CERTY8@:?/e+ATC+���|P3VXAB@DCE? 687J����ABZ0<�FBAT<�<\RB@:?/eNVX?/ehgD<��#CER b+<\ABZ0C%Y_AuUvC+@DFG[�VXgD[\P/g�VXAT<�Y VEFABZ0<wQC]CEx y�Z0<�VEY�y�?/ehgD<Wd�@:?]P0F5ABZ0<�CERB@D<\?]A^VXAB@DCE? a � ��j%g���U�Z/@D[^Z d�<�VX?0F�ABZ3VXA5ABZ0<�FBAT<�<\RB@:?/eHVX?/ehgD<VXP/ATCEd�VXAB@D[�VXg:g:s Z3VEFABZ0<�RB@:ehZ]AGFB@:eh?�VX?0Y d�VXeh?/@:ABP0Y/<��{Z0<\?0[�<�UW<�Y/Cq?0CEAe�<\AGABZ0<�o/RTCE�/gD<\dY/<�FT[\RB@:��<�Y+VX��C(n�<H@:?�b+<\ABZ0C%YqAuUvC/7

−60 −40 −20 030

40

50

60

70

80

90

100

110

120

130

[m]

[m]

����� ������ ���� #�� �� � � ��� � �������� �������� � ���&��� � �. � ���� �

−40 −20 0 20 40

10

20

30

40

50

60

70

80

90

100

110

[m]

[m]

��� �!���"��� ���� #�� �� � � �(� � �.����� �.������ � ���(��� � � � ���� �����

� 1����X)��3� � � @:ABZ�V gDC�Uv<\R�x � nhVXg:P0<��/b+<\ABZ0C%Y�CE?0<��l<�[�CEd�<�F�gD<�FTF�FT<\?0FB@:AB@:n�<�ATC�?0CE@DFT< a#[�CEd �o3VXRT<HATC��{@:ehP/RT<�9��1g>7

� ������� ���{�m� � �������"���X)|�h� � ���

� Z0<\?+ABZ0<n�<\Z/@D[\gD<@DF�V�n�CE@DY8@:?/eqVX?+CE�0FBA^VE[\gD<G?0<�VXR�ABZ0<<\?0Y+CX�cABZ0<o3VXABZMa#CER-@��c@:A�@DF��l<�FB@DY/<ABZ0<qo3VXABZ � CER�CEABZ0<\R�RT<�VEFTCE?0F��WFBP0[TZ VEF�?0CE@DFT<q@:? ABZ0<qolChFB@:AB@DCE? F^<\?0FTCERTFAg���VNo/RTCE�/gD<\d [�VX?C][�[\P/R�7 � CEd�<\AB@:d�<�F�ABZ0< n�<\Z/@D[\gD< Y/C%<�F�?0CEA�RT<\ABP/RB?NATC�ABZ0< o3VXABZ �]P/@D[Tx%g:s�<\?0CEP/ehZ �"U�@:ABZNABZ0<RT<�FBP/g:A�ABZ3VXA�@:A�VXRBRB@:n�<�F-�l<�FB@DY/<�ABZ0<G<\?0Y8olCE@:?|A�VEF�@:?S�{@:ehP/RT< IhpRa V1g>7vO�Z/@DF�o/RTCE�/gD<\d @DF-FTCEg:n�<�Y�|s FT<\ABAB@:?/e�ABZ0<�w�C]CEx y-Z0<�VEY_z5CE@:?]AGATCqABZ0<�<\?0Y8o�CE@:?]A CE?_ABZ0<�o3VXABZ¡@���ABZ0<��#CEg:gDC(U�@:?/eSA�UWC[�CE?0Y8@:AB@DCE?0F�VXRT<HF^VXAB@DFB=3<�Y�M

! O�Z0<Y8@DFBA^VX?0[�<��l<\A�UW<�<\?SABZ0<�n�<\Z/@D[\gD<VX?0YqABZ0<�o3VXABZq@DF�g�VXRBe�<�a�g�VXRBe�<\R�ABZ3VX?�CE?0<�d�<\AT<\R

� ��� � '5)S) � )|�����3¢��('5)��('5�X)|)S� ��� )|�X)]����¤r)]'5,�� � �"�5��. � �

0 50 100 150 200−200

−100

0

100

200

φβ (orientation difference)

Time [s]

[deg

]

0 50 100 150 200−33.5

−33

−32.5

−32

−31.5

−31

φγ (mimic steering angle)

Time [s]

[deg

]

0 50 100 150 200−100

−80

−60

−40

−20

0

20

φα (move towards path)

Time [s]

[deg

]

0 50 100 150 200−200

−100

0

100

200Resulting steering angle

Time [s]

[deg

]

0 50 100 150 200−50

−40

−30

−20

−10

0

10Distance to path

Time [s]

[m]

−100 −50 00

20

40

60

80

100

120The path

[m][m

]

PathVehicle

� 1"�5��)+�0� � O�Z0<H�l<\Z3V�n%@DCEP/R�CX�cb+<\ABZ0C%YqCE?0<HU�Z0<\?+VX?+<\RBRTCER�@DF�o/RTC�n�CEx�<�Y�7

0 20 40 60 80 100−100

−50

0

50

100

φβ (orientation difference)

Time [s]

[deg

]

0 20 40 60 80 100−40

−30

−20

−10

0

10

20

φγ (mimic steering angle)

Time [s]

[deg

]

0 20 40 60 80 100−100

−80

−60

−40

−20

φα (move towards path)

Time [s]

[deg

]

0 20 40 60 80 100−200

−150

−100

−50

0

50Resulting steering angle

Time [s]

[deg

]

0 20 40 60 80 100−35

−30

−25

−20

−15

−10

−5Distance to path

Time [s]

[m]

−50 −40 −30 −20 −1040

60

80

100

The path

[m]

[m]

PathVehicle

� 1����X) ����� O�Z0<�F VXd�<tFT@:ABP3VXAB@DCE? VEF�@:? �{@:ehP/RT<qIXj+�/P/A U�@:ABZ VSFTd�VXg:gD<\R�x � nhVXg:P0<�U�Z/@D[^Zd�VXx�<HABZ0<Hn�<\Z/@D[\gD<HABP/RB?S[�CERBRT<�[\ABg:s�7

� � � � G��� ��� �

@:?SCEP/R�[\P/RBRT<\?]A�@:d�o/gD<\d�<\?|A^VXAB@DCE?)g>7

! O�Z0<wQC%CExfy�Z0<�VEY�z{CE@:?|A-@DF�VXA�CER��l<\s�CE?0YqABZ0<<\?0Y8olCE@:?|A�CX�{ABZ0<�o3VXABZQ7

�5@:ehP/RT<�IhpRa��)g-FBZ0C(U�F-ABZ0< RT<�FBP/g:A�U�Z0<\?NP0FB@:?/e�ABZ0< [�CERBRT<�[\AB@DCE?_VX�lC�n�<h7Hy�F�UW<�[�VX? FT<�<���ABZ0<n�<\Z/@D[\gD< @DFHVX�/gD<�ATCtRT<\ABP/RB? dGP0[TZ � VEFBAT<\R�ATCtABZ0<Go3VXABZ_VX?0YNABZ0<\RT<\�|s <\?0Y/F�P/oN[\gDChFT<\R�ATCtABZ0<<\?0Y8o�CE@:?]A�7 O�Z/@DF�[�CERBRT<�[\AB@DCE? [�VX? FTCEd�<\AB@:d�<�F �l<�F^<�<\?�@:? K k VEF VSFTo/@:x�<�@:? ABZ0<tFBAT<�<\RB@:?/e[�CEd�d�VX?0Y�U�Z0<\? ABZ0<�n�<\Z/@D[\gD<�@DFG� CERT[�<�Y¡ATCNFBAT<�<\R�Z3VXRTY/<\R ATC(UWVXRTY/F�ABZ0<�o3VXABZ a#F^<�<��{@:ehP/RT<6�~(g>7

10 15 20 25 30 35

80

85

90

95

100

105

110

115

120

[m]

[m]

������� � � � � � �� �� � ���,� ��

10 15 20 25 30 35

80

85

90

95

100

105

110

115

120

[m]

[m]

������� � � � �" � �� �� � �&�,�� ��

� 1����X) �3� � O�Z0< <>�l<�[\A�CX�vVXo/o/g:s]@:?/efV�[�CERBRT<�[\AB@DCE?NU�Z0<\?+ABZ0<Gn�<\Z/@D[\gD<G@DF�?0CEA-CE?SABZ0<Go3VXABZVX?0Yq?0<�VXR�ABZ0<H<\?0Y�7

F `� U � M�R\KD^ > P K � I <QZ K X ^ P

��? CERTY/<\R ATCN��P/?0[\AB@DCE? @:? ABZ0<t� CERT<�FBA d�VE[^Z/@:?0<SVXo/o/g:@D[�VXAB@DCE? Y/<�FT[\RB@:�l<�Y @:? � <�[\AB@DCE? 9��mABZ0<o3VXABZ ABR^VE[Tx�<\R�dGP0FBA��l<�VX�/gD<fATC UWCERBx ATCEe�<\ABZ0<\R�U�@:ABZ RTCEP/AB@:?0<�F�� CER�CE�0FBA^VE[\gD<SV�n�CE@DY0VX?0[�<h7�5@:ehP/RT<�I�~HFBZ0C(U�FrABZ0<�n�<\Z/@D[\gD<-V�n�CE@DY8@:?/e�CE�0FBA^VE[\gD<�FvU�@:ABZ�ABZ0<���� B� d�<\ABZ0C%Y �CD ���h� �3U�Z/@:gD<ABR^VE[Tx%@:?/e�ABZ0<�o3VXABZqU�@:ABZqABZ0<H�0CEg:gDC�U ABZ0<Hz{VEFBA�d�<\ABZ0C%Y�7mO�Z0<�AT<�FBA�FTZ0C�U�F�ABZ3VXA�ABZ0<H�0CEg:gDC(UABZ0<Gz�VEFBA�d�<\ABZ0C%Y�UWCERBx%F-Uv<\g:g5@:?+[�CEdG�/@:?3VXAB@DCE?NU�@:ABZ+CE�0FTA^VE[\gD<�V�n�CE@DY0VX?0[�<h7��-CEAT<GABZ3VXA�ABZ0<Y/<\n]@�VXAB@DCE?���RTCEd ABZ0<�o3VXABZ¡@DF [�VXP0FT<�Y_�]s ABZ0<�CE�0FBA^VE[\gD<tV�n�CE@DY8@:?/e FBs8FBAT<\d U�Z0<\? CE�0FBA^VE[\gD<�FVXRT<Y/<\AT<�[\AT<�Yq?0<�VXR�ABZ0<Hn�<\Z/@D[\gD<h7

�Q�

-10 0 10 20 30

40

50

60

70

80

90

100

110

120

130

[m]

[m]

� 1����X)S� ��� y�n�CE@DY8@:?/e�CE�0FTA^VE[\gD<�F�U�@:ABZqABZ0< ��� B� d�<\ABZ0C]Yf@:?�[�CEd �/@:?3VXAB@DCE?SU�@:ABZ��0CEg:gDC(UO�Z0<Gz�VEFBA�UWCERBx%F-Uv<\g:g 7�O�Z0<GY/<\n%@�VXAB@DCE?��#RTCEd ABZ0<Go3VXABZ�@DF�Y8P0<GATC�V�n�CE@DY8@:?/e�ABZ0< CE�0FBA^VE[\gD<�F��?0C�?0CE@DFT<�@DF�VXo/o/g:@D<�YSZ0<\RT<h7

� ���d��� W[� ���A�d���

�_<�Z3V�n�<�@:d�o/gD<\d�<\?|AT<�Y VX?0Y�<\nEVXg:P3VXAT<�Y Vq?0<\U o3VXABZ¡ABR^VE[^x]@:?/e VXg:e�CERB@:ABZ/d ABZ3VXAGP0FT<�F ABZ0<CEo�<\R^VXATCER � FWFBAT<�<\RB@:?/e [�CEd�d�VX?0Y/F�VEFr@:?/o/P/A��|@:?�VEY/Y8@:AB@DCE?tATCHolChFB@:AB@DCE?tVX?0Y�Z0<�VEY8@:?/eG@:?8� CERBd�V �AB@DCE?Q7mO�Z0<VXg:e�CERB@:ABZ/d Z3VEF�FBP/ol<\RB@DCER�o�<\R � CERBd�VX?0[�<�U�Z0<\?�[�CEd�o3VXRT<�YqATC�FBA^VX?0Y0VXRTYfd�<\ABZ0C%Y/Fg:@:x�<HABZ0<GzcP/RT<GzcP/RTFBP/@:A�VX?0YS�0CEg:gDC�U ABZ0< ��VXRBRTCEA�VXg:e�CERB@:ABZ/d�F(7vO�Z0<HAuUvCtY8@��l<\RT<\?]A�d�<\ABZ0C%Y/F�#CER�[�CEd�o/P/AB@:?/e K k �vABZ0<���<\Z3V�n]@DCEP/R�RT<�FBo�CE?0FT@:�/gD<f�#CER�d�C�n%@:?/e¡ATC�U�VXRTY/FtABZ0<qo3VXABZ �WZ3VEF�V[�CEd�o3VXR^VX�/gD< ol<\R � CERBd�VX?0[�< VXA�FBd�VXg:g{?0CE@DFT<GgD<\n�<\gDF�@:?+ABZ0<GolChFB@:AB@DCE? Y0VXA^V87 � Z0<\?+ABZ0< ?0CE@DFT<RT<�VE[TZ0<�F�Z/@:ehZ0<\R�gD<\n�<\gDF�bN<\ABZ0C]Y�6�Z3VEF��l<\ABAT<\R�ol<\R �#CERBd�VX?0[�<ABZ3VX?Sb+<\ABZ0C%Y 9��8U�Z/@D[TZ+AT<\?0Y/FATCq��<�[�CEd�<�P/?0FBA^VX�/gD<�VXAZ/@:ehZ_?0CE@DF^< gD<\n�<\gDF�7�bN<\ABZ0C]Y¡6tZ3VEFG?0CEAFBZ0C�U�? VX?|s FB@:eh?0FCX��@:?��FBA^VX�/@:g:@:A�sN<\n�<\? VXA�n�<\RBs�Z/@:ehZN?0CE@DFT< gD<\n�<\gDF�7�O�Z/@DF-@DF�RT< �]P/@:RT<�Y��#RTCEd V�e�C%C%Y�o3VXABZ ABR^VE[Tx%@:?/eVXg:e�CERB@:ABZ/d�7

O�Z0<�CEo/AB@:d�VXg"F^<\ABAB@:?/e�Fc� CERmABZ0<�wQC%CEx�y�Z0<�VEY�;�@DFBA^VX?0[�<���VX?0Y�ABZ0< � � nhVXg:P0<�RT<�FBol<�[\AB@:n�<\g:s @DFvVABR^VEY/<���CX����<\AuUv<�<\?tABZ0<�VX�/@:g:@:A�s�ATC �]P/@D[Tx%g:s RT<\ABP/RB?�ATCHABZ0<�o3VXABZtVX?0Y�ABZ0<�F^<\?0FB@:AB@:n]@:Aus ATCHZ/@:ehZ?0CE@DFT<�@:?qABZ0<�o�ChFT@:AB@DCE?qnEVXg:P0<�F�7rO�Z0<H<�/VE[\A�?3VXABP/RT<CX�{ABZ0<�FT<HRT<\g�VXAB@DCE?0F�U�@:g:g���<�@:?|n�<�FTAB@:e|VXAT<�Y��P/RBABZ0<\R�@:?�CEP/R���P/ABP/RT<HRT<�F^<�VXRT[TZQ7

�"� � � ���S�N��� �r � �f$ ��S� �

� � ��� � ��¨ WA�4© �¢�.~��4�¢�{�

O�Z/@DF�UvCERBx�U�VEF��/P/?0Y/<�Yf�]s�������� �>* �i�$�� �#+' �1�(�2.3 �'+VX?0Y�� ,$,� �! P/?0Y/<\R�y�UWVXRTYq��C/76h�h�hI �u�h��~h~h~%7

� � ��N�G� �"�

� �4� ���y�4� �x�4�

� ��������� ��������������������� �!��" #%$'&)(+*-,/.0(�121435.768359+3�14(;:�<=3�*-,?>�*A@CB5<D:�143�<=3�*-,E>+,/FG(+*CH5(�.JIK>�,2LIM14>�*-*NF2*PO7>�*A@RQ�(+1214(�S%F2*POJF2*C>�*7TVUW,E(+*�(+<X(+U�Y7Z[.�\�>+*^]N3�L�FG_�1435$a`cb-dWe��;fg���gdh-�+i;eji0kemlonpl�q`Db-n4r!e���lon4itsRu�v%w�sWdWbexsgq-ycu+rge�z�{Nex�D|��g�-�g$

�~}�u!h-�!|+=�R���V��� ������"���������[$�B5<D:�1435<X3�*-,E>+,/FG(+*�(oH�,2L-3?IMUW.03?IMUW.�Y�UWF2,jIK>�,2L��.0>!_;��F2*PO�TV1 OP(+�.�F2,�L�<8$8��ex��kb-n��x���%�Ke��Aug�0i�}M�7`K�E�K���E�K�����P|��E��!qA�Ku!{Nugi0np��lj�Eb-loi0n4i0hi0egq�}M���;be�f!n4e8�Je����4u!b`Db-n4r!e���lon4itsgqW�)n4ioi�lo{-h�0f!k�q�� ¡qN¢!�gbPhN���;sJ�x�g�P|W$

� £ce��p�!|�¥¤a� "�¦D��§7¨���P��§5����© ¦¡$�TVUW,E(+*A(�<=(�U�Y�ª�>+9mF«OP>+,/FG(+*JH5(+.XQ (�.03�Y�,V¬­>g_�L�F2*A35Y�$J�)�;e5�Elti;h-dWsgq`Db-n4r!e���lon4itsRu�v�`Dz=ex®-q-��h-fX|g�g�P|W$

� �Kn�b-�!¯+±°��!�²�¡�2# ��³�� ���-$AIK>�,2L²�W.;>g_��+F2*�O8>�*A@R´K\5Y�,�>!_51�3�TV9+(+FG@!>+*�_;3[H5(�.MQ (�.035Y�, ¬­>g_�L�F2*A35Y�$A�­�glo�i0ex�xµ lMi0k-exl0n�lxq-`Db-n4r!e���lon4itsXu�v%`Dz=ex®-q-���-�K|g�g�!¯$

� l0u�vG�!¯+¶§m" ·P��¸A����·���[$ Tj\�(+UW,º¹�F2*�3�Y»>+*�@»6�FpY�,�>�*A_�3¼,E(½>¾¹�F2*A3�q¿|��À Dhf!h-loi¼|g�g�g¯-qÁWÂ�Â!Ã�Ä0Å�ÅgÆWÇ�È�É�Ç!ÂPÊ�ËÌ�Í�ÆÈgÊ�ÎxÂPÁ�É�Ï�Ð;ÑgÈxÉ�ÅgÒ!ÊAÑmÁ�ÎxÓÇWÅgÌWÍ�ÆÈ�Ê�Î�ÂgÁgÉ�ÔPÕNÖ�Õ�×PÅ $

� `V�M�!Ø+�Ù��VÚ8�P����ÛN��� #�³Ü� ���a" ���#�§5����2#�$Ý]�Q[ÞXßDà­ác3�1âFG>g\�143�´D\5Y�,�>!_5143ãTV9�(�FG@g>�*A_�3�H5(�.äQ >+Y�,¬­(g\�F2143äáV(g\�(+,åY�$æB;ç)ç)çèB�*-,Eéj&)(+*mH5éV(�*Các(g\�(�,åFG_5Yã>+*�@JTVUW,�(�<=>�,åFG(�*-q%�-�gfgexlä�mêPë�|mì��mêPëgë�q�7��s7�m�g�!Ø$

�0� � £-���t����� �r� �N�� �

� � �d�{�A� � WA�4� � ���4©

K O�Z0<Hn�<\Z/@D[\gD< � F-FBAT<�<\RB@:?/e�VX?/ehgD<K d O�Z0<HRT<�[�CERTY/<�Y�FTAT<�<\RB@:?/e�VX?/ehgD<j �-RB@D<\?|A^VXAB@DCE? CX��ABZ0<�n�<\Z/@D[\gD<j(d O�Z0<HRT<�[�CERTY/<�Y�CERB@D<\?]A^VXAB@DCE?� B�<�VEY8@:?/e�CX�{ABZ0<n�<\Z/@D[\gD<� y [�CE?0FBA^VX?|A�� �������� �5P0FT<�Yf@:?SbN<\ABZ0C%YqCE?0< O�Z0<[\gDChFT<�FTA�Y8@DFBA^VX?0[�<HATC�ABZ0<�o3VXABZ� wQC%CExty-Z0<�VEY�y-?/ehgD<� wQC%CExty-Z0<�VEY�;�@DFBA^VX?0[�<� � A^VX?0Y0VXRTYSY/<\n%@�VXAB@DCE?SCX��ABZ0<H?0CE@DFT<

� ��)])|� � �51¡¤r)]'5,�� � �"�5��.��K3k O�P/RB?SATC�U�VXRTY/F�ABZ0<�RT<�[�CERTY/<�Y+CERB@D<\?|A^VXAB@DCE?K/a b+@:d�@D[HABZ0<HRT<�[�CERTY/<�Y�FTAT<�<\RB@:?/e�VX?/ehgD<h7K k bNC�n�<HATC(UWVXRTY/F-ABZ0<Ho3VXABZQ7Kim �/P0F^<�YqABP/RB?/@:?/e��l<\Z3V�n%@DCEP/R a�ABZ0<FBP/d CX��ABZ0<GVX?/ehgD<�F�VX�lC�n�< g