0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj...

24

Transcript of 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj...

Page 1: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

�������������� �������������������� �"!#�$�&%'�)('�$�*�,+-�&%/. �&0��$��12�&03�4(��

57698;:=<�>@?�ACBD�EGFIHGJ'K�LNMOMOM

P�QSRUT�VXW�YZT

[Z\^]`_7a�bdc�egf-hihic�j�_�_kj�_mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~}��$�9��y�wk�d|C�Cy-�^��[Z\^f-l]�_���lk\ij3l�fd_ke�bvuf-�^l�b=rxfdlk]���fdp`pq���q}{��w�N���v�gf�rxf-n�bvu)lk\ijSj��G��]�ckb=�^r�j��Glma�\^]��\g]`_~�N_�jsu��^p)u�b=c7r�bdo^]`pqjc�bdo9bdl��^f���]��=f-l�]qb=��fd�ih�p�bv�sfdp`]q�sf-l�]qb=���[Z\^j~j$ �n^p�bdc{f-l�]qb=��r�j��\Nf-�^]`_kr¡]`_*]��vlkj��ih^j�h�l�bo9j¢fdnin^p`]q�sf-o^p�j)lkb�fd�7f-c�o^]�l�c{f-ck��j��v��]�ckb=�^r�j��Gl��Gf-�^h7]�_Z]��ihij�nNj��ihij��GlZb=uOlk\ij¢n^f-c�lk]����^p`f-cc�j�nic�j�_kj��Glkfdlk]�bd��bvu�l�\^ja�bdc{p�h���£3j�l�f-edj�f-�"]q�Nu�bdc{r�fdlk]�bd��¤¥l�\^j�bdc�j�l�]q�f-nin^ckbvf-�\�f-�^hf��dbv]qhl�\^j*�^_kj&b=u¦fdcko^]�l�c{f-ck�@\^j��ic{]�_klk]���_���§¨c�j�p`]�rx]��^f-ck�@c�j�_k�^p�lk_�fdckj*nickjs_�j��Gl�j�h©fd�ih©a;jh^]`_����^_�_�u��il��ic�j�h^]�ckj���l�]qb=�^_�u�bdc�]��G�dj�_klk]��=fdlk]�bd���

Page 2: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

����� T�� � T'R

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

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

,-����#/.0��� ��1324� ��5 67 �98;:�n^nickbvf-�\ijs_Z]q�=<;b=r�ni�^lkf-l�]qb=�^fdp0> j�b=r�j�lkc��������¢���4�����4���4���4��� ?7 �A@ [�bdn9b=p�bd�=]���f=pCB�j�nic�j�_kj��Gl�f-lk]�bd�N_�fd�ihD:��ilkbd�ibvr�bd�^_FESfdnD<;b=�^_kl�c��i��l�]qb=� G7 � 7IH �G�dj�c�_kj¢§�ckb=o^p�j�r [Z\ij�bdck�7f-�ihDJZfs�=js_k]`f-�K:��^f=p �i_�]�_��4�����4���4���4��� G7 � L ESfdn�¤M<;b=�^_kl�c��i��l�]qb=�@]��lk\ij&§¨c�j�_kj��^��j¢b=uON��i��j�c�lkfd]��Gl¥�������4���4���4���(8�P7 �A?IQX �n^p�bdc{f-l�]qb=�����4�����4���¢���4�����4���4���4�����¢���4�����4���4���4���(8�@

�SRK#T. #&"��!U����VW)YX[Z0#&� ��\]� ^`_ba�U�"$� ��)*����� �&,L^�98;:cJZfs�=js_k]`f-�d:�ninic�b=fd�\ �¢���4�����4���4���4�����¢���4�����4���4���4���(8 7L^�A@I:�h^h^]q�^�~f-�e:���l�]qb=��¤f<;b=_klgESj�fd_k�ic�j ���4���4�����¢���4�����4���4���4���(8�?

6�����#&"h�$%i�h0)j��\lkm#&1��O"n��1 �*6?��98-oG]`r&�^p`f-l�]qb=� �4���4�����4���¢���4�����4���4���4�����¢���4�����4���4���4���(8�??��A@I:��©]��^_klkf-�Gl�]�fdlk]�bd�b=uOprq�s t u�vfwyx ���4���4���4�����¢���4�����4���4���4���(8zG

{-|��0������#=Re�$��#&������� �1 �&}

~S� � ��/"h�01��$�C01 ���

@

Page 3: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

��� R�T ����� ���� V �)R

8 :�n9b=pq�G�=bd�a�]qlk\©]q�^_k��c{]�oNj�h_k\ibdc�l�js_�lZaZf-lk�\^rxf-�ckb=�il�j����4���4���4��� ?@ [Z\ij&_knNj���lkck�^r b=uUj��G��]qc�bd�^r�j��vlZc�j�nic�j�_kj��Gl�f-lk]�bd�N_������4�����4���4���4��� 7 E©b=�vlkjb<�f-c{pqb4_k]`r&�Np�fdlk]�bd��b=u9f c�bdo9bdl'�i�ih^j�c��db=]��i�4f-�x�i�i��j�c�lkf=]q�xr�bdlk]�bd�U� 8�PL oG]`r&�^p`f-l�]qb=��ckjs_��^p�lk_��g�4���¢���4�����4���4���4�����¢���4�����4���4���4���(8��? c{]q�v]q�^f=pO_���j��ij&fd�ih_����Gl�\ijs_k]���j�hp`f-�ihNr�fdcke�_��Z���¢���4�����4���4���4���(8zG� �UfdoNb=c�fdl�b=c��~_k��j��^j&f-�ihnic�bdo^fdo^]�p`]�l¥�7hij��N_k]�l¥�@u��i�i��lk]�bd��b=u�c�bdo9bdlZnNbv_�j*�(8��

7

Page 4: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

� � � T'V ��� YZT � � �

H �gbdc�hij�c~u�b=c7f-��fd�il�b=�ib=r�bd�N_xckb=oNb=l�lkb�bdn9j�c{f-lkjS]q��]�lk_~j��G�G]�c�bd�^r�j��Gl���]�lmckj��G�^]qc�j�_eG�ib�a�p�j�hi�=jbvu4]ql�_~lkf=_�e"f-�ih�\^b�a ]�l~��fd�goNj�f-����b=r�n^p`]�_k\ij�h�� :)��j�_�_�j��vl�]�f=p�fd_kn9j���lb=u�lk\^]�_�eG�ib�a�p�j�hi�=j7]�_�f��m�{�N��[Z\^f-l�]�_��X]q�Nu�bdc{r�fdlk]�bd��a�\^]��\�j��^f-o^p�j�_*l�\ij@ckb=oNb=l&l�bj�_klk]`rxf-lkj7a�\ij�ckj]qlx]�_��Xa�\ij�ckj]ql�� _��dbv]q�^�^�Xf-�ih�\ib�a2l�b��dj�l*l�\^j�c�jd� [Ub3h^f-lkjd��rxf-�v�c�j�_kj�f-c��\ij�c�_�\Nf��djhij��=bdl�j�h�f�_k]��d�N]i�sf-�Gl�f=r�bd�i�Gl�b=u�a�bdc�e l�b _�bvpq��]��i��l�\ijs_�jl�\ic�j�jlkf=_�e�_����v]q�=j���f©nic{]�bdc�rxfdn�� @/@G� 7 G� 8���� 7/7� �y: u�j�a�\Nf��djmlkfd�e�pqj�h�l�\ij~n^ckb=o^pqjsr bvu��b=�^_kl�c��i��l�]q�i�©f�rxf-n�_kj�rx] ¤ f-�^l�bd�^b=r�bd�^_�pq��� L&Gd��@�L �'fd�ih�_kb=r�jx\^f��=jxfdl�lkj�r�nil�j�h�l�\ijrxf-n7fd���G�^]`_k]�lk]�bd�mn^ckb=o^pqjsr2]q�7f�u��^p`p �7fd�il�b=�ib=r�bd�N_�_kj�l�l�]q�^��� @�P�� G� 7 8 � ¢uOl�\^j p`f-l�lkj�ca�bdc�eG_��sl�\ij�rxf�� b=c�]�l¥�¢��b=rxrx]ql'l�b4f�nNf-c�lk]����^p`f-c¦_�j��^_�]q�i�¢r�bvhNfdp`]ql¥� qC_kbd�Nf-c x f-�^h�fd_�_k�^r�jl�\Nf-l�l�\^j�a;b=c�p�h7�sf-�oNj�c�j�nic�j�_kj��Glkj�h�fd_�fr@�¤���n^]�j���j�a�]�_kj$¤ p`]q�ijsf-c�rxf-n��£3j�a�]`p`pOhij�i�^j¢j� �n^pqb=c�fdlk]�bd�7f=_�l�\ij�nic�bG��js_k_;b=uUh^]�_k��b��=j�c{]q�^�*l�\^b=_kj¢f=_�n9j���lk_Zb=uUl�\ij

a�bdc{pqh�l�\^fdl¦f=p�p�b�a��^_¦lkb4ckjsp�]`f-o^pq��r�bGhij�p�]�lk_X_kl�c��i��l��ic�j�f-�ih���bdc'oNj�\^f���]qb=�ics� �U]�edj�_k��]�j���¤lk]�i�&]��G�dj�_klk]��=fdlk]�bd���Gj� GnNpqb=c�fdlk]�bd�@]���l�\ij�c�j�f=p9a;b=c�p�h@�^j��dj�cZl�j�c{rx]q�Nf-l�js_����)b�a;j��dj�c���u�bdcnic{f-��lk]���fdp=c�j�fd_kbd�N_���lk\ijZckb=oNb=l��sf-�*��j�f=_�j�j$ �n^p�bdc{f-l�]qb=�&a�\ij��x]�l'\Nfd_¦f-���v�N]qc�j�h�_����x�s]qj��vl]��^u�bdc{rxf-lk]�bd�l�bxn9j�c{u�bdc{r ]�lk_�l�fd_keG_Za�]�l�\©f���j�ckl�fd]��r�jsfd_k�ickj4bvu���b=��ihij��i��j=�

�ic~a�bdc�e fd]`rx_~lkb�hij��=j�p�bdn�f�u�c�f=r�j�a�bdc�e u�bdc7fd�il�b=�ib=r�b=�^_mj$ �n^p�bdc{f-l�]qb=��lk\^f-l]`_*o9bdlk\�hib=rxf=]q��¤�f-�^h _�j��^_kbdck¤¥]��ihij�nNj��ihij��Gl�� £�j@f-����bvr�nNp�]`_�\ l�\^]`_*l�fd_ke o=��n9b=_�]q�^�l�\^j�n^ckb=o^pqjsr fd_¢bd�ij�b=u�bdnil�]�rx]���]��i�l�\ijxc�bdo9bdl�� _ ev�^b�a�pqj�hi�dj�bdcx�����$y-w��x�d|C�Cy-� f-o9bd�ill�\^j3a�bdc{pqhU��E©bv_�l@nic�]�bdc@a;b=ckeg]�� l�\N]�_hibvr�f=]q��]�_@��b=�^_�lkc�f=]q�^j�h lkb�_kbd�^fdc@_kj��^_�]q�^�bdcs�Ua�\^j�c�jm�G]`_�]qb=��_�j��^_�b=c�_�fdckjx�^_kj�hU�¦f�c�j�_kl�c{]���l�j�h ��p`fd_�_�b=uZr�bGhij�p`_�� 7 7 ��L G� 8�P���?*8-�@ ��£3jx_kj�j�e©lkb@�=j��ij�c�f=p�]���j*l�\ijxr�bvh^j�p`_ jsr�n^pqb��vj�h�� u�fd��]`p�]�lkfdlk]��i�f7a�]qh^j�c4hib=rxf=]q�Sbvuj��G��]�ckb=�^r�j��Glk_��9fd�ih3f=]�r lkbfdhihic�j�_�_�l�\^j�b=nNj����G�ij�_klk]�bd�N_)n9b=_kj�h3o=�©b=l�\ij�c¢j��Gl�c�bdnv�G¤r�bdl�]q��f-lkj�h�f-n^nickbvf-�\ijs_¢l�b©j$ �n^p�bdc{f-l�]qb=���m[Z\ij~nic{]��i��]�n^p�jm��bd�Glkc�]�oi�il�]qb=�3bvu�lk\^]�_&a;b=ckea�]`p�pOo9j�f@|! G}{y�w�"7y#�&}��$�O�qy�wk�d|C�Cy-�Sa�\^]��\fd����b=rxr�bvhNf-l�js_Zr&�^p�lk]�n^p�j¢\v��n9bdl�\^j�_kj�_Zf-o9bd�ill�\^j*��b=ckc�j���l�c�j�nic�j�_kj��Glkfdlk]�bd�©bvu¦l�\ij�a;b=c�p�hSf-�^hSlkfdedj�_¢]��vlkb@fd����b=�i�Gl�l�\^j��i�^��j�c�lkf=]q�Gl¥�fd_�_kbv�s]�fdl�j�h7a�]�l�\�l�\ij�c�bdo9bdl�� _�fd��l�]qb=�^_��[Z\ijxo^fdp`f-�^��jxb=u�l�\^]`_4n^f-n9j�c�a�]`p`p¦h^]q��]�hij�h fd_�u�bvp�p�b�a�_�$&%¦]qc{_kl���a�jxa�]�p`p¦hij�^�ijml�\ij

nic�bdo^p�j�r�Glkfde�]q�i�x]��vlkb�fd����bd�i�Gl�lk\ij'�G�ij�_klk]�bd�@b=uU_kj��N_�b=c�fd�ih7bGhibvr�j�l�c{]q�¢�i�i��j�c�lkf=]q�Gl¥�=�[Z\ij nickb=o^p�j�r2_�l�f-lkj�r�j��Gl�a�]�p`p9oNj¢u�b=p`pqb�a�j�h~ov�~f&c�j���]�j�a�bvuOnic�]�bdc�a�bdc�e�b=�~l�\ij¢j$ �n^p�b-¤c{f-lk]�bd�xnic�bdoNpqjsr@�'£�j)a�]`p�p^l�\ij��~nic�j�_kj��GlXbd�ic;f-ninic�b=fd�\xl�b�l�\^j)nic�bdo^p�j�r�vf-�ihmnic�b���]qh^j_kb=r�j�nic�j�p`]�rx]��^f-ck��c�j�_k�^p�lk_��(%¦]��^fdp`p �v�=a�j�a�]`p�pNpqbGb=e�fdl�u��il��^ckj�hN]qc�j���lk]�bd�^_¨u�b=c�lk\ij�a�bdc�e9�

) * V � Q�+ �&, -mT'W4T��., � � T

�icx�dbvfdpZ]`_�lkb hij��=j�p�bdn�f"wky=z0/21$|�f-�ih�}!354��C}$�9|�r�j�lk\ibGh�u�bdcmf-�ilkbd�ibvr�bd�^_�j$ �n^p�b-¤c{f-lk]�bd��b=u9f-�mfdcko^]�l�c{f-ck�=���^�ieG�ib�a���j��G�G]�c�bd�^r�j��Gl��'[Z\ij�l�fd_ke*]`_Xc�]`u�j�a�]�l�\x�i�i��j�c�lkfd]��Gl¥�76bGhib=r�j�l�ck�mfd�ih@_kj��N_�b=c;c�j�fdh^]q�^�=_�f-c�j �i�^ckjsp�]`f-o^p�j4f-�ih@\ij��i��j a�j4r&�^_klZlkf-e=j)lk\ij�_kj�u�f-�$¤l�b=c�_�]��Gl�bSfd����b=�i�Gl�a�\ij�� n^p`f-�^�^]q�^�©a�\ij�c�jml�bSr�b��djmlk\ij~f-�=j��Gl�� H ��l�\^]`_*_kj��^_kjd��bd�ic�dbvfdp;]�_�l�b�rxf� i]�rx]���jbd�ic���j�ckl�fd]��Gl¥� fdoNb=�il�lk\ija;b=c�p�h��8��b�a�j��dj�c��¦lk\ij@n^ckb=o^pqjsr ]�_u��ic�l�\ij�cm��bvr�n9bd�^�ihij�h"ov� bdlk\ij�c~u�fd��lkbdc{_���_��i�\gfd_xl�fd_kev¤¥_knNj���]�i�ckj��v�N]qc�j�r�j��Gl�_dq u�bdcj$ if=r�nNpqj=�O�)�Nw�y�w�¦�ij�j�h^_�u�b=c;\N]q�=\ij�c�fd�����ic�fd�$�x]q�~��j�c�lkf=]q�mn^fdckl�_¨bvuNlk\ij)a�bdc{p�h x{�G_kf=u�j�l¥�]`_k_k�ij�_��^lk\ij*p`]�rx]�lkfdlk]�bd�^_¢]�r�n9b=_kj�h©ov�©f-�v�@n^fdckl�]q���^p`f-c��\ibv]q��j&b=u¦r�bGhij�pUb=u'l�\^j*a�bdc{pqh��f-�^h9�G�ijs_�l�]qb=�^_4b=u;��bvr�ni�ilkfdlk]�bd�^f=p¦l�c{f-��lkf-oN]�p`]ql¥�v�:%¦]q�Nfdp`p �v��]���p�]��d\Gl4b=u;bd�ic*hij�i�N]ql�]qb=�

L

Page 5: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

b=u�j$ �n^p�bdc{f-lk]�bd�U�Xa;ja�]�p`pZfdp`_�b3ckj��G�^]qc�j94�w� |¥}$w�C� lk\^f-lxfdp`p�b�a��^_xl�b�hij�lkj�c{rx]q�ija�\ij��j$ �n^p�bdc{f-l�]qb=�7�sf-��oNj�lkj�c{rx]q�^fdl�j�h�� H l�_k\ibd�^p�h�oNj��spqjsf-c�lk\^f-l)fd�v�7ckb=oi�^_kl�f-�ih�j��x�s]qj��vl_kb=p��ilk]�bd�a�]�p`p c�j��G�^]�ckj¢��fdckjsu��^pO��b=�^_�]qhij�c�fdlk]�bd�@b=u'fdp`pOb=u�l�\ijs_�j�]`_k_k�ij�_��

� * V ��� � � R�� � V��

H �"l�\^]`_m_�j���l�]qb=��a�j@��bd�^_�]�hij�cxnic�j���]�bd�^_�a�bdc�e�lk\^f-lm]`_xckjspqj���f-�Gl�lkb l�\ij©j� GnNpqb=c�fdlk]�bd�nic�bdo^p�j�r�

��� �� � ������������������! "�$#% �&�'(�)'��*�$���,+.-/�0�$#%�1'(�(2§¨c�bdoNpqjsr�_�]��©��bvr�ni�ilkfdlk]�bd�^f=p �dj�b=r�j�lkc��Wq <F>�x�bvu�l�j��©nickb���]�hij&]��^_k]��d\Gl�fdoNb=�il�c�j�p`f-l�j�hc�j�fdpq¤¥a;b=c�p�h@l�fd_ke�_�� %^bdc�j� �f=r�n^pqj=�9_kbd�Nf-c)rxf-nN_�b=u¦lk\ij&c�j�f=p�a�bdc{pqh���fd� _kb=r�j�l�]�r�j�_)oNjf-n^nickb� i]`rxf-l�j�h�]q��lkj�c{rx_&b=uZn9b=pq���db=�^_��Xfd�ih�\ij��^��j7lk\ij7o9bGh���b=uZc�j�_kj�fdck�\���b=�i��j�ck��¤]��i� nNbvp ���db=�^_m]`_~]�rxr�j�hN]�fdl�j�pq�"fdnin^p`]q�sf-o^p�jd� :)��]�r�n9bdc�lkf-�Gl �G�ijs_�l�]qb=��lk\^f-lm�sf-�goNjf-�N_�a�j�c�j�h ]q� lk\ij~��b=�Gl�j$ �l�bvuF<F> ]`_&l�\Nf-l�b=u���b=r�n^p�j$ i]ql¥�v� H u�a�jmev�^b�a l�\Nf-l*f�n^fdc�¤lk]����Np�fdc <F>�nic�bdoNpqjsr/]`_)hN]�x���Npql¢l�b_�bvpq�=jd�^lk\ij��3a;j���fd�©��bd�^��p��ihij*lk\^f-l l�\ij�c�j�p`f-l�j�hc�j�fdpq¤¥a;b=c�p�h�n^ckb=o^pqjsr2]�_�f-l�p�j�f=_�l�fd_�\^f-c�h�� %i�ic�l�\^j�c{r�b=ckj=�=]`u l�\^j)a�bdc{pqhm�sf-�mo9j j� �fd��l�p �j$ �nic�j�_�_kj�h�]q��lkj�c{rx_�b=u��dj�b=r�j�lkc��v��lk\ij���lk\ij~j� �fd��l*_kb=p��il�]qb=��lkb f�n^fdckl�]q���^p�fdc&nic�bdo�¤p�j�r2c�j�nic�j�_kj��Glk_�f�pqb�a�j�c�oNb=�i�ihbd�@a�\^f-l�r&�N_�l�oNj�fd����bvr�n^p�]`_k\ij�h]q�l�\ij¢nic�j�_kj��i��j bvu�i�i��j�c�lkf=]q�Gl¥�=��n^ckb���]�h^]q�^���N_�a�]qlk\Sfmr�j�fd_k�ic�j4o=�a�\^]��\lkbmj���fdp��^fdl�j&fdp��db=c�]�l�\Nr�_Zlk\^f-llkfd�e�pqj&�^�i��j�ckl�fd]��vl¥�v��£3j ic{_�l ��bd�N_k]�hij�c)l�\ij43��d| 40 �m���65)y�/i|¥}87Zwkydz��q}����Oa�\ibv_�j*_kb-¤p��ilk]�bd�mc�j�nic�j�_kj��Gl�_Xl�\ij r�]��^]`r&�^r�h^]`_klkf-�^��j)f�c�bdo9bdl;r&�^_kl¨l�c{f��djspi]��mbdc�hij�c¨lkb&h^]`_���b��dj�cl�\^j�a;b=c�p�h���fd_Zc�j�nic�j�_kj��Glkj�ho=�f�n9b=pq���db=���

S

%¦]q�=�ic�j�8 $ :�nNbvp ���db=�@a�]�l�\©]��^_k��c{]qo9j�h©_k\ibdc�l�js_�l�a�fdl��\^rxfd�@c�bd�ilkj�u�bdc�_klkf-c�lk]��i�xn9b=]��vlo9�^[Z\^j¢c�bd�^l�j�]�_)r�fdcke=j�h@o=�@l�\ij�o9b=p�h©p�]��ijd�[Z\ij93©�-| 40 �m���:5)y�/i|¥}�7�wkydz��q}��I]�_�c�j�p`f-l�j�h&l�b l�\ij�; w$|,<��-���q}�w�"�7�wky=z��q}�� � @ G� 7 ? �

<;\^]��"fd�ih�=�lkf=u�b=_xhij�i�ijSf?>��-| 4� 7�x�-� wky /i| }Su�bdcmf�nNbvp ���db=� p fd_mf3��p�b=_kj�h"a�f=pqe]�� p¡_k�i�\gl�\Nf-l~j��dj�ck��n9b=]��Gl~b=u p ]�_7��]�_�]qoNpqjSu�c�b=r _�bvr�j©nNbv]q�Gl7]���l�\ij a�f=pqe � G �

?

Page 6: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

>¢]��dj���f7_klkfdckl�]q�i�n9b=]��vl o9� f-�^h3f@nNbvp ���db=� §¦�Ol�\ijxaZf-lk�\^rxf-�Sc�bd�ilkj�n^ckb=o^pqjsr ]�_¢l�bi�ih"f3_k\ib=cklkj�_kl�aZf-lk�\^rxf-��c�bd�ilkj@u�b=cx§ l�\Nf-lx_�l�f-c�lk_xf-l o q %']��d�^ckjY8�x�l<;\^]���f-�^h=�l�fdu�bv_Xhij�r�b=�^_�lkc�fdl�j�lk\^f-l¨lk\ij)n^ckb=o^pqjsr ]`_�=)§¨¤ \Nf-c�hx]`u9l�\ij �=]��dj���n9b=pq���db=��\^f=_¨\ibvpqjs_����b�a�j��=j�cs��l�\ijZnic�bdo^p�j�r�]`_Xr&�^�\�_�]�r�n^p�j�cXu�b=c'n9b=pq���db=�^_'a�]qlk\ibd�il¦\ibvpqjs_��-fd�ih <;\^]q�xf-�^h=�l�fdu�bv_Xhij��=j�p�bdn~fd� � q���x¨fdp��db=c�]�l�\^r u�bdc�fd�mbdc�l�\ib=�db=�^fdpGn9b=pq���dbd� � G � H �mb=l�\ij�c¨a�bdc�e9�l�\^j$�©nic�b��G]�hij�f-� � q�����x�f=pq�=bdc{]qlk\^r u�bdc4_k]`r�n^pqj�nNbvp ���=bd�^_ � � �[<�f-c{p`_k_kbd���¦}�|��d�X\^f��=jhij��dj�p�bdn9j�h�f-� � q����zx�fdp��dbdc{]�l�\^rx_Zu�bdc�_kb=p���]q�i�xlk\ij�nic�bdo^p�j�r�a�\^j�����]`_Z�i�^_kn9j���]�ij�h��

%ib=c¨l�\ij)ni�ic�nNbv_�js_¦b=uNj$ �n^p�bdc{f-lk]�bd��a�]�l�\~f�_�]q�i�vpqj�c�bdo9bdl��-bd�^j�]�_¨u�fd��j�h�a�]�l�\~_kb=p���]q�^�l�\^j*aZf-l��\Nr�fd�©ckb=�il�j*nickb=o^p�j�r ]q�Slk\ijxu�f-��j*b=u¦bd�Np ��n^f-c�lk]`fdpUeG�ib�a�pqj�hi�dj=�)[Z\ij�ckj*]`_)f��p`fd_�_ b=uX�^f���]��=fdlk]�bd� f=pq�=bdc{]qlk\^rx_��OeG�ib�a�� f=_ /d�Sf=pq�=bdc{]qlk\^rx_��Oa�\^]��\ h^j�fdp'a�]�l�\�l�\ijlkf=_�exbvu i�ih^]��i�xf&n^fdl�\u�ckbvr�f�_klkf-c�l�n9b=]��Gl;lkb�f*�dbvfdp9]q�f-�~j��v��]�ckb=�^r�j��Gl�n9bdn^�^p�fdl�j�ha�]�l�\*�^�ieG�ib�a��*b=o^_�l�f-�spqjs_ � @ ?G� @ � �O:� /d�=¤ _kl¥��p�j�j$ �n^p�bdc{f-lk]�bd��_�lkc�fdl�j��d�&]q�G�dbvpq�=j�_Ul�c{f��=¤j�p`]��i�*lk\ickb=�i�d\mlk\ij¢a;b=c�p�h��G_�jsf-c��\^]q�^�*u�bdc;bdo^_klkfd��p�j�_�f-�ih~�s]qc����Nr��Nf��G]��=fdlk]��i��bdoN_�l�f-��p�j�_fd_¦lk\ij$�xf-c�jZj��i��bd�i�Gl�j�ckj�h��¦[�fs�ip�bdcXf-�ih �4c{]qj��=rxf-��j�r�n^p�b���_k�i�\mf-�mf-n^nickbvf-�\���o^�^]�p�h�¤]��i�*f&_kj�l;b=u p�bv�sfdpNr�fdn^_¨oNfd_kj�hmbd�~f�_�j�l¨b=uO��]`_k]�o^p�j*�q�-�����m��w�� 1 � L � � ov�i�\3| y��yd�qyk�d�!4{�-�c�j�nic�j�_kj��Glkfdlk]�bd�^_;b=u�l�\^j�a;b=c�p�h@fdckj4��b=�^_�]qhij�ckj�h]��l�\ij��ij� �l�_�j���lk]�bd�U�: _�j���bd�^h��sp�f=_k_@b=u3<F> nic�bdo^p�j�rx_a�\^]��\ fdckj�c�j�p�j���f-�Gl�f-c�j <�}{y��m}�| w�!4"7Zwkydz��

���v��n^ckb=o^pqjsrx_�� > j�b=r�j�lkc�]��~nic�bdo^]��i�S��bd�^_�]�hij�c{_�nic�bdo^p�j�rx_*b=uZhij�l�j�c{rx]q�N]q�i� �dj�bvr�j�l ¤c{]q�7_kl�c��i��l��ic�j7u�c�b=r¡l�\ij@ckjs_��Npql�_&b=u�f3r�j�fd_k�ic{]q�^�©hij���]���j7b=c&�9wkydz{}�� H ��lk\ij7��bd�Gl�j� Glb=u¦j� �n^pqb=c�fdlk]�bd���N�dj�b=r�j�lkc�]��&nic�bdo^]��i�7]`_�c�j�p�j���fd�vl oNj���fd�^_�j*ckjs_��^p�lk_¢u�c�b=r/l�\ij�nic�bdo^p�j�rhibvr�f=]q�g_knNj���]`u`�gp�b�a;j�c�¤¥o9bd�i�ihN_�b=��lk\ijS�G�^r*oNj�cmb=u h^]�_k��c�j�lkjS_k]�l�js_ma�\^]��\�r&�^_klmoNj_�fdr�n^p�j�h�]���bdc�hij�c�l�b3��b=r�n^p�j�lkj�pq��c�j���bd�^_kl�c��i��l&l�\ij@j��G��]qc�bd�Nr�j��Gl�� H � rxfd�=� ��f=_�js_��l�\^j7�9wkydz����v�8�my��d}�� 1@c�j��ij���l�l�\ij�]�hij�f=p�]���j�h�oNj�\^f���]qb=�ic�b=uZc�j�fdpq¤¥a;b=c�p�h�_�j��^_�b=c�_��X_��^�\fd_4p`fd_kj�ck¤ c{f-�^�dj&n9b=]��Glk_�����f=p�]�nNj�c�_���b=c¢lk\ijmf-o^_kbdc�nil�]qb=� b=u���¤¥c�fs�i_���ove�]�j��^fnic�b���]qhijs_ foic�b=fdh@_k�ick�=j$�~bvuUlk\ij�a�]qh^j�c�fd�i�dj¢b=u�nic�bdo^]��i�xr�bGhijsp�_)f-�ihckjsp�fdl�j�h@c�j�_k�^p�lk_ � L�8 �

¢uO_k]��d�^]�i�sf-�Gl¨]q�Glkj�c�j�_kl¨]�_Xlk\ij����v�v}$wX�9wkydz{} r�bvh^j�p �v]��Gl�c�bvh^�i��j�h�ov�m<;b=p�j�f-�ih���fdn� � ��: i�i�dj�c4nickb=oNjxr�j�f=_��ic�j�_ l�\^j ^c�_kl n9b=]��Gl4b=u¨]��Gl�j�c{_kj���l�]qb=� o9j�l a�j�j��3f@h^]qc�j���l�j�hp`]q�ijSfd�ih�f-�gbdo�� j���l�� ov�^�\gf�r�bGhijsp�]�_~j��G�^]���f=pqj��Glml�b�f n9j�c{u�j���l@_�b=�^f-cmr�j�f=_��ic�j$¤r�j��Gl�bdc�p`fd_kj�ck¤ c{f-�^�dj�_�fdr�n^p�jd�K<;bvpqjmfd�ih���f-n _�\ib�a l�\^fdl 7 ��nickb=oNjs_�f-c�jm_����x�s]qj��vll�b���bvr�nNpqj�l�j�pq�~hij�l�j�c{rx]q�^j¢f���b=�v�=j$ xnNbvp ���=bd��� ��b�a�j��=j�cs�=lk\ij�]�c;n^ckb=o^]q�^��r�bGhij�pO]`_�]���¤_k���x��]�j��Gl�u�b=c;h^j�l�j�c�rx]��^]��i��fdckoN]qlkc�fdc��xnNbvp ���=bd�^_�� :)p��G]���bv_���}�|��-�Ur�bGh^]`u`�7lk\ij n^ckb=o^]q�^�r�bGhij�p;hij��=j�p�bdn9j�h�ov� <;b=p�j7f-�^h���f-n��¨f=p�p�b�a�]��i�©l�\ij@nic�bdo9j7l�b3u�b=p`pqb�a fd��f-c�o^]qlkc�fdc�����^ck�=jZfd�ihmc�j�l��^ck�^]��i���ib=l¨bd�Np ��l�\ij�n9b=]��Gl¨b=uO]��Gl�j�c{_kj���l�]qb=�xa�]qlk\ml�\ij�n9b=pq���db=��o^�il�fdp`_kbl�\^j�_k�ic{u�f-��j��ib=c�rxfdp'f-l¢l�\^j�n9b=]��Gl bvu¨]��vlkj�c{_�j���l�]qb=� �A8 � N��ihij�c4l�\^]`_4nic�bdo^]��i�@r�bGhijspC�l�\^jxfd�il�\^bdc{_¢_k\ib�aIlk\^f-l 7 ��� 7 nic�bdo9j�_4f-c�j�_k���x��]�j��Gl4l�b�nic�bdo9jmfnNbvp ���db=� a�]�l�\���ib=��¤ ��b=p`p�]��ijsf-c)j�hi�djs_�� ¢uXc�j�p`f-lkj�h ]��Gl�j�c�j�_kl ]`_ l�\ijxnic�bdo^p�j�r bvu¨�=}{y��m}�|Cw�!47| }01$|C���G�-�Obdcbdo�� j���lZ�=j�c{]i�sf-l�]qb=�@nic�bdoNpqjsr�$X�v]q�=j��fm_�j�l�b=u�b=o � j���lk_�fd�ih�fxlkf-c��dj�lZbdo � j���l�� ^�ih©l�\ijrx]q�N]�r*�^r _kj�l¦b=u^nic�bdo9j�_¦a�\^]��\�f=p�p�b�a�lk\ij�lkfdck�=j�l'l�b4oNj�hN]�_k��c{]�rx]��^f-lkj�hxu�ckbvrIfdp`pGbdlk\ij�cbdo�� j���l�_ ]�� lk\ijm_�j�l���B�bvr�fd�^]�e©nickb���]�hij�_ f~hij�lkf=]�p�j�h�_��ic��dj��©b=uXc�j�_k�^p�lk_¢]q�3�dj�bvr�j�l�c{]��l�js_�l�]q�^��� 7 @ �

%¦]q�^f=p�pq�=�!B�j�e�p�j�]�lk]`_�f-�ih � �ihij�e7\^f��=j¢h^j��djspqb=nNj�hfmr*�^p�lk]q¤ c�bdo9bdlZ��bvp�p`f-o9bdc{f-l�]q�=j¢f=p ¤�db=c�]�l�\Nr u�bdc)j$ �n^p�bdc{]q�i�mfm_�]�r�n^p�j&n9b=pq���dbd� � 7 8-� 7 P � H �©l�\^]`_�a�bdc�eO��l a;bmc�bdo9bdl�_���bvr*¤r�j��i��jSj$ �n^p�bdc{f-l�]qb=�go=�"nNbv_k]�lk]�bd�N]q�i� l�\ijsr�_kj�p��djs_7fdl~l a;b�fdh���f-��j��Gl~�=j�c�lk]���j�_~b=u¢l�\ijn9b=pq�G�=bd��f-�ih©_ka;j�j�n�l�\^j&u�ckj�j&_�n^fd��j�ov�@lkc�f��=j�p`]q�^��f=pqb=�i�xj�hi�=j�_�a�\^]`p�j&r�f=]q�Gl�fd]��^]q�^�xfp`]q�ij�b=uX_�]q�=\Gl��4[Z\ij*lkc�f��=j�c{_kf=p b=u¦lk\ij�j��Glk]�ckj&n9b=pq���db=�Sckjs_��Npql�_��ib=l b=�^p �S]�� f~h^j�_k��c{]qn�¤lk]�bd�7b=uOl�\^j)n9b=pq�G�=bd���=oi�ilZf�l�b=nNbvpqb=�=]���f=pic�j�nic�j�_kj��Glkfdlk]�bd�xa�\^]��\7]`_�o^f=_�j�hmbd�7f&n^p�fd�^f-c

Page 7: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

hij���b=r�n9b=_�]ql�]qb=�Yq u�bdc�j� �f=r�n^pqj=��l�c{]�fd�i�d�Np�fdlk]�bd� x�b=u�l�\^j�nNbvp ���db=���

���� �:�� �$+*���$��� ��+��4�0 ������ ��� '(� '��*� ���:����� �/&�'(�$� � # �$&��� �, �$��� '(��&�� '��*�$�

�ij3b=u4l�\^j3jsf-c{p�]�j�_kl~a�bdc�eG_~l�\^fdlf=]�rx_@l�b�fdhihic�j�_�_~lk\ij�fd�il�b=�ib=r�b=�^_~j� GnNpqb=c�fdlk]�bd�nic�bdo^p�j�r ]`_�l�\Nf-l)bvu �4�^]qn9j�c{_)fd�ihyJ;���i� � @j8-� @�P ��[Z\^f-l)a;b=cker�bGhijsp�_ l�\^j&a;b=c�p�hSfd_f©�=c�fdni\ j�r&o9j�h^hij�h ]�� f=@�¤ hN]�r�j��N_k]�bd�^f=pXj��G��]�ckb=�^r�j��Gl�nNb=ni�^p`f-lkj�h�a�]�l�\�n9b=]��Gl*f-�^hp`]q�ij�u�j�f-lk�ic�j�_��Z[Z\^j&�dbvfdp�]`_�lkb7f-�ilkb=rxf-l�]q�sfdp`p �j$ �l�c{f-��l)fxl�b=nNbvpqb=�=]���f=p ckj�nic�j�_kj��Glkfdlk]�bd�a�\ij�ckjx�dj�ckl�]q��j�_4f-c�jxpqbG��fdl�j�h�fdl&pqbG��f=p¦r�f- i]�rxfb=u;fr�jsfd_k�ickjxbvu ��� 1$| ����4�|C� ��}��9}01 1mb=�f7_k�io^_kj�l b=u¦lk\ij�_kj��N_�b=c���u�j�f-lk�ic�j�_��Ofd�ihS�=j�c�lk]���j�_)b=u¦lk\ij��=c�fdni\Sf-c�j&c�j���bd�d�N]q��j�h©ov��fp�bv�sfdp�1{�q�d�9�d|!/�w�}*bvu¦l�\ij�j��G��]�ckb=�^r�j��Gl��)[Z\^j*�dj�ckl�]q��j�_ f-c�j&��b=�i�ij���l�j�hSl�b~b=�ij�fd�ibdlk\ij�cov� f-c���_ a�\^]��\3hij�i�ij��qy 4��d��4{y��N| wkyd�(1| wk�d| }¥�d�C}�1xu�bdc4l�c{f��djsp�]��i�7o9j�l a�j�j�� lk\ijx�dj�c�lk]���js_��f-�^h �dj�cklkj$ ©c�j���b=�d�^]�lk]�bd�3]�_¢�dj�c�]�ij�hS��]�ff w�}� �}��-w 1��-�¨n^ckbG��j�hi�ic�jd�&[Z\^]`_4f-nin^ckbvf-�\©l�brxf-n�¤ rxf-e�]��i�S\^f=_�o9j�j�� hi�^n^p�]���fdl�j�h�_kb=r�j�a�\^fdl*ov�l<;\ibv_�j�l�� _3> j��ij�c{fdp`]q��j�h��¨b=ckb=�ib=]> c{f-ni\iq >�� >�x{�9a�\^]q�\Sf=p�_kbmhijsfdp`_�a�]�l�\Slk\ij*n^fdckl�]q���^p`f-c)]`_k_k�ijs_�b=u¦hij�i�^]��i�~lk\ij�p�bG��fdp��b=�Gl�c�b=pOr�j��\^fd�^]�_�rx_�u�bdc�r�b���]q�^�xu�c�b=r �dj�c�l�j� ~l�bm�=j�c�l�j$ 9� �:��ib=l�\ij�c¨l�b=nNbvpqb=�=]���f=pvfdninic�b=fd�\*l�\Nf-l¨]`_Xb=u^ckjspqj���fd�i��jZlkb�l�\ij�a�bdc�e�nic�j�_kj��Gl�j�h�\^j�c�j

]`_¨l�\Nf-l¨bvu [�fd�=f-c�jd�9}�|X�-��� L L � H �~l�\Nf-l¨a�bdc�eO�dl�\^j�a;b=c�p�h~]`_¨c�j�nic�j�_kj��Gl�j�hxl�bdn9b=p�bd�v]q�sfdp`p �fd_)fm_�j�l�b=u'n^p`f-��js_��^j�f-�\b=u'a�\^]��\©]�_)�\^f-c{f-��l�j�c�]���j�h7ov�fxn^fdckl�]q���^p`f-c���]�_k�^f=p�f-nin9j�fdc�¤f-�^��jd��[Z\ij&nic�bdoNpqjsr b=u¦p�bG��fdp`]���]��i�~]q�S_k�i�\©fxa�bdc{pqh©]`_�l�\^fdl�b=u�wk} 4{y��-�N���$���G��fxeG�ib�a��n^p`f-��j¢oNfd_kj�hbd�l�\ij����ickc�j��Gl�]��ini�il�]`rxf-�dj=�[Z\ij)fdhi��f-�Glkfd�djZb=uNj�r�n^p�b��i]q�i��f4lkbdn9b=p�bd�=]���f=pGckj�nic�j�_kj��Glkfdlk]�bd�x]�_Xlk\^f-l;]ql�_�]�hij$¤ _�lkj�n^_

l�\^j~h^]������^p�l&nic�bdo^p�j�r b=uZrxfd]��vl�fd]��^]��i�©l�\ij7ckb=oNb=l�� _4nNbv_�j7]���f-��fdo^_�bvpq�^l�jmbdc��vpqb=o^fdpc�j�u�j�c�j��^��j�u�c�f=r�j=� %i�^cklk\ij�c{r�bdc�jd��]�lm]q�Gl�c�bGhi�i��j�_xf pqj��dj�p�bvu�f-o^_kl�c{f-��lk]�bd��a�\^]q�\���fd�o9j¢jsr�n^pqb��vj�h7u�b=cZ\^]q�=\�¤ pqj��dj�p9]��^u�j�c�j��i��jrqCu�bdcZj� �f=r�n^pqj=�G�i�ihij�c�_klkfd�ih^]��i��lk\ij¢��b=rxrxf-�^h� > b lkb�l�\ij � � �����G� w�y�y �t��]�f lk\ij���� | 40 G}��^� �Tx �ij©h^]������^p�l¥�=�Z\ib�a�j��dj�c��;a�]qlk\�l�\ijf-n^nickbvf-�\ijs_Ul�f-e=j���o=���4�^]qn9j�c{_¦f-�ihrJ;���i�xf-�ih <;\ib=_kj�lX]`_¦l�\^fdlXl�\ij�c�bdo9bdl¦]`_Xu�bdc���j�h�l�bbdn9j�c{f-lkj�]q�xl�\^j���bd�Glkj$ �lXb=u^l�\ij�j� �l�c{f-��lkj�h�l�b=nNbvpqb=�-�=��a�\N]q�\mrxfs�*�ib=lXoNj�fdhij��G�^fdl�j�u�bdclkf=_�e�_�l�\Nf-lZckj��G�^]qc�j _kn9j���]`fdp`]���j�heG�ib�a�p�j�hi�=j)b=uUn^p`f-��js_�l�\^fdlZfdckj¢�ib=l�a�j�p`pq¤ c�j�nic�j�_kj��Glkj�hov�lk\ij&�=c�fdni\��boG]`r�\^bd�Sf-�^h��)�ih^j�e�ckjs_�bvpq�=j4l�\^]`_ ]`_�_��ij*o=��hij�i�^]��i�7f~_kj�l�bvu¦r�j�l�c{]q�rxf-n^_��^b=c&� 1$�q��� � 1my �&w�}�� �C�=z$� � � |!"©]��©l�\ij��ij�]��d\Go9bd�ic�\ibGbGh^_�b=u'h^]`_klk]��i��l�]q�=j&n^p`f-��js_���[Z\ijh^]`_�l�]q�^��lk]��dj3n^p`f-��js_7lk\ij�rx_kj�p��djs_@fdckj�_�jspqj���l�j�h bd� f r�j�f=_��^ckjSlk\^f-l��bvr&o^]��ijs_7l�fd_ke=¤_knNj���]�i��]q�^u�b=c�rxfdlk]�bd�a�]qlk\Sf �G�^fd�Glk]�lkf-l�]q�=j�r�jsfd_k�ic�j bvuU\ib�a�a;jsp�pOlk\ij�c�bdo9bdl�]�_)p�]�edjsp �l�b3oNjf-o^p�j7lkb3p�bG��fdp`]���j7]���l�\^j@�ijs]q�=\GoNb=�ick\^bvbGh��X_kj�p�j���lk]��i� lk\ij7o9j�_kl�_kj��^_kbdc�u�bdc�l�\ijlkf=_�e@]q��l�\^j��=]��dj��@�ij�]��d\Go9bd�ic�\ibGbGh��

%¦]q�=�ickjr@@_��^rxrxf-c{]���j�_ l�\^jx_kn9j���lkck�Nr b=uXc�j�nic�j�_kj��Gl�f-lk]�bd�N_�l�\Nf-l4a;j�\^f��=j*��b=�^_�]qh�¤j�c�j�hglk\v�N_@u�fdc��Zc{f-�i�v]q�i��u�c�b=r ni�ic�j�pq��r�j�l�c{]q�3r�fdn^_~lkb�ni�ic�j�pq�gl�bdn9b=p�bd�v]q�sfdp�rxf-nN_��<�p�j�f-c{pq�=�)fd�=���^_kj�u��Np4ckj�nickjs_�j��Glkf-l�]qb=��ckj��G�^]qc�j�_@o9bdl�\ r�j�l�c{]���fd�ih \^]��d\i¤¥p�j��=j�p¢]q�^u�b=c�¤rxf-l�]qb=��� �)�ih^j�e \Nfd_xnic�bdn9b=_kj�h"l�\^fdlml�\ijs_�j�c�j��G�^]�ckjsr�j��Glk_m]`r�nNbv_�j©f \^]�j�c{f-c��\v� bvuc�j�nic�j�_kj��Glkfdlk]�bd�^_;l�\Nf-l�]��i��c�j�f=_�j�]��@�=j��ij�c�f=p�]�l¥�7fd_Zb=�ij&fd_k��j��ih^_�l�\ij�\N]qj�c�fdck�\v�9� 8�L �

�� � � ���$����� ���6����� +*��# � � �0����2�������� �12 ��� �*��� �/� ��+ 2 � ���J�j�u�b=ckjxa�jxr�b��=j�b=�3lkb©l�\ijmn^ckb=o^pqjsrx_4b=u�r�fdn�¤¥oi�^]`pqh^]��i�Sfd�ih3j� �n^pqb=c�fdlk]�bd���Ua;j ic{_�lj$ if=r�]��ij4l�\^j�l�\ij�bdck�7l�\^fdl�r�bdlk]���f-lkj�_�r�b=_klZ���ic�ckj��GlZfdninic�b=fd�\ij�_;l�bmlk\ij�_kj�lkf=_�e�_��

G

Page 8: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

pure metric pure topological

%¦]��d�ic�j @ $X[Z\ij&_knNj���lkck�^r b=uUj��G��]qc�bd�^r�j��vlZc�j�nic�j�_kj��Gl�f-lk]�bd�N_��

£3j)��b=rxr�j��^��j�ov� ^c�_klXhij�^�^]q�^�*_�bvr�j��ibdl�f-lk]�bd� � lk\ij��iy 1�} u@bvu^lk\ij�c�bdo9bdlXc�j�nic�j$¤_kj��Glk_�lk\ij&�vpqb=o^fdp nNf-c{fdr�j�lkj�c{_Zl�\^fdl���fdnil��ic�j&]�lk_[qC��j�ckbd¤ lk\b=ckh^j�c x�_�l�f-l�j�]q�©lk\ij&a�bdc{pqh��[��GnN]q�sfdp`p �v� u ]`_�hij�i�^j�h"b��dj�cxf3��bd� i�d�ic{f-l�]qb=� _kn^f-��j���a�\^]q�\"hij�i�ij�_�lk\ij©_�j�l�bvuf-hNr�]`_�_k]�o^p�j�n9b=_kj�_�� %i�ic�l�\^j�c{r�b=ckj=� f-�"y=z�1�}�w ���d|C�Cy-� s�]�_¢hij�i�ij�h�f=_ lk\ij�bd�ilkni�il4b=u¨f_kj��^_kbdc�fd�ih©oNjspqb=�i�=_)l�b~lk\ij*_kn^f-��j�� bvuXf-h^rx]`_k_�]qoNpqj&_kj��^_kbdc)ckjsf-h^]��i�=_�� %ibdc)j$ ifdr�n^p�jd�sSrx]��d\Gl�ckj�nic�j�_kj��Gl4l�\ij~c{fd_kl�j�c�]���j�h�_kj�l�bvu��=c�fs�G¤ _��sfdp�j�nN]  �j�p�]q�Gl�j��^_�]ql¥����f=pq�^j�_&]���l�\ij]`r�fd�dj¢f-���G�^]�c�j�h~ov�7f&�sfdr�j�c{fi� %¦]q�Nfdp`p �v��a;j a�]�p`p9hij�i�^j4f�rxf-nmw ]��7lk\ij�f-o^_kl�c{f-��l;fd_f7r�j�l�\ibGh u�bdc �Nwk}����!4�| ���v� bdo^_kj�c���f-lk]�bd�N_�o^f=_�j�h bd�3l�\ij�n9b=_kj�bvuXl�\ij�c�bdo9bdl����i: rxfdna�]`p�p l¥��n^]���f=p�pq�7o9j���b=�in^p�j�ha�]qlk\�fx_kj�l�bvu�nic{]qb=c�bdo^_kj�c���f-l�]qb=�^_��[�fdc�fd�Gl�b=p`f�� _¨h^j�i�^]�lk]��dj4a�bdc�e�n^ckb���]�hij�_;bd�ic�u�b=�i�ih^fdlk]�bd�@u�b=c ��� ��}�w 1�}��9wkydz��q}��/|! G}��y�w�" � LT? � H �G�dj�c�_kjmnic�bdo^p�j�r l�\ij�bdck����bd�i��j�c��^_�lk\ij7nic�bdoNpqjsr bvu�]q�^u�j�ckj��i��j7o^fd_kj�h�b=�bdoN_�j�ck��f-l�]qb=���X£�j&nic�bv��j�j�h�N_k]��i�ml�\^j&j$ ifdr�n^p�j�b=u�c�bdo9bdl�p�bv�sfdp`]q�sf-l�]qb=����[Z\ij&]`rxf-�dj sl�\Nf-lZl�\ij4c�bdo9bdlZj��^��bd�^�vlkj�c{_;u�c�b=r nNbv_k]�lk]�bd�euS�sf-�oNj4j$ �nic�j�_�_kj�h7ov�7lk\ij4ckjsp�fdlk]�bd�^_k\^]�n

s��rq�u0x qM8�x

[Z\ij&p�bv�sfdp`]q�sf-l�]qb=�7nic�bdo^p�j�r ]�_�lk\ij���lk\^f-l�bvu�]��G�dj�c�lk]��i�3Q��G�^fdlk]�bd�Y8 $u���� � qhsTx qh@/x

f-�ih l�\G�^_�]q�^u�j�ckc{]��i� u�u�c�b=r si� [Z\ij�nic�bdo^p�j�r ]`_�� ��� ���iy 1�}�� ]q�Ilk\ij�_kj��N_�j�bvu�)fdh^fdrxfdckh���\^b�a;j��dj�cs��_k]��i��j�lk\ij�c�j�f-c�j;�^b¢�=�^f-c{f-�Glkj�j�_�l�\Nf-l rq�� x']`_¦bd�ij�¤ lkb-¤¥bd�ij=�U[Z\^f-l]`_��ir�bdc�j4l�\^fd��bd�ij4n9b=_kj¢�sf-�bdo^_kj�c��dj¢l�\^j&_kf=r�j�_kj��^_kbdc�]`rxf-�=jd�H �G�dj�c{_kj�nickb=o^p�j�r lk\ij�b=c�����bvr�j�_�lkb~l�\ij*ckjs_����ij&ov�l�f-e�]��i�ml�\ij*nic�]��i�s]qn^p�j&bvu'p�j�f=_�l

��bvrxr�]�lkr�j��vl���£�\^j�c�j�rq��Ax']`_'�ibdl¦bd�^j$¤¥l�b-¤¥bd�^jd��_�j��dj�c�f=pdh^]��9j�c�j��Gl¦nNbv_�js_'rxfs�&o9j�p�]�edjsp �u�bdc¨f4�v]q�=j��x]�rxfd�djd�¦[Z\ij�ckjsu�bdc�jd��a�j�ckj�nickjs_�j��Gl'lk\ij�n9b=_kj�b=uNl�\ij�c�bdo9bdlXfd_�f n^ckb=o^f-o^]`p`]ql¥�hij��^_k]�l¥�@u��i�^��lk]�bd��b��=j�c����i�=]��dj��@lk\ij&]��ini�il�]`rxf-�dj�smfd�ih©f�rxfdnKw9$ prq�u t sjv wDx�[Z\ij��G�ijs_�l�]qb=��b=uv\ib�a�l�b���b=r�ni�ilkj prq�u t sjv wDx�f-�^f=p ��lk]���f=p�pq�¢]`_��^bdl��spqjsf-cs����b�a�j��=j�cs�

]`u�j��ib=�i�d\ b=uZl�\^j]`r�nNb=ckl�f-�Gl*n^fdc�f=r�j�l�j�c�_*b=uZlk\ij@a�bdc{p�h f-c�j~eG�ib�a�� q u�bdc�j$ ifdr�n^p�jd�p`]q�=\vl�]q�^�@��bd�ih^]�lk]�bd�N_��U_k�ic{u�f-��j��=j�b=r�j�l�ck�©fd�ih�ckj �ij���lkf-�^��j�nickb=nNj�ckl�]qjs_�� j�l��zx{��lk\ij���a;j

����������� �! �"$#&%' �'�(#&� ����#&%!)*#+),"-��)(�/.0)(1�1!"-�*2�� �! ��43'��56� #&%�)�.� 7#&%'�8�/9:�;"�1!"& ��<� =7#&�>�'?03'1@=���.0�>�'?�>�;#& �"&�- �=�#&� ���'��AB�;" )C#&�;1@�;?;"�)(1!%!� =�);DE.0)(1F);D>D ��54� 3'�G#-�H1!"& ��<� =7#I#&%' ��&D �;1J ��;9K#&%' �#-"�);�>DL);%' �)(�NM�O4%� 3�#&�>D>� #QPC�;9R)CST%83!.0);�8UQ"& �)(�<)(V<D �W�.0)(1H)(�4),D �8=�);D>� X� �"Y� �E"&�8�(#& ��Z�>����3�"�"& �.0)("&[*)(V<D I)(V<�>D>� #QP�#&�\"�)(1<� �!D P�& �)("-=7%Z9:�;"Y1<D>)(=� ��Y5+%'�;�& I1'"& ��<� =7#�� �����6.0)*#-=7%/��3�"4�;V'�& �"&2*)*#�� ���'��M

Page 9: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

��fd�@h^j��djspqb=nf�|! G}{y�w�"mlk\^f-l�n^ckj�h^]q��lk_�b=�icZbdo^_kj�c���f-l�]qb=�7�=]��dj��@lk\ij�n9b=_kj¢bvuUlk\ij�c�bdo9bdl��prq�sCt u�v wDx�e<�p�j�fdc�pq�=��lk\ij~rxf-nWw jsr&o9bvhN]qjs_�l�\ibv_�jxnic�bdn9j�c�lk]�j�_4bvu�lk\ijma�bdc{pqh�a�\^]��\j��Nf-o^p�j4�^_Zl�bxnic�j�h^]���lFsx�v]q�=j��Ku¦��%^bdc�lk\^]�_Zc�j�f=_�b=���*JZfs�=j�_�]`f-�^_;�^_k�^fdp`p �@n^f-c{fdr�j�lkj�c{]q��jw fd_ % f-�ih�c�j�u�j�c�lkb©]ql&f=_�f �my��d}�� � H ��u�fd��l��Ul�\^jmnNbv_�jru�ckj�nickjs_�j��Glk_4f�_��ioN_�j�l¢bvul�\^j�_kjmn^f-c{fdr�j�lkj�c{_��@[Z\ij~c�j�f-h^j�c&_k\ibd�^p�h oNj@f�a�fdckj=��\ib�a�j��dj�c��Ul�\^fdl&n^f-c{fdr�j�l�j�c{]���]��i�w ]�r�n^p`]�j�_�l�\^fdlmf n^fdckl�]q���^p`f-c���b=r�ni�^lkf-l�]qb=�^fdp�u�c{fdr�j�a�bdc�e3\Nfd_�oNj�j��g_�jspqj���lkj�h��iJ;�c�j�u�j�c�c{]q�i� l�b w ]�� l�\^j3f-oN_�lkc�fd��l��)a;j3ckjsu�j�c7lkb"f�r�j�r&o9j�cb=u4lk\ij��i�^]��dj�c{_kjSb=u�f=p�p��bvr�ni�ilkfdlk]�bd�^f=p;r�j�l�\ibGh^_�u�bdcx��bvr�ni�ilk]��i�Ys3u�c�b=r u¦��a�]�l�\^bd�il�j$ �n^p`]q�s]ql�p ��_kj�p�j���lk]��i�^�u�bdc�j$ if=r�nNpqj=�¨j$ �n^p`]q�s]ql�bdo�� j���lxr�bGhij�p`_���ckj��d�^p`f-c{]q�sf-l�]qb=��bvu�bdoN_�j�ck��f-l�]qb=�^_*]�� �O¤ _kn^f-��jbdc�_kn^p`]q�ij�]��Gl�j�c�n9b=p`f-lk]�bd�U�¦[Z\^j4]`_�_��ij4bvu �\ibGb=_�]q�^��o9j�l a�j�j��@��b=r�ni�il�f-lk]�bd�NfdpOr�bvh^j�p`_�]�_f-h^hickjs_k_kj�h3ov�YESfd� ��fs� � @ � �d<�fdp`p�fdc�]¦fd�ih�%ij�c�c{]qjxfdp`_�b�hijsfdp¦a�]�l�\�lk\^]�_�]`_�_��ij~]�� l�\ij��b=�Gl�j$ �l�b=u�b=o � j���lxckj���bd�=�^]�lk]�bd���¨_�]q�^��jl�\ij�bdo�� j���l�_�l�\ijsr�_kj�p��djs_�ckj�nickjs_�j��Gl�f _�j�l�bvuf-oN_�lkc�fd��l���p`fd_�_�js_ � ? �

��b�a ��fd�©a;j��^_kj prq�sCt u�v wDx�l�b7]��^u�j�c�nNbv_�j � oG]�r�n^p�j*fdnin^p`]q�sf-lk]�bd��bvuOJZfs�vj�_�� �Uf�ac�j��djsfdp`_�l�\^fdl

prq�u t sjv wDx � prq�sCt u�v wDxMprq�u t wDxprqhs t wDx q 7 x

a�\ij�ckj prqhu t wyx�ckjsu�j�c{_�l�b¢l�\^j¢�;�Nw�Cy-w��nic�bdo^fdo^]`p�]�l¥�¢h^j��^_�]ql¥�&u��i�^��lk]�bd��u�bdcOuyqC] � j=��a�\^j�c�jl�\^j)c�bdo9bdlXl�\N]q�ie�_�]ql�]�_�nic{]qb=c�l�b*lkf-e�]��i�&lk\ij)b=o^_�j�ck��fdlk]�bd� x�=f-�^h prq�s t wyx¨]`_¨c�j�u�j�ckc�j�hxl�bfd_Zlk\ij@} ��� �=}$� 4{}�u�b=c�l�\ij�r�fdn��;[Z\^j&p�fdl�l�j�c�l�j�c�r/]�_�b=u�l�j��Sfd_�_��Nr�j�h@lkb~o9j��i�^]`u�bdc{r�pq�h^]`_�lkc�]�oi�ilkj�hf-�ih]�_�l�c�j�fdl�j�h@fd_�f��ibdc{rxfdp`]q�s]q�i�*��bd�N_�l�f-�Gl���=�b=l�j¢l�\^fdl�]`u�lk\ij¢nic�]�bdcZf-�^hl�\^j�j���]qhij��i��jxf-c�j*�^�^]�u�b=c�rxpq� h^]`_�lkc�]�oi�ilkj�h3l�\ij��3lk\ijmr�bv_�l4p`]qe=j�pq�SnNbv_�j�b=u�l�\ijxc�bdo9bdl���=]��dj��xl�\ij bdo^_kj�c���f-lk]�bd�U�d]�_�l�\^fdl�a�\^]��\~rxf� i]`r�]���js_Xl�\^j p`]qe=j�p`]q\ibGbGhmb=u9lk\ij�bdoN_�j�ck��f-l�]qb=�sm]�lk_kj�p`u¥�<;b=r�ni�il�f-l�]qb=�^fdp`p �v��l�\ij�_�bvpq�^lk]�bd��b=u Q �v�Nf-lk]�bd� 7 u�b=c7lk\ij3rxf� i]`r&�^r�¤ p�]�edjsp�]�\ibGbGh

n9b=_kj�b=u'l�\ij*ckb=oNb=l�r�fs�oNj*h^]�x���^pql 6@l�\^j&�i�ihij�c�pq�i]q�^�mnickb=o^f-oN]�p`]ql¥�hij��N_k]�l¥�u��i�^��lk]�bd�rxfs��oNj©r*�^pql�] ¤ r�bGh^fdp ����b=r�n^p`]q�sf-lk]��i� _k]`r�n^p�j�=c�fdh^]�j��Gl ¤ fd_k��j��vl�r�j�l�\ibGh^_��5%i�ic�l�\ij�c�¤r�bdc�jd�v]ql�]`_¨bvu�l�j��m�ij���j�_�_kfdc��*b=c¨hijs_k]�c�fdo^p�j�l�b&��b=r�ni�ilkj)f4��bvr�nNf-��l)wk} �9wk}01$}��N|¥�-| �Cy���u�bdcl�\^j�§ �&%�� [Z\ij�c�jxf-c�j�f~��fdc�]�j�l¥�©b=u¨��b=r�ni�^lkf-l�]qb=�^fdp�l�bGbvp�_¢l�\Nf-l¢��f-�3oNj�j�r�n^p�b��=j�hSl�bc�j�nic�j�_kj��Gl&f©nic�bdoNf-o^]`p�]�l¥� hij��^_�]�l¥��u��^�i��l�]qb=��� [Za�bSb=uZlk\ij@r�bdc�j~��b=rxr�bd��l�bGbvp�_�f-c�jrx]  �lk�ickj�r�bGhij�p`_�fd�ihdESbd�Gl�j <�fdc�p�b�_�]`r&�^p`f-l�]qb=���ES]q Glk�ic�jxr�bGhijsp�_�c�j�n^ckjs_�j��vl4fnic�bdo^fdo^]�p`]�l¥�Shij��^_�]�l¥�3u��i�i��lk]�bd��fd_�f i�^]�l�j~_k�^r bvu

j�p�j�r�j��vl�f-ck�7§ � % � _ q u�bdc�j� �f=r�n^pqj=�ifm>¢f-�^_�_�]�fd�@u��i�i��lk]�bd� x��A8��i��8 7� $

prq��Cx � 8������ � � p � q� x q$L&x

a�\ij�ckj~jsf-�\ � � ckj�nickjs_�j��Glk_&f©a�j�]��d\Gl�]q�i� l�j�c{r l�b o9j@fdnin^p`]qj�h�l�b j�p�j�r�j��Glkfdc���§ � %p � q� x{�gov�i�\�f-��f-ninic�b=fd�\@]`_Z�^_kj�u��^p�u�b=c�ckj�nic�j�_kj��Glk]��i��r*�^p�lk]q¤¥r�bGh^fdp�h^f-l�fi� ��b�a�j��=j�cs�rx]  �lk�ickj¢r�bvh^j�p`_�bvu�l�j��7ckj��v�N]qc�j�� �Nw�Cy-w�XeG�ib�a�pqj�hi�dj)b=uOl�\^j �G�^r*oNj�c;bvu r�bGhijs_�bvuOl�\ijh^fdlkfi�E©bd�Glkj�<�f-c{pqbm_�]�r*�^p`f-lk]�bd�S]`_)fx��b=r�ni�il�f-l�]qb=�^fdp�fdninic�b=fd�\@a�\ij�ckjs]q�©lk\ij&nic�bdoNf-o^]`p ¤

]�l¥�xh^j��^_�]ql¥�7u��^�i��l�]qb=�]`_�c�j�n^ckjs_�j��vlkj�h7]`r�n^p`]q�s]ql�p �~ov�~l�\^j¢_kj�l�b=u b=�il���b=r�j�_;b=uOc�j�n9j�fdl�j�h_kl�bG�\^fd_klk]���_�]�r*�^p�fdlk]�bd��� %ib=c¨j$ if=r�nNpqj=�dl�\^j)§ � %3u�b=c¨l�\^j�nNbv_�j�bvuOf4ckb=oNb=lX��f-�mo9j�c�j�n�¤c�j�_kj��Gl�j�h@ov�fm_kj�l)b=u�a;js]q�=\vlkj�h�n^fdckl�]q�spqjs_��ijsf-�\b=u'a�\^]��\©]�_�p�bG��fdl�j�h©fdl�fxn^f-c�lk]����^p`f-cn9b=]��vl ]q�Slk\ij&��bd��i�=�ic{f-lk]�bd�©_kn^f-��j&f-�ih a;js]q�=\vlkj�h�ov�lk\ij�p`]�edj�p`]�\ibGbvh©lk\^f-l)l�\ij�ckb=oNb=l

Page 10: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

0 50 100 150 200 250 300 350−200

−150

−100

−50

0

50

100

150

200Translate forward 300cm

σ trns 3cm/m

σ drft 5degrees/m

%¦]q�=�ic�j 7 $`ESbd�Gl�jr<�f-c{pqb7_�]`r&�^p`f-l�]qb=�Sb=uXf7ckb=oNb=l)�i�ih^j�c��db=]��i�7fd�S�i�i��j�c�lkf=]q� r�b=lk]�bd���N��^��j�c�lkf=]q�Gl¥�&]���lk\ij;c�bdo9bdl�� _'\ijsf-h^]��i�)��]�j�p�h^_Xf)h^]�_klk]��i��lkpq�*�ibd�i¤M>¢fd�^_k_�]`f-�&h^]`_kl�c{]qoi�^lk]�bd���%¦]q�=�ic�j¢��bd�ic�l�js_ � H bvf-�i�N]�_FB�j�e�pqjs]ql�]�_

]`_&pqbG�sf-l�j�h�f-l�lk\^f-l�n^f-c�lk]����^p`f-c�n9b=]��Gl�� %¦]q�=�ic�j 7 � 7 hij�n^]���l�_*fDE©b=�Gl�jd<�fdc�p�b©_k]`r&�^p`f�¤lk]�bd�3b=uXlk\ijxckjs_��Npql�]q�i�n9b=_kj*b=u;f~c�bdo9bdl¢�i�ihij�ck�=b=]��i�@fd� �i�^��j�c�lkf=]q��u�bdc�a�fdckhSr�b=lk]�bd���=�b=l�j l�\^fdl;�i�^��j�c�lkf=]q�Gl¥�~]��7l�\^j c�bdo9bdl�� _;\ij�fdh^]��i��p�j�fdh^_�lkb�f*h^]�_klk]��i��lkpq�7�ibd�i¤M>¢fd�^_k_�]`f-�n9b=_kj*h^]`_�lkc�]�oi�il�]qb=��� H � lk\ij�p`]`r�]�l���f=_)lk\ijx�G�^r&o9j�c¢b=uXnNf-c�lk]���p�j�_��=bGj�_ l�b�]q��^�^]ql¥�v� l�\ijh^]`_�lkc�]�oi�il�]qb=��b=u�n^fdckl�]q�spqjs_�j$ if-��lkpq�3c�j�nic�j�_kj��Gl�_�l�\ij@§ � % bvu;lk\ij~c�bdo9bdl�� _�n9b=_kjd�©[Z\ijf-h^��fd�Glkf-�=j&b=u E©bd�Glkjr<�fdc�p�b7_�]�r*�^p�fdlk]�bd��]�_¢l�\Nf-l4�ib����Nw�Cy�w� ckj��G�^]qc�j�r�j��vl�_ fdckj�]�r�¤n9b=_kj�h b=� lk\ij©_�lkck�i��l��ic�jb=u�lk\ij©§ � %;�X[Z\ij�h^]`_�f-hi��f-�Gl�f-�dj@]�_xlk\^f-l�l�\ij��G�^r*oNj�c�bvun^fdckl�]q�spqjs_¦ckj��v�N]qc�j�h�l�b*fdhij��G�^fdl�jsp �*c�j�n^ckjs_�j��vl¦lk\ij)§ � % ]��i��ckjsfd_kj�_¦j$ �n9bd�ij��Glk]`fdp`p ��a�]qlk\l�\^j�hN]�r�j��N_k]�bd�^f=p�]�l¥�©b=u�l�\ijxn^fdc�f=r�j�l�j�c¢_kn^f-��jd� %^�ic�l�\ij�c�r�bdc�jd� u�f=]qlk\^u��^p`p � _k]`r&�^p`f-l�]q�^�l�\^j&_�lkbv�\Nfd_klk]���_Zb=u¦f�ni\v�i_k]���f=p�_���_kl�jsr ]`_�fxh^]��x���^p�l�nic�bdo^p�j�r ]q�Sfd�ih�b=u']ql�_�jsp�u¥� =�j��v¤j�c�l�\^j�p�j�_�_��TE©bd�Gl�j <�fdc�p�b&_�]�r*�^p�fdlk]�bd�x\^f=_XoNj�j��7fdnin^p`]qj�hm_��i����js_k_�u��^p`p �xl�b&lk\ij�nic�bdoNpqjsr�_b=uUckb=oNb=l�pqbG��f=p�]���fdlk]�bd��f-�ih��]�_k�^f=pOl�c{f-�e�]q�^� � 8�PG� 8� �:�c{r�j�hma�]�l�\ JZfs�=js_ �Uf�a��dfd�ihml�\ij)�^_�jsu��^pic�j�p`f-lkj�hxl�bGbvp�_��da�j��ib�a"l��ic��xbd�ic�fdl�lkj���¤

lk]�bd�xlkb�l�\ij�l�fd_ke*bvu^��bd�^_kl�c��i��l�]q�^�4f�rxfdnmb=uNl�\ij�j��v��]�ckb=�^r�j��Gl��,=�b=l�j�lk\^f-l¨a�j�f-c�jZ�ibdl�=j�l�h^]`_����^_k_�]��i�©l�\ijmn^ckb=o^pqjsr�bvu;hNf-lkf©fd���G�^]`_k]�lk]�bd� b=c7}��$�9��y�wk�d|C�Cy-�^�¦oi�il&��bd�^_�]�hij�c{]q�^�a�\^fdl�lkbmhibxbd�i��j4l�\ij�h^fdlkf�\^f=_�f-c�c�]��dj�h��

���� � �, "�$��� '(��&�� '��*�$� ��� '���� �6����� ����� � ����� ��� �0��' ����� ' 2:)_'f ckb=oNb=lU��b=�^_kl�c��i��l�_�f r�fdn&b=u^]ql�_�j��G�G]�c�bd�^r�j��Gl���]�l¦r&�^_kl�j� �j����^l�j;fd��l�]qb=�^_�fd�ih*l�f-e=j_kj��^_kbdc ckjsf-h^]��i�=_�� Q�f-�\©bvuXl�\^j�_kj�l�fd_keG_ ]q�G�=b=p��dj�_ f7��j�c�lkf=]q� r�jsfd_k�ickj*b=uX�i�^��j�c�lkf=]q�Gl¥�a�\^]��\ �sf-� ��bdc�ck�^nil lk\ijxckjs_��Npql�]q�i��rxf-n���fd�ih _k]��i��j�l�\^j��^�i��j�ckl�fd]��vl¥�SbvuXl�\^j�c�bdo9bdl�� _f-��lk]�bd�^_¨��b=r�n9bd�i�^h^_Xa�]�l�\mj�fd�\m_��i����js_k_�]q�=jZj$ �j����ilk]�bd���=l�\ij r�fdnmj��=j��Gl��^f=p�pq�*o9j���b=r�j�_�^_kj�p�j�_�_�q �ib=l�l�bmr�j��Glk]�bd��lk\^f-lZl�\^j�ckb=oNb=l�o9j���b=r�j�_�\ibdn9j�p�j�_�_kpq�@p�b=_kl x�£�\^]`pqj&]�l�rxfs�7_�j�j�r p`]�edj4f��sp�f=_k_�]��¢�\N]q�e=j���¤ f-�ihi¤ j��d�&nic�bdoNpqjsr@�i]�l�]`_�_�l�]�p`pOnNbv_k_�]�o^pqj

l�b7��bd�N_�lkck�i��l�fm�^_kj�u��^p�rxf-n©ov�n9b=_�]��i�~l�\^j&nic�bdo^p�j�r ]q�Sf~nickb=o^f-oN]�p`]�_klk]��&u�c{fdr�j�a�bdc�eO�H �l�\^]`_Z��b=�vlkj$ �l���lk\ij4nic�bdo^p�j�r�bvu�c�j�nic�j�_kj��Gl�]q�i��l�\ij4a�bdc{pqh@oNj���bvr�js_Zbd�ij4bvuU_kj�p�j���l�]q�^�l�\^j�ckj�nickjs_�j��Glkf-l�]qb=�7a�\^]��\�]�_ �my 1$|��Nwky=z��=z���}$�[Z\ij�r�b=_kl¦_kl�c{fd]��d\Gl�u�bdc�a�fdckh&fdninic�b=f-�\*l�b4nic�bdo^fdo^]�p`]`_�l�]q��rxf-n���b=�^_�lkck�^��lk]�bd��]�_¦lk\^f-l

8�P

Page 11: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

b=u�j�r�n^p�b��i]q�^�~f ��fdp`r�fd� %¦]`pqlkj�c�q � % x;l�b~�^nNh^fdl�j&o9bdlk\�l�\ij&c�bdo9bdl�n9b=_�]�lk]�bd�©f-�ih�n9b-¤_�]ql�]qb=�^_4b=u;p�fd�ih^rxf-c�e�_�q _�j�jd�Uu�b=c�j$ ifdr�n^p�jd� � @ 7� f-�^h � 8�G x{�m£�\N]�p�jxl�\ijm��bd�i��j�nil�_�f-c�j_kl�c{fd]��d\Glku�b=ckaZf-c�h���lk\ijSh^]��x���^p�l¥�gnNbv_�j�h�o=�gl�\^j���fdp`rxf-� %¦]�p�l�j�c@]`_~l�\Nf-l7f=p�p)�i�i��j�c�¤lkf=]q�Gl�]qjs_&f-c�jmj�r*oNbGh^]�j�h�f=_ >¢f-�^_�_�]�fd��hN]�_kl�c{]qo^�ilk]�bd�^_��'a�\ij�c�j�f=_4l�\ij~j��dbvpq�il�]qb=� b=u�l�\ijf-��l��^f=pU�^�i��j�ckl�fd]��vl¥��b=u¨l�\ij�nNbv_�j�b=uXlk\ij�ckb=oNb=l)rxfs��o9j��ibd��¤f>¢f-�N_k_�]�fd��� H ��f-hihN]ql�]qb=�l�b >¢fd�^_�_k]`f-�xfd_�_��Nr�n^lk]�bd�^_��-l�\^j ��fdp`r�fd� %¦]`pqlkj�c�f=_k_k�^r�j�_¦lk\^f-lXlk\ij�nic�bdoNpqjsrI]�_�p�]��ij�fdc��[Z\ij�QX �lkj��ihij�h ��f=p�rxf-� %¦]�p�l�j�c q�Q�� % x;f-����bvr�r�bGh^fdl�j�_;�ib=��¤¥p`]��ij�fdcZu�b=c�r*�^p`f-lk]�bd�N_�ov�ic{_�l;p�]��ij�fdc�]���]��i�*]��~lk\ij)�ijs]q�=\GoNb=�ick\^bvbGhxb=uOlk\ij rxf- �]`r&�Nr*¤ p`]qe=j�p`]q\ibGbGhmj�_klk]`rxf-lkj���]�f&f['fs��p�bdc _�j�c�]�j�_)j$ �n^f-�N_k]�bd����[Z\^j*h^]��x���^p�l¥�©a�]qlk\ l�\N]�_¢f-nin^ckbvf-�\©]�_)l�\^fdl p`]q�^j�f-c{]���f-l�]qb=���fd�@�sf-�^_kj4l�\ij Npqlkj�c�l�bmhN]q�=j�c��dj4u�ckbvr�l�\ij���bdc�ckj���lZ_�bvpq�^lk]�bd���H �©bdc�hij�c�lkb7f��dbv]qhl�\^j&nic�bdo^p�j�rx_�j��i��b=�i�Gl�j�c�j�hhi�ij*l�b7f=_k_k�^r�nilk]�bd�N_Zb=u¦p�]��ijsf-c{]ql¥�

f-�^hy>¢fd�^_�_k]`f-��i�i��j�c�lkfd]��Gl¥�=��a�j�l��ic���l�b~fmr�bdc�j4�dj��ij�c{fdp u�c{fdr�j�a�bdc�e~u�bdc�n9b=_�]��i�ml�\ijrxf-n�¤¥oi�^]`p�h^]q�^��nickb=o^p�j�r��fd_m_k�i�d�=j�_kl�j�hgov�"[Z\^ck�i�I}�|��d� � L&G ��qC_kj�j3fdp`_�b � L G� L � x{�[Z\ic��i� � _~a�bdc�e�l�f-e=j�_7fdhi��f-�Glkfd�djSbvu�fiESfdcke=b���fd_�_k�^r�nilk]�bd� fd�ih JZfs�=js_���p�f�a � LT? l�b©_k\ib�aIlk\^f-l��U�=]��dj��3f�_kj��G�ij��^��j�b=u�f-��l�]qb=�^_4f-�ih�bdoN_�j�ck��f-l�]qb=�^_¢fd_�]��ini�il�h^fdlkf^��l�\ijr�b=_kl*nic�bdoNf-o^p�j7rxf-n ]`_�l�\^fdl�a�\^]��\ rxf� i]`rx]q��j�_�l�\ijp�]�edjsp�]�\ibGbGh�b=uZlk\ij@hNf-lkf^�3[Z\ijr�j�lk\ibGhgu�b=cxnickbGhi�^��]��i� l�\ijSr�b=_klxnic�bdo^fdo^pqj�rxf-n qCfd�ih"_�]`r&�^p�lkfd�ij�b=�^_kpq� lk\ij©r�bv_�lnic�bdo^fdo^p�j_kj�lmbvu�c�bdo9bdlmr�bdlk]�bd�N_ x�]`_xo^fd_kj�h"bd�"l�\ij�����i} 4�|¥�-| �Cy����3�s�d� �����s�-| �Cy�� bdc�������-� �=y-w� | 7��� 8�@ �[Z\ij��������-� �vy�w� |! � ]`_�f-� ]qlkj�c{f-lk]��dj�\^]`p�pq¤ �sp�]`r&oN]q�i�©_kl�c{f-lkj��-�S]���p`]�edj�p`]�\ibGbvh _kn^f-��j

a�\^]��\xbdn9j�c{f-l�js_X]q�xl a�b4ni\^fd_kj�_��dfd������i} 4�|¥�-| �Cy��@ni\^f=_�j=�df-�ihmf������d� �������d|C�Cy-��ni\^fd_kjd�[Z\ic��i� � _�c�j��ih^]�lk]�bd�©bvu'l�\ij���g¤ fdp��db=c�]�l�\^r nNbv_�js_�l�\^j� ¤ _�lkj�nSf=_�l�\^fdl�b=u'��b=r�ni�il�]q�i�l�\^jxr�bv_�l4p�]�edjsp � _kj�l¢b=u¨ckb=oNb=l¢fd��lk]�bd�N_¢f=_k_k�^rx]��i�7lk\ijx���ic�ckj��Gl js_�l�]�rxf-lkj�bvuXl�\^jxrxfdn]`_ ��bdc�ckj���l��4[Z\^j���¤¥_kl�j�n lk\ij�����bvr�n^�il�js_ lk\ijxr�f- i]�r*�^r p�]�edjsp�]�\ibGbGh3rxf-n�u�ckbvr l�\ijc�bdo9bdl�� _ZbdoN_�j�ck��f-l�]qb=�^_��^fd_�_��Nr�]��i�xl�\^j&nNbv_�j*j�_klk]`r�fdl�js_�u�ckbvr lk\ij���_kl�j�nSfdckj4��bdc�ckj���l��[Z\ij4nic�bG��j�_�_Z��b=�Glk]��v�^j�_�]�l�j�c{f-l�]q�=j�pq�~�i�Glk]`p l�\^j&r�fdn���bd�G�=j�c��dj�_;l�bmfxp�bv�sfdp rxf� i]`r&�^r�H �Sr�bdc�j�ckj���j��Gl�a�bdc�eO�i[Z\ick�^�S_��i�=�djs_�l�_�f-�©]q�^��c�j�r�j��Glkf=p�r�j�lk\ibGhSa�\^]��\�fdh�� �^_klk_

l�\^j�rxf-n f=_�h^fdlkf~fdckc{]��dj�_)]q�Sc�j�f=p ¤¥lk]`r�j � ?�P �)[Z\ij�r�j�l�\^bvh3j$ �n^p�b=]�lk_ fmE©b=�vlkj�<�f-c{p�bc�j�nic�j�_kj��Glkfdlk]�bd�3b=u;l�\ijmn9b=_kjmb=u�l�\ij~c�bdo9bdl�fd�ih h^]�_kl�c{]�oi�il�js_�l�\^jm��b=r�ni�^l�j�h j�ckc�bdc�]��c�bdo9bdl�n9b=_kj~o^f-�eGaZf-c�h^_�]���lk]`r�j~b��dj�c&l�\ij@nic{]qb=c&bdo^_kj�c���f-l�]qb=�^_��yov�i�\�fd��f-nin^ckbvf-�\rxfd]��Glkfd]��^_ f7_kj�l)bvu¦�dbGbGh _klkf-c�lk]��i�~n9b=]��Glk_¢u�bdc¢�dc{f-h^]�j��Gl)f=_���j��Gl)]��3p`]qe=j�p`]q\^bvbGh _kn^fd��jd�a�\^]��\7�sf-�~o9j ��b=r�ni�ilkj�h@c�fdn^]qhNp �v�Gu�b=ckj��dbv]q�i�*l�\ij¢��bv_�l;b=u�_kj�fdck�\^]��i�&lk\ij¢j��Glk]�ckj4_kn^f-��jb=u�rxf-n^_�u�b=c�l�\ij&rxf- �]`r&�Nr p�]�edjsp�]�\ibGbGh@js_�l�]�rxfdl�jd�[Z\ij���� f-n^nickbvf-�\"\^fd_7fdp`_�b�o9j�j�� j$ �n^p�b=]�l�j�h"ov� �)jsp�p`f-j�ckl@]q��lk\ij ��bd�Gl�j� Glmbvu

��bvr�ni�ilk]��i� ovlkck�i��l��ic�j�u�ckbvr E©bdl�]qb=� q o2%�E x��^_�]q�i��f3��f=r�j�c�f � 8 8 �"[Z\N]�_xa�bdc�e�]�__�]q�=�^]^��f-�Gl)]��©l�\Nf-l ]�l)�sf-�©o9j*j� GnNpqbv]qlkj�h f=_)f-�Sf=pqlkj�c��^f-l�]q�=j�l�b@l�\ij�r�fdn©ckj�nic�j�_kj��Glkf-¤lk]�bd��a�j�a�]`p`p¦j�r�n^p�b��3]q�iovj���lk]�bd�+?��A@�� H l&_k\ibd�^p�h�oNjx�ib=l�j�h�l�\Nf-l�o9bdl�\�l�\^j ��fdp`r�fd�%¦]�p�l�j�c�f-�^h�ESfdcke=b��v¤ o^f=_�j�h*f-n^nickbvf-�\ijs_'l�b�rxf-n�¤¥oi�^]`pqhN]q�i�&fdckj�r�bdl�]q��fdl�j�hxov�*nic�j���]�bd�^__k�i����j�_�_�js_�]���ckb=oNb=l�pqbG��f=p�]���fdlk]�bd��� @/@G�jLT@���8 ���08�P �[Z\ij)f=pq�=bdc{]qlk\^rIu�bdc;r�fdnm��b=�^_�lkck�^��lk]�bd��nic�j�_kj��Gl�j�hxo=�x[Z\ic��i�~]�_�_kfdlk]`_ku`�i]��i�&]q�mlk\^f-l

�ibm_knNj���]�i��ckj�nic�j�_kj��Glkfdlk]�bd�b=u�lk\ij&�i�^hij�c{p �i]��i�x�i�i��j�c�lkfd]��Glk]�j�_�]`_�c�j��G�^]�ckj�h�����b�a�j��=j�cs�l�\N]�_ f-ninic�b=fd�\@f=_k_k�^r�j�_�lk\^f-l)l�\ij&c�bdo9bdl�\Nfd_�f=pqc�j�fdh��7n9j�c{u�bdc{r�j�h�l�\ij&l�fd_ke7bvuZ}{�$�9�qy �wk�d|C�Cy-��6�lk\ij)��b=p`pqj���l�]qb=�mb=u9bdo^_kj�c���f-l�]qb=�^_Xu�ckbvr a�\N]q�\mlkb*oi�^]`p�hml�\ij¢r�fdn�� H l�rxf-e=j�_X�ibf-lkl�jsr�n^l lkb@]��^_�lkck�^��l¢l�\ij�ckb=oNb=l f-o9bd�^l a�\ij�ckj�l�b@�ij$ �l)��b=p`pqj���l4f-�Sb=o^_kj�c���fdlk]�bd��� £3ja�]`p�p ��bd�^_�]�hij�cZl�\N]�_�l�fd_ke7]��lk\ij��ij� Gl�_kj���lk]�bd���

8 8

Page 12: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

���� ���� �+*����� '��*� ��)f���]��i����b=�^_k]�hij�ckj�h3lk\ijmlkf=_�e�_4b=u�p�bv�sfdp`]q�sf-l�]qb=��fd�ih�rxfdn���bd�^_kl�c��i��lk]�bd����a;jxl��^ck��f-lp`fd_kl�lkbxl�\ij�nic�bdoNpqjsr b=uUj$ �n^p�bdc{f-l�]qb=���[Z\ijSrxf�� b=c�]�l¥� b=u j$ �n^p�bdc{f-l�]qb=��fdninic�b=f-�\^j�_xf-ninNp �"\ij��ic{]`_�l�]q�s_xl�b�]��i��c�j�r�j��Gl�fdp`p �

h^]`_���b��dj�c¨�i�ij� GnNpqb=ckj�h7_�nNf-��j � L �X[Z\ij h^]�x���^pql¥�~a�]�l�\@l�\ijs_�j¢r�j�l�\^bvhN_�]`_;j$ if-��lkpq�xlk\^f-ll�\^j$�©f-c�j�\ij��ic{]`_�l�]q�s_�6@�^b~��b=�^_k]�hij�c�fdlk]�bd��]�_)�=]��dj��©lkb~l�\^j �G�^fdp`]�l¥�@b=u'lk\ij*c�j�_k�^p�lk_��Nf-�^hl�\^j�ckjs_��^p�lk_Zlk\ij�rx_kj�p��djs_�f-c�j¢]`r�nNbv_k_�]qoNpqj�lkb���f=p�]�h^f-lkj�bdcZj���f=pq�^fdl�j=�[Z\^]`_Xnic�bdo^p�j�r�\Nfd_Xo9j�j��xfdhihic�j�_�_�j�h�ov��_kj��=j�c{fdpGf-�^l�\ib=c�_X]��xl�\ij���bd�Gl�j� GlXbvuN['f-c{f-�i¤

l�bvp�f � _Z]��G�dj�c{_kj nic�bdoNpqjsr�l�\ij�bdck�@f-�^h %^j�hib=ckb�� � _;lk\ij�b=c��~bvuUb=nilk]`rxfdpOj� Gn9j�c{]`r�j��Glk_ � LT?G�8�? ��ESf-����fs� � _'_�j�c�]�j�_'bvuin^fdnNj�c�_¦f-h^hickjs_k_kj�_'lk\ijZnickb=o^p�j�r�b=u!JZfs�=j�_�]`f-�*]��Gl�j�c�n9b=p`f-lk]�bd� 6]��vlkj�c�nNbvp�fdlk]��i�Sf©u��i�i��lk]�bd� u�c�b=r _kf=r�n^pqj7bdo^_kj�c���f-lk]�bd�N_�]q��lk\ij7nic�j�_kj��^��jmb=u�b=o^_�j�ck��f-¤lk]�bd�Nfdp;�i�i��j�ckl�fd]��Gl¥� � @ ��� @TG �WESfd� ��fs�3j� �n^pqbv]ql�_3oG\^f-�i�^bd� � _*j��Gl�c�bdnv� � 7 � lkb3_k\ib�al�\Nf-l�l�\^j�bdnil�]�rxfdp�n^p�fd��j&u�c�b=r�a�\N]q�\�l�bmb=oilkf=]q��lk\ij��ij$ �l�_�fdr�n^p�j&]�_�l�\^fdl�a�\ij�c�j�l�\ijnic�j�h^]���l�]qb=� b=u�f-��bdoN_�j�ck��f-l�]qb=��]`_&p�j�f=_�l���j�c�lkfd]���� H ��bdl�\^j�c&a�bdc�h^_���bd�ij@_�\ib=�^p�h�l�f-e=jbdoN_�j�ck��f-l�]qb=�^_Zf-lZl�\^j�n^p�fd��js_Za�\ij�c�j�l�\ij � j�c�ckb=c�¤¥o^fdc�_ �4bd��l�\^j&]q�Gl�j�ckn9b=p`f-l�]q�^��u��i�^��lk]�bd�f-c�jxp�fdck�=j�_kl��*£�\N]�p�jml�\ij~fdnin^p`]q�sf-l�]qb=� b=u ESf-����fs� � _ a;b=ckeS]`_4bd�Np � lkbW8$¤¥h�u��i�^��lk]�bd�N_��l�\^j4u��^�ih^fdr�j��vl�fdpOlk\ij�b=c��~o9j�\^]��ih\^]`_Zhij�c{]q��fdlk]�bd�7a�]`p`pOoNj�nic�j�_kj�c��dj�h~a�\ij��@a�j h^j�c{]q�=jbd�^c�b�a��@b=o � j���lk]��dj4u��i�i��l�]qb=�Su�bdc�h^fdlkfx_kj�p�j���l�]qb=���£�\^fd]�l�j�f-�ih %ij�ckc{]�j¢jsr�n^pqb��eESf-����fs� � _Zu�c{fdr�j�a�bdc�e~]q��lk\ij�]�c)a�bdc�embd�©f-��lk]��dj�j� v¤

n^p�bdc{f-l�]qb=�@u�bdc)l�\ij�ni�^ckn9b=_kj�_ZbvuUb=oilkf=]q�^]��i�xbdo�� j���l�r�bGhijsp�_ � ?*8d�!? @ ��[Z\ij��_k\ib�a lk\^f-ll�\^j�oNjs_�l¨p�bG��f-l�]qb=��u�c�b=rIa�\^]q�\xlkb¢l�f-e=jZfd��b=o^_kj�c���fdlk]�bd�x]�_¦lk\^f-lXa�\N]q�\mrxf� i]`r�]���js_¦l�\ij�Nw�} �-�!4�| �Cy�� ����w�C��� 4{}$�No^fd_kj�hbd�l�\ij���b���f-c{]�fd�i��j¢r�fdl�c{]  @b=u�lk\ij&_kj�lZb=u'r�bvh^j�pOn^fdc�f=r*¤j�lkj�c{_¢hij�_k��c{]qoN]q�i��lk\ijxbdo � j���l��m[Z\ij��3u�bdc{r&�Np�fdl�jmf7h^j���]`_�]qb=�3nic�bG��js_k_¢a�\^]��\�]`_�o^f=_�j�hbd�Np � b=��fp�bG��f=p¨r�j�fd_k�ic�jxb=u¨lk\ij7r�bGhij�p¨�i�i��j�ckl�fd]��Gl¥�=��h^]�c�j���l�]q�^�©l�\ij~fd�dj��Gl4l�b©l�f-e=j_kj��^_kbdcmc�j�fdh^]q�^�=_xfd_~]�l~�sp�]`r&o^_=qCf-�^hg_�]`r&�^p�lkfd�ij�b=�^_kpq��_k�ininic�j�_�_kj�_ x&lk\ijS�i�^��j�c�lkf=]q�Gl¥��dc{f-hN]qj��vl��:�c�oNjsp)fd�ih %ij�c�c{]qj©j� GnNpqbv]qlmlk\^]�_@_kf=r�jS]qhijsf�]q��_kb=p���]q�i��lk\ij©bdo�� j���lmc�j���bd�=�^]ql�]qb=�

nic�bdo^p�j�r�-hN]qc�j���lk]��i��l�\ij�_kj��N_�b=cXl�b�n^p`f-��j�_Xa�\^]��\xa�]�p`pirxf� i]`rx]q��j�l�\ij�j� Gn9j���l�j�hmckj�hi�i�$¤lk]�bd�@]q�~j��Gl�c�bdnv��bvuOl�\ij¢nic�bdo^fdo^]`p�]�l¥��hN]�_kl�c{]qo^�ilk]�bd�~b��=j�c;nNbv_k_�]qoNpqj bdo � j���l��sp�f=_k_kj�_ � 7 �&@ �H � l�\^]`_��sfd_kj�lk\ij~f-�^l�\ib=c�_¢o9j��ij�il*u�c�b=r lk\ij~u�f-��l�l�\^fdl�l�\ij � j��Gl�c�bdnv� rxf-n ��sf-� oNj��b=�^_kl�c��i��lkj�h�]q�©f-h^��fd�i��j=���=]��dj��7l�\^j�ev�^b�a��@�i�N]q�=j�c{_�j4b=uUbdo � j���l�_��

%¦]q�^f=p�pq�=�)l�\ij ]q�Nu�bdc{r�fdlk]�bd��lk\ij�b=ckj�lk]�� f-nin^ckbvf-�\g\^fd_fdp`_kb�oNj�j�� fdnin^p`]qj�h l�b�l�\ijnic�bdo^p�j�r bvu�c�bdo9bdl¢�^f���]��=f-l�]qb=��� B�b��3f-�ih [Z\ic��i��hij�i�^jxfd�3b=o � j���lk]��djmu��i�i��lk]�bd��u�bdc�^f���]��=f-l�]q�^�3lkb�f �db=f=pZnNbv_k]�lk]�bd��]���f ev�^b�a��"a�bdc{p�h�lk\^f-l~_�]�r*�^p�lkf-�^j�bd�N_kpq��fd]`rx_ml�brx]q�N]�rx]���j7lk\ij7j��Gl�c�bdnv��bvuZl�\ij@nic�bdo^fdo^]�p`]�l¥��hij��^_�]ql¥��u��i�i��lk]�bd� h^j�i�^]��i�Slk\ijnNbv_�j~bvul�\^jc�bdo9bdl�f-�ih�l�b c�j�f-�\ l�\ij��dbvfdp�fdp�bd�i��f c�j�f=_�b=�^f-oNpqj~nNf-l�\ � 7 L � [Z\ij�c�j�_k�^pql�]q�^�n^fdl�\n^p�fd�i�^]��i�mr�j�l�\^bvh©]`_Zckjsu�j�c�ckj�h@lkbmfd_ � ��b=f=_�l�fdpN�^f���]q�vf-lk]�bd� �*u�bdc�]�lk_�l�j��^hij��i���7l�bh^]�ckj���l4l�\^j~ckb=oNb=l�fdp�bd�i��l�\^jmoNb=�i�ih^fdc�]�j�_4bvu�b=o^_�l�f-�spqjs_���a�\^j�c�j�lk\ijm��j�ckl�fd]��Gl¥� b=u;l�\ijc�bdo9bdl�� _�nNbv_�j��sf-�oNj&]`r�nic�b��dj�hU�[Z\ij fdninic�b=fd�\ij�_Xl�f-e=j��mov� ESf-����fs��f-�^hm£�\^fd]�l�j¢ckjsp �xbd�~_kj��=j�c{fdpN]�r�n9bdc�lkf-�Gl�f=_ ¤

_k�^r�nilk]�bd�^_ f-�ih©fdninic�b� �]`rxf-l�]qb=�^_�� %¦]qc{_�l��N]�l)]`_)fd_�_k�^r�j�h�l�\^fdl�l�\^j&nNbv_�j*b=u'l�\ij*ckb=oNb=l]`_ eG�ib�a��3j$ if-��lkpq�=� =)b�]��ih^]���fdlk]�bd��]�_¢�=]��dj��3f=_)lkb\^b�a�l�b�hij�f=p'a�]qlk\3lk\ij��^�i��j�ckl�fd]��bd�^l���bvr�j�_�bvu�lk\ij*c�bdo9bdl�� _)f-��l�]qb=�^_��`oGj���b=�ih��9]ql)]`_)f=_k_k�^r�j�h©lk\^f-l)l�\ij�h^]�_kl�c{]�oi�ilk]�bd�©bvul�\^j¢r�bGhij�p9n^fdc�f=r�j�l�j�c�_;]�_F>¢f-�N_k_�]�fd���Ga�\^]��\7u�f-�s]�p`]ql�f-lkj�_Zf-�7fd�^fdpq��lk]�� _kb=p��ilk]�bd�@u�bdcZl�\ijo9j�_kl¨bdo^_kj�c���f-l�]qb=��_�]qlkjd� :)_�£�\^fd]�l�j)bd�^pq��fdnin^p`]qjs_Xl�\ij >¢f-�^_�_k]`f-�mf=_k_k�^r�nilk]�bd�m]��mbdc�hij�c

8�@

Page 13: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

l�b�f-ninic�b� i]�rxfdl�j�l�\^j p�bv�sfdp^�dc{f-h^]�j��Gl�]��~l�\^j)�i�i��j�c�lkf=]q�Gl¥�m_�n^fd��j)fd�ih~rxf-e=j)f&p�bv�sfdp^hij$¤��]`_�]qb=�Sf-o9bd�il�a�\^j�c�j&l�br�b��=j��ij$ �l��Nlk\^]�_¢fd_�_k�^r�nilk]�bd�S_kj�jsr�_)ckjsfd_kbd�^fdo^p�jd� ��b�a�j��=j�cs�l�\^j r�bdc�j���c{]�lk]���fdpNfd_�_��Nr�n^lk]�bd�~]`r�n9b=_kj�hmov�ml�\^j)l�\^j�bdck�m]�_�l�\^fdl�l�\^j r�bGhij�p �Gbdc¨c�j�nic�j$¤_kj��Glkfdlk]�bd���-�sf-��j$ if-��lkpq��r�bGhij�p�lk\ij�c�j�fdpq¤¥a;b=c�p�h&ni\ij��ib=r�j��^f4l�\^fdlX�=]��dj�c�]`_kj�lkb�h^f-l�fi� H ul�\N]�_Xf=_k_k�^r�nilk]�bd��]�_X]��i��bdc�ckj���l���lk\ij���l�\^j�f-�dj��Gl��sf-�*o9jZh^]�ckj���l�j�h�lkb4ckj�nNjsf-l�j�h^pq�&��b=p`p�j���l_�fdr�n^p�j�_�u�c�b=r _�]�l�j�_&l�\^fdl&l�\ij~r�bGhijsp¨��fd�i�ib=l�ckj�nickjs_�j��Gl��Up�j�fdh^]q�^�©l�bSf©_k�io�¤¥bdnil�]�rxfdp]`r�n^ckb��=j�r�j��Gl;]��l�\ij4c�j�nic�j�_kj��Glkfdlk]�bd����:)p�l�j�ck�Nf-lk]��djsp �v��b��dj�c�¤¥��b=��ihij��i��j)]��l�\ij�r�bGhij�p��fd�"p�j�fdh lk\ij©ckb=oNb=l�lkb�]q�=�ibdc�j@c�j��v]qb=�^_�a�\^]��\"fdckjnNbGb=c�pq� c�j�nic�j�_kj��Gl�j�h�� E f-� ��fs�c�j�u�j�c{_�lkbSl�\^]`_�]�_�_��^j7fd_*l�\ij � :��\N]�p`pqjs_��(��j�j�p �©b=u�l�\^j@f-n^nickbvf-�\�� �ij~bvuZl�\ij@�dbvfdp`_b=u�lk\^]�_�a�bdc�e3]`_*lkb hij��dj�p�bdn�f3u�c�f=r�j�a;b=cke3u�bdc�fd����b=rxr�bvhNf-lk]��i� fSckj�nickjs_�j��Glkf-l�]qb=� � _]��^f-o^]`p`]ql¥�7lkbm��bvr�nNpqj�l�j�pq�7�sf-nilk�ickj4lk\ij�h^f-l�fi�

� � ��� �&+ ��� � � � W����e� � V� � �� ��� + � VXW4T � ����)f���]��i�¢��bd�N_k]�hij�c�j�h*j$ �l�j��^_k]��djsp ��l�\ijZnic{]qb=c�a�bdc�e�bd��l�\ij�lkbdn^]���b=uij� �n^pqb=c�fdlk]�bd��f-�ih�j���¤��]qc�bd�Nr�j��Gl�c�j�nic�j�_kj��Glkfdlk]�bd���=a;j¢a�]`p�p9nic�bG��j�j�h~l�b�hij��=j�p�bdn~lk\ij¢l�\ij�bdck�xlk\^f-lZr�bdl�]q��f-lkj�_bd�^c¢n^ckb=nNbv_�j�h f-nin^ckbvf-�\���£�jxa�]�p`p¦j�r�n^p�b�� f-��]q�^u�b=c�rxfdlk]�bd��¤¥l�\ij�bdc�j�l�]q��f-ninic�b=fd�\Sl�bl�\^jn^ckb=o^pqjsr��fdp`_�b�lkfde�]q�i� ]��vlkb�fd����bd�i�Glxf-�v��lkfd_kev¤¥_kn9j���]�i�ckj��G�^]qc�j�r�j��vl�_xf-�ih"f-�$¤��b=�i�Glk]��i�mu�bdc��i�^��j�c�lkf=]q�Gl¥�@]��l�\ij�n9b=_kj�b=uUl�\ij�c�bdo9bdl��

� �� � � �02$��� �*��� �� � ����������>¢]��dj��Sf7r�fdnyw��9fd�ihSf~_kj��^_kbdc ckjsf-h^]��i� si� Q �v�Nf-lk]�bd� 7 ��b=r�ni�ilkj�hSlk\ij*n^ckb=o^f-o^]`p`]ql¥�b=uUl�\ij�n9b=_kj u3b=u�lk\ij�c�bdo9bdlZ�^_�]q�i�rJZfs�=js_�� ��f�a $

prq�u t sjv wDx � prq�sCt u�v wDxMprq�u t wDxprqhs t wDx qh?/x

a�\ij�c�jeprq�s t u�vfwyx*ckj�nic�j�_kj��Glk_�bd�icxnic�j�hN]q��lk]��dj �my��d}���bdc�|! G}{y�w�"�lk\^f-lxfdp`pqb�a�_��^_l�b@r�jsfd_k�ic�j�l�\ij�p�]�edjsp�]�\ibGbGhbvuXf-�©b=o^_�j�ck��fdlk]�bd�o^fd_kj�h©b=�©a�\^f-l�a�j&eG�ib�a�f-o9bd�^l�l�\ija�bdc{pqh��H �Sl�\^j&��b=�vlkj$ �l�bvu�j� �n^pqb=c�fdlk]�bd���Nfx�^fd]��dj*fdninic�b=fd�\@lkb~h^f-l�fx��bvp�p�j���lk]�bd�Srx]q�=\Gl�oNj

l�bmr�b��dj4lkbmn^p�fd��js_Zbdc�_kj�p�j���l�fd��l�]qb=�^_�_��^�\lk\^f-l�lk\ij��i�i��j�c�lkf=]q�Gl¥�@]���l�\ij�n9b=_kj�b=u�l�\ijc�bdo9bdl ]`_�rxf� i]`r�]���j�h��&[Z\^]`_4a;b=�^p�h3fdr�b=�i�Gl)l�b©_kj�p�j���lk]��i�7lk\ijxnNbv_�j�l�\^fdl�r�f- i]�rx]���j�_l�\^j�j$ �nNj���lkj�h��y�1$| }�w�Cy�w�;j��Gl�c�bdnv��� b=uOprq�u t sjv wDx�$

� � ���������[prq�u t sjv wDx�pqb=� prq�u t sjv wDx��Ts q �Tx

H l�]�_�]��Gl�j�ckjs_�l�]q�i�xlkbm��bd�Glkc�f=_�l�l�\^]`_�]�hij�fxa�]�l�\�l�\^j��^f���]q�vf-lk]�bd�Nfdp9f-ninic�b=fd�\~b=uOB�b��f-�^h�[Z\ic��i���;a�\ib�r�b��=j@lkb�a�fdckh f3�dbvfdp�]q�"l�\^jeG�ib�a���a;b=c�p�h�ov��rx]��^]�rx]���]��i� l�\ijj��Glkckb=n=�¢b=uGlk\ij�n9b=_kl�j�c�]�bdc{]�n9b=_kj¨h^]`_�lkc�]�oi�il�]qb=��� H ��u�f-��l���r�f- i]�rx]���]��i��� ]`_U]��i��b=ckc�j���lUu�bdcj$ �n^p�bdc{f-l�]qb=�*o9j��sf-�^_kj�]qlXh^j�n9j��ih^_�bd� � �j�hxnic�bdn9j�c�lk]�j�_¦bvu^lk\ij�j��G��]qc�bd�Nr�j��Gl�6&h^]��Nj�ckj��vln^p`f-��j�_&]���l�\ij7a;b=c�p�h�r�fs� pqbGb=e3fdp`]�edj~f-�^h��ibSrxf-l�lkj�c�\ib�a�rxfd�=��bdo^_kj�c���f-l�]qb=�^_4a;jlkfdedjbd�icx�i�^��j�c�lkf=]q�Gl¥��f-o9bd�^l�o9j�]��i��]���b=�ij@nNp�fd��j�dj�c{_k�^_�fd�ibdlk\ij�cm]�hij��Gl�]q�sfdpZn^p`f-��j_k\ibd�^p�hckjsr�f=]q� � �j�hU�

8 7

Page 14: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

H �^_kl�j�fdh���a�j�f-c�j�r�bdc�j�]��vlkj�c�j�_kl�j�h*]q�*l�\ijZp�]�edjsp�]�\ibGbGh�b=uGb=�ic'rxfdn&�=]��dj���bd�ic����ic�ckj��GlbdoN_�j�ck��f-l�]qb=� $

prq$wit u�v sTx � prq�sCt u�v wDxMprq$wit u0xprqhs t u0x q G x

Q��G�^fdlk]�bd� G7ckjsp�fdl�j�_&l�\ij~p`]qe=j�p`]q\^bvbGh b=u;l�\^j7rxf-n�]q��lk\ij~u�f-��jmb=uZf-��bdoN_�j�ck��f-l�]qb=�lkfdedj���u�c�b=r�f n^f-c�lk]����^p`f-c'n9b=_kj;lkb lk\ij�p�]�edjsp�]�\ibGbGh*b=uil�\ijZbdoN_�j�ck��f-l�]qb=�*f-�^h*bd�^c��Z�Nw�Cy�w�r�j�f=_��ic�j�bvu l�\ij¢nic�bdo^fdo^]`p�]�l¥�mb=u lk\ij¢rxfdn���=�bdlkj lk\^f-lgprq�sCt u0x;]`_;]��ihij�nNj��ihij��vlZbvu�uS]��l�\^j&f-o^_kj��i��j4b=uUfxrxf-nU�[Z\ij;]��^u�b=c�rxf-l�]qb=�&l�\^j�bdc�j�l�]q�;f-ninic�b=fd�\4l�b)j� �n^pqb=c�fdlk]�bd�&]`_�lkb.i�ih�l�\ij;nNbv_�j �uxa�\^]��\

rxf� i]`r�]���js_7lk\ij j� �nNj���l�j�h c�j�hi�^��lk]�bd��]q��j��Gl�c�bdnv�gb=u�lk\ij3n^ckb=o^f-o^]`p`]ql¥�gh^]`_�lkc�]�oi�il�]qb=�prq$wit u�v sTx��£�j*_klkf-lkj�l�\ij*j$ �nNj���lkj�h��\^f-�^�dj�]q��j��vlkckb=nv�@]q�©lkj�c{rx_�b=u�a�\^f-l`E f-� ��fs�hij�i�ij�_�f=_ 4$w�y 1 1 ��}$�9|Cwky� "�� $

� � � ����prq�w+t u�v s&xGp�bd� prq$wyx

prq�w+t u�v s&x �/w q /x

f-�^h� � � � �

������ � � q �Tx

a�\ij�ckj`prq�wDxXckj�nickjs_�j��Glk_¦l�\^j�nic{]qb=c�u�bdc w o9j�u�bdc�j�l�\^j f-c�c{]q��fdpGbvu9l�\ij�hNf-l��Nr s � ]�l;rxfs�hij�nNj��ih��i\ib�a�j��=j�cs�Gbd��f�_kj�l�bvu�nic�j���]�bd�^_�pq�7��bvp�p�j���l�j�hh^fdlkfi�Q¨��fdp��^fdlk]��i� Q��G�^fdlk]�bd� c�j��G�^]�c�j�_7fd� ]q�Glkj��dc{f-l�]qb=��b��dj�cml�\ij _�nNf-��j b=u¢nNbv_k_�]�o^pqj

rxf-n^_��3[Z\N]�_�nNbv_�js_*f h^]�x���^pql¥�v� H ����bd�Gl�c{fd_kl&lkb3r�j�lk\ibGh^_*a�\N]q�\���bd�^_kl�c��i��l*n^fdc�f-¤r�j�lkc�]��~r�bvh^j�p`_�o^fd_kj�h�bd� lk\ijmh^f-l�f � ?j8 �Ua�jm\^f��djx�ib=l4�=j�l�]��^_klkf-�Gl�]�fdl�j�h b=�ic&rxf-n^_]��Sl�j�c�rx_ b=uXf-�©j� �n^p�]���]�l4_�j�l�b=u¦n^fdc�f=r�j�l�j�c�_�� H � u�fd��l��9a�j&\^f��=j&��b=�^_k��]�bd�^_�p �©f��dbv]qh^j�hf-�v��_��i�\©]��^_klkf-�Gl�]�fdlk]�bd�©]q�©b=ckh^j�c�lkb~fdp`pqb�a�u�bdc��=j��ij�c�f=p�]�l¥�76~l�\ij&nNf-c{fdr�j�lkj�c{_�r�fs�oNj]`r�nNp�]���]�lkpq�3j��i��bGhij�h3ov��fd��]��^fd����js_k_�]�o^pqj�o^p`f-�ev¤¥oNb� Sb=c&r�fs�So9jmj�r*oNbGh^]�j�h�_kb=p�j�pq�3]��l�\^j&_�j�l�bvuUn^c�]�bdcZbdoN_�j�ck��f-l�]qb=�^_��:Zl lk\^]`_ � �^�i��lk�ickj=��a�j�f-c�c{]q�=j*f-l4f~��ckbv_k_kckbvf-h^_)u�bdc¢��bd�Gl�]q�G�ij�h ]q�G�=j�_klk]��=f-l�]qb=��� �

l�\^j�bd�ij4\^fd�ih��ia�j4��f-��]��^_k]`_kl�l�\^fdl�bd�ic�rxf-nN_�oNj�nNf-c{fdr�j�lkj�c{]q��j�h��¦[Z\^]`_�pqjsf-h^_Zlkbml�\iju�bdc{r&�Np�fdlk]�bd�^_*nickb=nNbv_�j�h�ov�WESfd� ��fs��f-�ih�£�\Nfd]�l�j�}�|��-� �¨a�\^]��\ f-c�j7_�]�r�n^p`]^j�h�ov���bvr�ni�ilk]��i��lk\ij ^c�_kl ¤¥bdc�hij�c�['fs�ipqb=cU_kj�c{]qjs_ j$ �n^fd�^_k]�bd�&fdoNb=�il�lk\ij;rxf� i]`r&�^r�¤ p�]�edjsp�]�\ibGbGhrxf-n �% fd�ih�f=_k_k�^rx]��i��l�\^fdl�l�\ij~p`]qe=j�p`]q\^bvbGh _�nNf-��jx]`_��ibdc{rxfdp`p �ShN]�_kl�c{]qo^�il�j�h ]�� l�\ij�ijs]q�=\vo9bd�^ck\ibGbGh©b=u �% � �ij*n9b=_�_�]qo^p�j�f��=j��G�ij&u�b=c ��bd�^_�]qh^j�c{f-lk]�bd�S]`_ l�\ij�j$ �n^p`]q�s]ql¢n^f�¤c{fdr�j�lkj�c{]q�sf-lk]�bd�@b=u0w fd_Zlk\ij&f-��l��^f=p _�j�l�b=u�bdo^_kj�c���f-lk]�bd�N_��¦[Z\^]`_Z�i]qjspqh^_)f�nNf-c{fdr�j�lkj�ck¤]���f-l�]qb=� %� ����� ������ ��a�\ij�c�j�l�\ij*bdo^_kj�c���f-lk]�bd��_kn^f-��j&]�_���ckbv_k_kj�h�a�]�l�\S]�lk_kj�p`ubd�^��j u�b=cZj�f-�\7h^f-lk�^r�¦[Z\^j¢fd�il�\ib=c�]`_����ic�c�j��Glkpq�~]q�G�djs_�l�]q�vf-l�]q�i��a�\ij�lk\ij�cFQ �v�Nf-lk]�bd� ��fd�@o9j&_�]�r�n^p`]^j�h©]q��lk\^]�_���bd�Glkj$ �l��:)p�l�j�ck�^fdlk]��djsp �v� a;j���fd� a�bdc�ea�]�l�\�l�\^j�lkj$ �l¢b=u E f-� ��fs� � _�b=o^_�j�ck��fdlk]�bd�Slk\^f-l¢a;j

_k\ibd�^p�h���bvp�p�j���l&h^fdlkf � f-l�l�\^j7n9b=]��vl�a�\ij�c�jml�\^j7j�c�c�bdc�o^fdc�_*bd��l�\^j7]q�Glkj�c�nNbvp�fd�vl�f-c�j���^ckc�j��Glkpq�*p`f-c��dj�_kl �^��[Z\ij�]`r�n^p`]q�sf-lk]�bd�x]`_Xl�\^fdl¨f-l¦l�\^j�_kj�nNbv]q�Glk_¨l�\ij�fdo^]�p`]�l¥�*b=uNl�\ij�rxfdnl�bnic�j�h^]���l¢l�\^j�h^fdlkf]�_¢a�j�fdedj�_kl�� ESfdl�\ijsr�fdlk]���f=p�pq�=� \ib�a�j��=j�cs�Nlk\ijxf-�ilk\ibdc¢\^fd_¢o9j�j���i�^fdo^p�j�lkb@hij�c�]��dj�f-�3j$ �n^p`]q�s]ql¢ckjsp�fdlk]�bd�^_k\^]�n o9j�l a�j�j�� l�\^j��\^fd�i�dj�]q��j��Glkckb=n=�©bvuXl�\ijr�bGhij�pifd�ihxl�\ij)j��Gl�c�bdnv�=�dbdc¨�i�^��j�c�lkf=]q�Gl¥�v�-b=u9l�\^j�nickj�h^]���l�j�hmbdo^_kj�c���f-l�]qb=��� H �mu�f-��l��vl�\ij�ijsf-c�j�_kl;hij�c�]���f-l�]qb=��_k�i�=�dj�_klk_ �G�^]�l�j�lk\ij�bdnin9b=_�]�l�j�67]�l�]�_�r�b=ckj�]��^u�bdc{rxf-lk]��dj4lkbmlkf-e=j¢fp�b�aZ¤ �ibv]�_kjZbdo^_kj�c���f-l�]qb=��lk\^f-�mb=�ij�lk\^f-l�]�_��ib=]`_ �v�'[Z\N]�_;f-nin^fdckj��GlX��bd�Glkc�fdh^]q��lk]�bd�m_kl�j�rx_

8�L

Page 15: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

u�c�b=r l�\ij�u�f-��l*l�\Nf-l�l�\ijr�bGhijsp;rxfs��o9j:�G�^]�l�j7��j�c�lkf=]q��lk\^f-l�l�\ijnic�j�h^]���l�]qb=� f-c{]`_�js_u�c�b=r�f&�ibv]�_���n^ckbG��js_k_��va�\ij�c�j�f=_�]�l;rxfs��o9j¢\^]q�=\^pq�x�^�i��j�ckl�fd]��~]q�~n^ckj�h^]q��lk]��i��f*h^f-lk�^ru�bdc�fxn^ckbG��js_k_�a�\^]��\©]�_�ev�^b�a��@lkbmoNj&c�j�p`f-l�]q�=j�pq�~�ib=]`_kj�p�j�_�_��¨[Z\ij���bd�i�spq�N_k]�bd�l�\^fdl�a;jf-c�j4u�bdc���j�hl�bmh^c�f�a ]`_�lk\^f-l�lk\^]�_)_�j���bd�^hp`]q�^j¢bvu�fdl�l�f-�e7]`_Zl�\ij�bdc�j�l�]q�sfdp`p �7�^�^_�b=�i�ih��

%¦]q�^f=p�pq�=�Oa�j�r*�^_�l ��b=�^_k]�hij�c)l�\^j*�i�i��j�c�lkf=]q�Sn9b=_kj&bvu¦l�\ij�ckb=oNb=l�� H � l�\^]`_¢��fd_kjd�Na;j��fd�i�ib=l)_�jspqj���l l�\ij�oNjs_�l nNbv_�j�l�b7�Nf��G]��=fdl�j�lkb^�)£�j�_k\ib=�^pqh3]q�^_kl�jsf-hS_kj�p�j���l f-�3f-��l�]qb=�) l�bxoNj�j� �j����^l�j�hov�7l�\^j�ckb=oNb=l�_kbmfd_�l�b~rxf� i]`rx]q��j lk\ij�j$ �n9j���l�f-l�]qb=�@bvu ��� � �=]��dj��) b��dj�c�lk\ij���b=��i�=�ic�fdlk]�bd�_�n^fd��j � $

�iq ) x\� ��� ��� � t ) � ����prq�u t ) x ��� � �Tu qf8�PTx

��j�lm�^_~fd_�_k�^r�jSu�bdcml�\^j r�b=r�j��Glml�\Nf-lml�\^jS��b=��i�d�^c�fdlk]�bd��_kn^fd��j�b=u l�\ij ckb=oNb=l]`_�h^]`_���ckj�lk]���j�h�bd�Glkb�f�^�^]qlkjm�dc{]qhU� H u�a�jmc�j�_kl�c{]q��l¢lk\ijmn9b=_�_k]�o^p�j~f-��l�]qb=�^_4l�bSf�_�rxfdp`pC�h^]`_���ckj�l�jm_kj�lrqC_k�i�\ fd_��Ur�b��djxu�bdc�a�fdckh 8�P-�sr��bdc4l��^ck��p�j�u�lr8�P���x{��l�\ij���]ql&_k\ibd�Npqh�oNjn9b=_�_k]�o^p�j l�b"u�bdc{r&�^p`f-lkjSl�\^j3l�fd_kegf=_7f wk}$��� �$y�w 4{}��x}��N|m��}��-w�^���G�3�Nwky=z��q}�� � L 7� � H �l�\N]�_m��bd�Gl�j� �l���lk\ij©lkf=_�e�]�_mlkb ��b=r�ni�^l�jSf�nNbvp�]��$����lk\^f-l~_kj�p�j���lk_mfd��lk]�bd�N_m]q�"bdc�hij�cl�bSrxf- �]`rx]q��jml�\ij7j$ �nNj���lkj�h�c�j�aZf-c�h���bdc���j�c�lkf=]q�Gl¥�=�'f-����c��ij�h�l�bSlk\ijmc�bdo9bdl�b��dj�c�l�\ijp�bd�i�d¤ lkj�c{r@�&[Z\^j�nic{]��i��]�n^fdp'h^]��x���^p�l¥� a�]�l�\�l�\^]`_�f-n^nickbvf-�\S]`_4l�\^fdl4l�c{f-��l�f-o^]`p`]ql¥�Sfdp`_kbc�j��G�^]�ckjs_&f�h^]`_���ckj�lk]���f-l�]qb=� b=uZl�\^j~ckb=oNb=l�� _&_klkf-lkj~b=u�eG�ib�a�p�j�hi�=jd�'a�\^]��\�r�fs� �ib=l&oNj_kl�c{fd]��d\Glku�b=ckaZf-c�h���[Z\N]�_�]`_�f �G�ij�_klk]�bd��u�bdc�u��ilk�ic�j�ckjs_�jsf-c��\��

� ��� � ������� � ��� �/� '��*�$� "�$� ' � �0��� &����[Z\ijSu�b=c�r*�^p�fdlk]�bd����fd_mn^ckjs_�j��vlkj�h��;h^]�ckj���lk_~l�\ij©c�bdo9bdlmlkb�lk\ijSn^p`f-��j+q bdcmlkb�j$ �j����il�jf-�Sfd��l�]qb=� xZa�\^]��\ ]`_��vpqb=o^fdp`p ��bdn^lk]`r�f=pUu�b=c h^fdlkfm��b=p`pqj���l�]qb=���.��b�a�j��=j�cs�il�\^j*�=p�bdoNfdp`p �bdn^lk]`r�f=p�f-��lk]�bd��rxfs�@]q�©u�fd��l�c�j��G�^]�ckj¢l�\^j&ckb=oNb=lZl�bml�c{f��djspOfm_k]��d�^]�i�sf-�GlZh^]�_klkfd�i��j=�ibdcni�il�]�lk_kj�p`u�fdlZc�]`_�emh^�ij�l�b�j��G��]qc�bd�Nr�j��Glkfdp9\^fd��fdckh^_��¦[Z\^]�_�u�fd��l�\^f=_�l a;b�j8�9j���lk_ 6 ^c�_kl��l�\^j�ni\v�i_k]���f=p���bv_�lXbvuNj� �j����^lk]��i��l�\ij)bdnil�]�r*�^r fd��l�]qb=�xrxfs��oNj�lkbGb&\^]��d\��vf-�ih~_kj���bd�ih��l�\^j c�bdo9bdlZrxfs�~oNj���b=r�j¢\ibdn9j�p�j�_�_�p �7p�b=_kl�]��l�c{f��dj�p`]��i�&l�bx]�lk_Z�dbvfdp �'[�bxf-����b=rxr�bGh^f-lkjl�\N]�_�hN]�x���Npql¥�v�ia;j4nic�j�_kj��Gl�f��ij�a bdo�� j���l�]q�=j�u��i�i��l�]qb=�

rq ) x\��� �iq ) x� qf8 ��Cx���q ) x qf8 8�x

a�\ij�ckj ��q ) xXckj�nickjs_�j��Glk_Xlk\ij)��b=_kl¨b=u9j$ �j����il�]q�i��f-��l�]qb=� ) f-�^h��7]`_;f&n^f-c{fdr�j�lkj�c¨h^j�l�j�c�¤rx]q�N]q�i��lk\ijxhij�_�]qc�j�h3ckjsp�fdlk]��dj�a;js]q�=\vl¢o9j�l a�j�j���l�\^j�j� �nNj���l�j�h3�\Nf-�i�=j�]��3j��Gl�c�bdnv� �f-�^h���� H l;]`_��^bdl��spqjsf-c�f-l�lk\^]`_ � �i�^��l��^ckj¢a�\ij�lk\ij�c �iq ) x¨_k���x��]�j��Gl�p �mn9j��^f=p�]���js_¨\^]��d\^pq��i�i��j�c�lkf=]q��f-��lk]�bd�^_��X[Z\^]�_)]�_�f=p�_kbmf �G�ijs_�l�]qb=�u�b=c�u��il��^ckj&]��G�dj�_klk]��=fdlk]�bd���

� * V �.+ � , � � W�V��� �)R +T¦R

� �� ���# &�+*�)'��*�$�H �~lk\^]�_Z_kj���lk]�bd�~a�j���b=�^_�]qhij�c;lk\ij _klkfdl�j b=u lk\ij)a�bdc�e�lkb*h^fdl�j=� �^c ic{_kl;]��^_�l�f-�Glk]`f-l�]qb=�b=u¦lk\ij*j� �n^pqb=c�fdlk]�bd�©r�j�lk\ibGh3_�]�r*�^p�fdl�js_)f~c�bdo9bdl ]q� f7nNbvp ���=bd��¤ _�\Nf-n9j�h�j��G��]qc�bd�Nr�j��Glq!%¦]��d�ic�j L!q fTxfx�

8�?

Page 16: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

%¦]q�=�ic�jbL $ oG]`r&�Np�fdlk]�bd�~c�j�_k�^p�lk_��¦f&xXlk\ij¢ckb=oNb=l�� _�j��v��]�ckb=�^r�j��Gl��¦o x¨l�\^j¢lkj�c{rx]q�^fdlk]�bd�~bvuj$ �n^p�bdc{f-l�]qb=���

£3j�r�bvh^j�p=l�\ij;j��G��]qc�bd�^r�j��vl�f=_Ufd�&bG�����^n^f-�i��� �=c�]�h���a�\ij�c�j¨jsf-�\��dc{]�h�nNbv]q�Gl q��v � x\ibvpqh^_ l�\^j���fdp��ij�� ����� q��v � x� l�\ij�nic�bdo^fdo^]`p�]�l¥�©l�\^fdl n9b=_�]ql�]qb=� q��0v � x ]�_¢]��vlkj�c{_�j���lkj�hSov�f-�7j�hi�=j)b=u�l�\ij¢nNbvp ���=bd���¦[Z\ij¢bv�����inNf-�i���x�=c�]�h~]`_�]��^]�lk]`fdp`]q��j�h@l�b P^� ?&j��=j�ck�Ga�\^j�c�jd�=]���¤h^]���fdlk]��i��l�\ij�_klkfdl�j�bvu^�ib&]��^u�b=c�rxf-l�]qb=���dfd�ihxl�\ij�c�bdo9bdlX]`_¨]��^]ql�]�f=p�]���j�hml�b�n9b=_kj�q 7 P�v 7 PTx{�]��lk\ij&p�b�a;j�cZp�j�u�l���bdc��ij�cZbvuUlk\ij&rxf-n��QX �n^p�bdc{f-l�]qb=�@]`_�nNj�c�u�b=c�r�j�h@ov��^c�_kl�]qhij��Glk]`u`��]��i�~fm_kj�l�bvu���fd�ih^]�h^f-lkj�nNbv_�js_Za�\^]��\

f-c�jZj�p`]q�v]qoNpqj)u�b=c¨j$ �n^p�bdc{f-l�]qb=�dqCl�\ibv_�j)a�\^]q�\~fdckjZeG�ib�a��xl�b*oNj�c�j�fd�\^f-oNpqjZo^f=_�j�h�b=�ml�\ij���^ckc�j��GlZ_�l�f-l�j�bvu�lk\ij�bG�����^n^f-�i���7�dc{]�h x{�^fd�ih�^_k]��i�xl�\^j�bv�����inNf-�i���7�dc{]qhl�bm_�]�r*�^p`f-l�jl�\^j�§ � %Sb=u9f4_k]`r&�Np�fdl�j�hm_kbd�^fdcX_��sf-�xf-lXj�fd�\�n9b=_�]�lk]�bd���¦[Z\ij�n9b=_kj�a�\^b=_kj�_�b=�^f-cX§ � %\^f=_�\^]��d\^j�_kl;j��vlkckb=nv�m]`_�_kj�p�j���lkj�hfd_;l�\ij4bdn^lk]`r�f=p9nNbv_�j4u�ckbvr2a�\^]��\7l�b�lkfdedj l�\ij4�ij� Gl_k��f-�U� %¦]q�=�ickj3L q o x _k\ib�a�_)lk\ijm_�l�f-lkj�bvu¨lk\ijxbv�����inNf-�i���S�dc{]qh�fdu�l�j�c 7 P]qlkj�c{f-lk]�bd�N_ bvul�\^j@fdp��db=c�]�l�\Nr@�DN)�^u�bdc�l��i�Nf-l�jsp �v�¨f-l&lk\^]`_�_klkfd�djd�¦lk\ij@r�bGhij�p�ckj�nNjsf-lkj�h^pq� _kj�p�j���lk_�l�\ij_�fdr�j�n9b=_�]�lk]�bd� u�ckbvr a�\^]��\3lkb�_k��fd�3fd�ih3lk\ij~fdp��db=c�]�l�\Nr l�j�c{rx]q�Nf-l�js_��x[Z\ijxc�j�fd_kbd�N_u�bdc*l�j�c{rx]q�Nf-lk]�bd��fdckjm��f-c{]�bd�^_ 6Sa;jm��bd�i�spq�ih^j�h�]�� ovj���lk]�bd�+L^�98ml�\Nf-l&lk\ij~\ij��ic�]`_klk]��~]�_]��i��b=ckc�j���l�� %^�ic�l�\ij�c�r�bdc�jd� ESf-����fs� � _xf-c��d�^r�j��vl�l�\Nf-lm]q�N_����x�s]qj��vlmr�bGhij�p`_mp�j�f-h"l�b_k�ioNb=nilk]`rxfdpZo9j�\^f���]�bd�icxfdp`_kb�fdnin^p`]qjs_���_k]��i��j�a�j�\^f��djr�bGhijspqj�h�lk\ijS_kbd�^fdc�_k��fd�i�ij�c\ij��ic�]`_klk]���fdp`pq�=�£�\^]`pqj¢l�\ij4_�]�r*�^p�fdl�j�h7j$ �n^p�bdc{f-l�]qb=�mb=u %¦]��d�ic�j�_ L!q fTx�f-�ih L q o x¨hijsr�b=�^_kl�c{f-l�j¢_�bvr�j

b=uGlk\ij;]`r�nNb=ckl�f-�Gl�]`_�_��ijs_Uu�fd��j�h*o=�&]��^u�bdc{rxf-l�]qb=��¤ lk\ij�b=ckj�lk]��¨j$ �n^p�bdc{f-l�]qb=���sj�f-�\�bvuGa�\^]��\r&�N_�l�oNjZf-hih^ckjs_k_kj�h&]��*lk\ij;��bd�ic{_kj¨b=uGlk\^]�_'a�bdc�eO�s]�l']`_�b=ui_�]q�=�^]�i��fd�vl�]q�Gl�j�ckjs_�l�l�b ni�ic{_��ijf�c�]��\ij�cZ_kj��N_k]��i�mr�bvh^j�p �X£3j��ib�a�l��ic���bd�ic�fdl�lkj��Glk]�bd�l�bmlk\ij�lkf=_�e~bvuUr�bGhij�p`]��i�ml�\ijnic�bdo^fdo^]`p�]�l¥�~hij��N_k]�l¥�u��i�^��lk]�bd��bvuUfx��]�_�]�bd��_�j��^_�b=c�]q�©f�ckjsfdpOa�bdc{pqh_�j�l�l�]q�i�N�

8 �

Page 17: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

%¦]q�=�ic�j�?�$ c�]��=]��^fdpO_k��j��ij&f-�ih�_��G�Glk\ij�_�]q��j�h�p�fd�ih^rxf-c�e�_��

� ��� �/� ����� '(��� '(��� '��*� � ��� ������������ N)p�lk]`rxf-lkj�pq�=�vbd�ic¨�=b=f=pi]�_¨l�b*fd�il�bvr�fdl�j�lk\ij)f-���G�^]�_�]�lk]�bd�mb=uNl�c{fd]��^]��i�&]��ini�il�_;u�b=c�f&rxf-n�¤p�j�f-c��^]��i�~r�j��\^fd�^]`_kr�N_k�i�\Sf=_�l�\ij�bd�ij*h^]�_k���N_k_kj�hS]�� ovj���lk]�bd� 7 � LN� %^�i�ih^f=r�j��Glkf=p�l�bl�\N]�_;r�j��\^f-�N]�_�r ]`_¨lk\ij)h^j��djspqb=n^r�j��Gl¨bvuNlk\ij�|! G}{y-w�"��vlk\^f-l;]�_��=lk\ij fdo^]`p�]�l¥��lkb&��bvr�ni�il�jprq�sCt u�v wDx�I£�jSa�]�p`p h^]�_k���N_k_7\ij�c�j©bd�ic@nic{]qb=c~a;b=cke"]q��lk\ij h^b=rxfd]���b=u4p`f-�ih^rxfdckep�j�f-c��^]��i�Sf-�^h�]q�Gl�c�bGhi�i��j7f-��]��^_klkf-�Gl�]�fdlk]�bd� b=uFprqhs t u�vfwyx4a�\^]��\���fd��oNjf-nin^p`]�j�h�]��f=JZfs�vj�_�]�fd����bd�Gl�j� �l*l�b3ckb=oNb=l�p�bG��fdp`]���f-l�]qb=�(� 7 v� 7 ����L/P � H l�_k\ib=�^pqh oNj@�ibdlkj�h lk\^f-ll�\^jmnic�]�bdc�a�bdc�eS�s]qlkj�h hibGj�_��^bdl�j�r�n^p�b���fDJZfs�vj�_�]�fd�3u�c{fdr�j�a�bdc�e 6�lk\ij~u�bdc{r&�^p`f-l�]qb=�nic�j�_kj��Gl�j�h@\^j�c�j4]`_��ifd_��=j�l��i�^�ini�io^p`]`_�\ij�h��

�icX�dbvfdp�]`_¦l�b���b=r�ni�ilkj�l�\ij�nic�bdoNf-o^]`p�]�l¥�*hij��^_�]ql¥�xu��i�i��l�]qb=�xb=u9f �sfdr�j�c{f¢]�rxf-�=j`si��=]��dj��4l�\ij¨n9b=_kj u�b=uvl�\^j¨ckb=oNb=l���f-�ih&f�rxfdn�w3� �^cUf-nin^ckbvf-�\4r�fdedjs_ l a;b�_�]`r�nNp�]`u`�i]q�i�fd_�_k�^r�nilk]�bd�^_ 6 ic�_kl��=c�fdl�\ij�cXl�\^fd�x�dj��^j�c{f-l�j�l�\^j�j��Glk]�c�jZ]`rxf-�=jgs^�da�j���b=�i��j��vlkc�fdl�j�bd�^pq�bd���=j��ij�c�fdlk]��i��l�\ij]�rxfd�dj~]���l�\ij7�ij�]��d\Go9bd�ic�\ibGbGh�bvuZfS_�j�l�� ��� " � v "�� v�� ����v "���� bvu��]�_�]�o^pqj _kf=p�]�j��Gl¨n9b=]��Glk_¨b=c)�q�-�����m��w�� 12� 7 G � ovj���bd�^h��=lk\ij)n^ckb=o^f-o^]`p`]ql¥��hij��^_�]�l¥�xu��i�^��lk]�bd�b=u�f-� b=o^_kj�c��dj�h3p�fd�ih^rxf-c�eS]�_¢�ib=c�rxf=p�pq�Sh^]`_�lkc�]�oi�ilkj�h f-o9bd�il4f@rxf- �]`r&�Nr p�]�edjsp�]�\ibGbGhbdoN_�j�ck��f-l�]qb=� � " � a�\^]q�\�]`_���bvr�n^�il�j�hj� �n^p�]���]�lkpq�@u�c�b=r u3fd�ihKw��[Z\ij&u��i�i��lk]�bd��l�\Nf-lZ�dj��ij�c{f-lkj�_ � " � ]`_Zo^fd_kj�hbd��f-���i�N_��in9j�c���]�_kj�h�p�j�f-c��^]��i�xr�j��\^f�¤

�^]`_kr a�\^]��\~��b=�^_kl�c��i��l�_¨l�\^j)r�b=_kl;p`]qe=j�pq�x]`rxf-�dj�n9b=_�]�lk]�bd�7fd�ih~f-nin9j�fdc�fd�i��j�bvuOf*p`f-�ih�¤rxf-c�e@u�c�b=r f~_kj�l)bvu'nic�]�bdc)bdo^_kj�c���f-lk]�bd�N_ q b=oilkf=]q�ij�hS]q� l�\ij�nickbG��j�_�_�b=u'j$ �n^p�bdc{f-l�]qb=� x{�%i�^cklk\ij�c{r�bdc�jd�Uf 4�wky 1 1 � ���-� � �=�-| �Cy���_��\^j�r�jm]�_��N_�j�h�lkbS]q�Nu�j�c&lk\ij~��b���fdc�]`f-�i��j�rxfdl�c{]  b=u;l�\^j7h^]`_�lkc�]�oi�il�]qb=����[Z\ij�c�j�u�b=ckj=�Ul�\^j~��b���f-c{]`f-�i��j��sf-nilk�ic�j�_&fdl&bd�i��jml�\ij@_�lkbv�\Nfd_klk]���^fdl��ic�j7bvu�l�\ij©_kj��^_kbdc�f-�ih�l�\ij©]��^f-h^j��G�^f-�s]qjs_*bvu�l�\^jr�bGhij�p � %¦]��d�ic�jd?�qCfTx&hij�n^]q��lk_f~_k��j��ij*a�\ibv_�j�p�fd�ih^rxf-c�e�_�a;j�ckj�pqjsf-c��ij�h©ov���i�^]`u�bdc{r _�fdr�n^p`]��i�~b=u¦lk\ij*n9b=_kj�_kn^fd��jd�%¦]q�=�ic�j�?�q o x;h^j�n^]���l�_�fm_ ���Gl�\^j�_�]q��j�h�]�rxf-�=j&u�ckbvr fx�ijsf-c�o=�@nNbv_�j�]��lk\ij&n9b=_kj�_�n^fd��j06bd�Np �ml�\^j4]`rxf-�=j ]��7l�\^j �ijs]q�=\GoNb=�ick\^bvbGh~bvu j�fd�\7p`f-�ih^rxfdckex\^f=_;o9j�j��7c�j���bd�^_kl�c��i��lkj�h��>¢]q�=j��©fxr�j�l�\^bvh©u�b=c���b=r�ni�^lk]��i� � " � ��a;j4��fd�©u�bdc{r&�^p`f-lkj prqhs t u�vfwyxZfd_�fxrx]  �lk�ickj

b=u�>¢f-�^_�_�]�fd�§ �&% � _��ijsf-�\@��bdc�c�j�_knNb=�ih^]��i��lkb~fxp�fd�ih^rxf-c�e~]q��lk\ij&]�rxfd�dj $

prq�sCt u�v wDx\� 8� � � prq " � t u�vfw � x qf8�@/x

8�G

Page 18: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

a�\ij�ckj3prq " � t u�vfw � x)��bdc�ckjs_�n9bd�ihN_ lkb�l�\ijK>¢fd�^_k_�]`f-� n^ckb=o^f-o^]`p`]ql¥�Shij��^_�]ql¥�3u��^�i��l�]qb=��bvup`f-�ih^rxfdcke � � ���=]��dj��@lk\ij�nNbv_�j�fd�ih�l�\ij&p�j�fdck�ij�h@n^ckj�h^]q��l�b=cZu�b=c�l�\^fdl�p�fd�ih^rxf-c�e w � �

�ij&u��ic�l�\ij�c)n9b=]��Gl�b=u � �^_�l�]^��f-l�]qb=�Sr&�^_kl�o9j*rxf-hij&u�b=c�bd�ic��\^b=]���j�b=u'fmrx]  �lk�ickjr�bGhij�p�]��lQ �G�^f-l�]qb=� 8�@i��£�\ij�ckjsfd_xf ��bdc�ckj���lxu�bdc{r&�^p`f-l�]qb=� a�bd�^p�h o9j�l�b ��bvr�ni�il�jf � bv]q�Gl&h^]`_kl�c{]qoi�^lk]�bd� r*�^pql�]qnNp�]���fdlk]��dj�pq�=�¦a�j~\^f��=jm�ibdl&��bd�^_�]�hij�c�j�h l�\^j7n9b=_�_k]�o^]`p�]�l¥��bvubd�^lkp`]qj�c�_@]q��l�\ij p�fd�ih^rxf-c�e"ckj���bd�=�^]�lk]�bd��ni\^f=_�j=��a�\^]��\ ��fd� bdoNp�]�l�j�c�fdl�j lk\ij�ckjs_��^p�lk_hi�ij l�b�p`]`r�]�l�j�h rxf-�\^]��ijSn^ckj���]`_k]�bd����£�\N]�p�j3f�_k�^rxrxf-l�]qb=� r�bGhijsp)n^ckj��dj��Gl�_~_��^�\��fdlkf=_�lkckb=ni\ij�_)f-l¢l�\ij���bv_�l�bvuX\^]��d\ij�c �^�i��j�ckl�fd]��vl¥�©]�� l�\^j�§ �&%��O]ql4fdp`_�b]�r�n9b=_kj�_ l�\ij]��i��b=ckc�j���l�f=_k_k�^r�nilk]�bd�l�\Nf-l�l�\^j&p�fd�ih^rxf-c�e~bdoN_�j�ck��f-l�]qb=�^_Zf-c�j hN]�_!� b=]��Gl��

%¦]q�=�ickj �!q o x4hij�n^]���l�_[prqhu t s&x¢b��dj�c&fy@-r�ov�i@-r nNbv_�j~_kn^f-��j~]q� lk\ij7p`f-o9bdc{f-l�b=c��_kj�l�l�]q�^��h^j�n^]���lkj�h�]q��%¦]q�=�ickj ��q fTx�¨[Z\ij�r�bGhij4bvu l�\ij�hN]�_kl�c{]qo^�ilk]�bd�nic�j�h^]���l�_;lk\ij4nNbv_�jb=u�l�\ijxc�bdo9bdl¢l�b�o9jxfdl4nNbv_k]�lk]�bd�lqM8 7 8 v @ �Tx�a�\ij�c�j�f=_¢l�\ijmf-��l��^f=p'ckb=oNb=l n9b=_�]ql�]qb=�3aZfd_qM8 7 P�v @ @Tx{�

� � T V � � � V ��YZT � � � R

£�j&\^f��dj4js_�l�f-o^p`]�_k\ij�hSfmu�c�f=r�j�a;b=cke@u�bdc)fd�il�b=�ib=r�bd�N_�]q�^u�b=c�rxfdlk]�bd��h^]`_���b��dj�ck�@]��©l�\ij��b=�Gl�j$ �l b=uXc�bdo9bdl�]q��j$ �n^p�bdc{f-l�]qb=���¢[Z\^j�c�j�fdckj�f~a�]�hij���fdc�]�j�l¥�©b=u�]�_�_��^j�_¢l�\^fdl c�j��G�^]�c�ju��ic�l�\ij�c�h^]`_����^_k_�]�bd���

� [Z\^j*r�b=_kl�_k]��d�^]�i�sf-�Gl�]�_�_��^j&]�_�lk\^f-l�bvu'l�c{f-��l�f-o^]`p`]ql¥�v��[Z\ij&]��vlkj��=c�f=p�_�l�\Nf-l)r*�^_�lo9j�_�bvpq�=j�h7]��7b=ckh^j�c�l�b*��b=r�ni�ilkj4l�\ij¢j��Gl�c�bdnv�~u��i�i��lk]�bd�@ckj��v�N]qc�j _�]�r�n^p`]�i��fdlk]�bd���H lm]`_�p`]�edj�pq��l�\Nf-l�a;j��fd��j$ �n^p�b=]�l�lk\ij©rx]  �l��^ckj�r�bvh^j�pZu�bdc{r&�^p`f-l�]qb=��a�j7\^f��=jn^ckb=nNbv_�j�h]���bdc�hij�cZl�bxhij�c�]��dj�fd�fd�^fdpq��lk]��4_kb=p��il�]qb=���

� [Z\^jxp`f-�e©bvu¨j� GnNp�]���]�l�r�bGhijsp�_4]`_�f7\Nf-�ih^]���fdn lkblk\ijxhij�c{]���fdlk]�bd��f-�ih r&�^_kl4oNjfdhihic�j�_�_�j�h��X[Z\ij'�G�ij�_klk]�bd�@b=uUr�bGhijspOn^f-c{fdr�j�lkj�c{]q�sf-l�]qb=�mckjsr�f=]q�N_;��b=r�n^p`]q�sf-lkj�h���v]q�=j��l�\ij���b=r�ni�il�f-l�]qb=�^fdp r�bGhij�p�l�\^fdl�a;j4\^f��=j nic�j�_kj��Glkj�h��

� [Z\^j)r�bvh^j�pia�j�\^f��=j�nickb=nNbv_�j�hmu�bdc�r�bGhij�p`]��i� prq�sCt u�v wDx¨]q�mlk\ij)]`r�fd�dj�hibvrxfd]��c�j��G�^]�c�j�_¢u��icklk\ij�c4]��v�=j�_klk]��=fdlk]�bd���9n^f-c�lk]����Np�fdc�pq�Su�bdc4r�b=ckj���bvr�nNp�]���fdl�j�h ��bd� i�d��¤c{f-l�]qb=�_kn^fd��j�_��

� ESbdc�j*a�bdc�e�]`_¢ckj��G�^]qc�j�h3]q�3l�\ijx��b=�Gl�j$ �l b=uXj���f=pq�Nf-lk]��i�l�\ij�j$ �n9j���l�j�h3j��Gl�c�bdnv��v]q�=j��~lk\ij¢�i�i��j�ckl�fd]��Gl¥�x]��7lk\ij c�bdo9bdl�� _�nNbv_�j=�X[Z\^]�_Z]`_��^j���js_k_�f-ck�mu�bdc�j���fdp��^f-l�]q�^�f�_�j�l�bvu f-��l�]qb=�^_��vf=_�b=ninNbv_�j�h~l�b��\ibGb=_�]q�^�&f*_�]q�i�vpqj ��f-�Glkfd�dj�n9b=]��Gl;u�c�b=r�a�\^]��\lkb3b=oilkf=]q�"f _kj��^_kbdc�ckjsf-h^]��i�N� [Z\^j��G�ijs_�l�]qb=��b=u�a�\^j�l�\^j�cxl�\ijc�j�]��^u�bdc���jsr�j��Glp�j�fdck�N]q�i�xn^fdc�fdh^]��=r2��fd�@o9j&f-n^n^p�]�j�h�a�]`p�p�oNj���bd�^_�]qh^j�c�j�h��

� £�jm\^f��=j��^bdl4�=j�l�f-hihic�j�_�_kj�h�l�\ij~]��^u�j�c�j��^��jmb=u;�dj�b=r�j�lkc�]�����bd�N_�lkc�f=]q�Glk_�]�� l�\ij��bd�Gl�j� �l�bvu��^f���]��=fdlk]��i�*lk\ickb=�i�d\@l�\ij�a�bdc{p�h26�nic�j�_k�^rxf-oNp �~lk\ij�_kj4��f-�o9j4fdhihij�hlkb©l�\ijm��b=_kl&u��i�i��lk]�bd���:�)b�a;j��dj�c��2� �^_kl*f=_4a;j~fdckjmfd]`rx]q�^�lkb©ckj���b=�^_�lkck�^��l4l�\ij]`rxf-�=j4u��^�i��l�]qb=���^]�l�_�\ib=�^p�ho9j�n9b=_�_k]�o^p�j¢lkbxckj���bd�N_�lkck�i��lZl�\ij&_kbd�^fdc�u��i�i��l�]qb=���

� :/_�j�n^f-c{f-lkjd�U�=j�l*j��G�^fdp`pq��]��vlkj�c�j�_klk]��i� ]`_k_k�ij7u�b=c*u��ilk�ickj@a;b=cke \^fd_*l�bShib a�]qlk\lk\ijS�^fdl��ic{fdpZlkbdn9b=p�bd�d��b=u l�\ij©j��v��]�ckb=�^r�j��Glxa�\^]��\g]`_mj� Gn^ckjs_k_kj�h�o=�"l�\ijS_kj�lbvu&p`f-�ih^rxfdcke�_~lk\^f-l�f-c�j ���ic�ckj��vl�p ��]�� ��]qj�a���[Z\^f-l�]�_���h^]��9j�c�j��Gl@c�j��v]qb=�^_~bvu

8�

Page 19: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

%¦]q�=�ic�j ��$ �UfdoNb=c�fdl�bdck��_k��j��ij)f-�^hmnickb=o^f-oN]�p`]ql¥�xhij��^_k]�l¥�mu��i�i��l�]qb=�~b=uOc�bdo9bdlXn9b=_kjd�X[Z\ijf-c�j�f¢hij�n^]���l�j�hm��b=ckc�j�_knNb=�ih^_¦lkb�l�\ij @���� ��b=��i�d�^c�fdlk]�bd�x_�n^fd��j�bvuNlk\ij�c�bdo9bdl��(�¢f-c�edj�cc�j��=]�bd�N_;��bdc�ckjs_�n9bd�^h7l�bmr�bdc�j�p�]�edjsp �@nNbv_�js_����v]q�=j��l�\ij&]`rxf-�dj=�

8 �

Page 20: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

_kn^fd��jx�=]��dj�c�]`_kj�lkb©f@hN]:�9j�c�j��Gl&_kj�l4b=u��G]`_�]qo^p�jmp�fd�ih^rxf-c�e�_�� <�p�j�f-c{pq�=��lk\ij�c�jmf-c�jb=nin9bdc�l��i�^]�lk]�j�_'u�bdc�n^f-lk\&n^p`f-�i�N]q�i� f-�ih*]��^u�j�ckj��i��j¨oNfd_kj�h&b=���^f���]q�vf-lk]��i��o9j�l a�j�j��lk\ij _kj�l�_¨bvuN��]`_k]�o^p�j)p`f-�ih^rxfdcke�_��'[Z\^]`_�fdninic�b=fd�\�p`]qjs_¨�=j�ck��r&�^�\~]q�~lk\ij�hibvrxfd]��bvu�f©\v��oic�]�h�c�j�nic�j�_kj��Glkfdlk]�bd� o9j�l a�j�j���f _�j�l*b=u�lkbdn9b=p�bd�=]���f=pXn^p`f-��j�_*fd�ih p�bG��fdpr�j�l�c{]q�&rxf-nN_��

� � � � Y + R � ��� R

�ijxb=u�l�\ij~r�bv_�l�_�]q�=�^]^��f-�Gl¢�=fdn^_4]q� l�\^jmhib=rxfd]���b=u¨c�bdo9bdl�]q�s_¢ckjs_�jsf-c��\ ]�_&l�\^fdl¢bvuf-�^l�bd�^b=r�bd�^_�j� GnNpqb=c�fdlk]�bd���¦£�\ij�ckjsfd_Zl�\^j�hib=rxfd]���b=u�r�fd�\^]��ij&pqjsf-c��^]��i��\Nfd_�_kl��ihN]qj�h\ib�aIlkbSrxf-edjx_kj��^_kjxb=u¨lk\ijmh^fdlkfi�U�dj�ck� u�j�a�c�j�_kj�f-c��\ij�c�_)\^f��djx��b=�^_�]qhij�ckj�h lk\ijmlkf=_�eb=u¦fd��l��Nfdp`p �©f-���v�N]qc{]q�^�ml�\ij�h^f-l�fml�\^fdl)b=nilk]`rxfdp`p �©u�f-�s]�p`]ql�f-lkj�_�lk\ij*l�fd_kefdl�\^fd�ih�� �ica�bdc�e�f=]�rx_�l�b ^p`pNlk\^]`_;�vf-n]q�f&aZfs�xlk\^f-lZ]`_�fdl;b=�i��j¢l�\^j�bdc�j�l�]q�sfdp`p �~_kbd�i�^h7f-�ih@nic{f-�$¤lk]���f=p�pq��u�jsfd_�]qoNpqj=�'£�j�\^f��=j�nic�bdn9b=_kj�hmf4l�\ij�bdck��bvuNj� �n^pqb=c�fdlk]�bd�xa�\^]��\m]�_�hij�c{]q�=j�hmu�c�b=r]��^u�bdc{rxf-lk]�bd�xlk\ij�b=c��v� ovn9j��s]^��fdp`pq�=�vl�\ij ckb=oNb=l¨]�_�h^]qc�j���l�j�h~l�b*fd���G�^]�ckj�_kj��N_�b=c¨ckjsf-h^]��i�v_u�c�b=r¡n^p`f-��js_&a�\ij�ckj]ql�_�fdo^]`p�]�l¥��l�b3r�bGhijsp;lk\ij7a�bdc{p�h ]�_�a�j�f-e=j�_kl��S£�j\^f��dj~js_�l�f-o�¤p`]�_k\ij�hnic�j�p`]�rx]��^f-ck�mckjs_��Npql�_;a�\N]q�\��fdp`]qh^fdl�j�]��@n^fdckl�l�\^j4u�jsfd_�]qoN]�p`]ql¥�~bvu�b=�icZ�db=f=p�_�f-�^h\^]��d\^p`]��d\Gl�_�bvr�j4bvu�l�\^j&]�r�n9bdc�lkf-�GlZh^]��x���^p�lk]�j�_�lk\^f-l�r*�^_�l�o9j�b��dj�ck��b=r�jd� �ic�u��ilk�ickja�bdc�e©a�]�p`pX��b=�Glk]��v�^jml�bS_kj�j�e3f©r�fdckc{]`f-�dj�o9j�l a�j�j���fc�bdoi�^_kl4l�\ij�bdck�3bvu�j� GnNpqb=c�fdlk]�bd�f-�^h@lk\ij�nic{f-��l�]q�sfdp ]`_k_k�ijs_Zb=uU]`r�n^p�j�r�j��Glkfdlk]�bd���

� � � �)V � � YF�)R

� 8 §¦��:)p�j���]���bv_����N� ��� J�b=]`_�_�b=�i�^f-l���f-�ih3E�� � ��]��ij��=� ��lk\ijZbdc�hij�cX]��ihi�i��j�h�ov��f _kj�lb=uOc{fs�i_�$O:�ninNp�]���fdlk]�bd�~lkb&l�\ij nic�bdo^]��i�&bvuO�ibd�m��bd�G�dj� �n9b=pq�G�=bd�^_�� H � 7�wky 4{}{} �-���v� 1y ��| �}�� ��������9| }�w�9�d|C�Cy-�9�-���¨y����$}�wk}���4{}xy�� 5�y=z�yd|C�!401���� �:;&/i|¥y��m�-| �Cy��N�in^fd�dj�_@�PT@ 6j@�PTG�� H Q Q Q"§¨c�j�_�_��C8�� �^�

� @ ['fdpj:)cko9j�p ��;.4�|C� ��}��z��} 4�| 5)} 4{y��-�N� |C�Cy-���¨y-����� | �Cy��O}��mz0" 7Zwkydz{�dz�� � � 1$| �!4 � �s� �d}���4{}��� � ���N| wky{��" ���{� 1��§¨\ � l�\ijs_k]`_��TE©��>¢]`p`p�N��N]q�=j�c{_k]�l¥�=�TE©bd�Gl�c� jsfdp ���¢�� j�oNj��d���vf-��¤�^fdc��d@ P P/Pi�

� 7 ['fdp�:)cko9j�p�fd�ih %ic{f-�ie@§¦� %ij�ckc{]qj=� �¢]�j�a�n9b=]��Gl�_�jspqj���lk]�bd��ov�@�Nf��G]��=fdlk]�bd�@l�\ic�bd�i�=\j��Glkckb=n=� rxfdn^_�� H ���O} ��}��N|! �� �������N|¥}�w�9�-| �Cy��O�-���¨y�� �$}$w�}$� 4�}3y����¨y�� � /i|¥}�w�U� 1�Cy-�^� �4j�ckev��c{fi�j> ckj�j���j=� 8 � � �i� H Q Q Q"§¨c�j�_�_��

� L £�b=p`u�c�f=r J;�^ck�vf-c�h��7�¢]�j�l�j�c %ib�  �*ESf-c�erE©bGbdc{_��*B�js]qhyoG]`rxr�b=�^_��Gfd�ihyovj�o^f=_�l�]�fd�[Z\ic��i��� <;b=p`p`f-o9bdc{f-lk]��dj¢r&�^p�lk]q¤¥ckb=oNb=l;j� GnNpqb=c�fdlk]�bd��� H � 7�w�y�4{}{}������v� 1*y ��|! G}�� �������9| }�w�9�d|C�Cy-�9�d���¨y�� �$}$w�}$� 4�}*y��95�y=z�yd|C�!401¢�-��� ;&/i| y �x�d|C�Cy-�^�joGf-��%^c�fd�i��]`_k��b^�*< :&�ESfs�d@ P P/Pi� H Q Q Q�§�ckjs_k_��

� ? %ic{f-�^��j�_k��b >�� <�fdp`p�fdc�]Xfd�ih %ic{f-�^e3§¦��%^j�c�c�]�jd�e:���lk]��dj~c�j���bd�d�N]ql�]qb=� $[N)_�]��i�©�i��¤��j�ckl�fd]��Gl¥��l�b�ckj�hi�i��j�fdr*o^]��d�^]�l¥�=� [�j��\^�^]q�sfdp B�j�n9bdc�lm[FBZ¤f< H E�¤ �T?�¤ 8 8=��ES��>¢]`p�pN��N]q�=j�c{_k]�l¥�=�F8�� �/?i�Sfdp`_�b l�b3fdnin9j�f-cs� H �Gl�j�c��^fdlk]�bd�^f=p��=bd�ic��^fdp�b=ub<;bvr�n^�il�j�c �4] ¤_�]qb=���

@�P

Page 21: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

� � <;\^]��@fd�ih.=)lkfdu�bv_���ov\ibdc�l�js_�l�aZf-l��\Nr�fd�mckb=�il�js_;]��@_�]`r�nNpqj¢nNbvp ���=bd�^_���<� �� � �� 5������&� 1�4$wk}�|¥}�� �¨y�� � /i|¥�-| �Cy��9�d��<�}{y��m}�| w�"�� �^��8 � �j8d�

� G £�j�]q¤¥§Xfd�i�3<;\^]���f-�ihyoG]`r�j�bd� =�lkf=u�b=_�� n^lk]`r&�^r�aZf-lk�\^rxf-�~c�bd�^l�j�_�� �$���$y-w��x� �| �Cy�� 7Zwky�4{}01 1{���G�U}�| |¥}�w 1{��@ jqf8�x $ 7 ��6TL/L^�/E fs�=8 � ��

� ��b�a�]�jb<;\ib=_kj�l¦f-�^h �=bGj�p&J��ickhN]q�eO��ovj��N_�b=c'oNfd_kj�h�n^p�fd�i�^]��i�^��nNf-c�l 8/8 $ H �^��c�j�r�j���¤lkf=p9��bd�N_�lkck�i��lk]�bd�~bvuOl�\ij¢�dj��ij�c{fdp`]���j�h7�dbdc�bd�N]��dc{f-ni\U� H � 7�w�y�4���y#� � ����� �¨y-���$}�w �}���4{}�y��95�y=z{y-| �!4�1¢�-��� ;&/i|¥y��m�-| �Cy��^�vn^fd�dj�_`8 ��L 7 6K8 � L �� =)f-�=b��vf^���=fdn^f-�U�/ESfs�8 � �/?i� H Q Q Q"§¨c�j�_�_��

� � B��!<;bvpqj4f-�ihD< � �x� ��f-n�� ov\^fdnNj�u�c�b=r2nic�bdo^]��i�N�� iy�/�w�O�-��y � ;¢� �vy�w� |! � 1� $98 ��67 i� 8 � /GG�

� 8�P %���� j�p`p�fdj�c�l��U£��OJ��ick�vf-c�h�� ����%ib�  ��fd�ihio9�U[Z\ic��i��� N _k]��i�©l�\^jm��bd�^hij��^_�f-l�]qb=�fdp��db=c�]�l�\Nr u�b=c¦ckb=oi�^_kl�����]�_�]�bd��¤¥o^fd_kj�h&r�b=o^]�p�j�ckb=oNb=l'p�bG��fdp`]���f-l�]qb=��� H � � ������¨y����� /i|¥}�w �Oy 4$�C}�| " �¨y����$}�wk}���4{}�y�� �¨y�� � /i|¥}$w �U� 1�Cy������ � 7��-| | }�w��5)} 4{y��-�N� |C�Cy-�^�H Q Q Qg§¨c�j�_�_�� �=�i�ij38 � � �i�

� 8 8 %ic{f-�^e � j�p`p`f-j�c�l���ovlkj��=j��mE���ovjs]qlk�d�!<;\^fdc�p�j�_ Q��G[Z\ibdc�n9jd��f-�iheovj�o^f=_�l�]�fd�7[Z\ic��i���ovlkck�i��l��ic�j�u�c�b=r r�bdl�]qb=�Sa�]�l�\ib=�il ��bdc�ckjs_�n9bd�^hij��i��jd� H � � ����� �¨y�� � /i|¥}�w � y�4�� �}�|!" �¨y�� �$}$wk}���4{}&y-� �¨y�� � /i|¥}$w �U� 1�Cy-�3��� � 7��-| |¥}$w�95�} 4{y��-�N� |C�Cy-�^� H Q Q Q §¨c�j�_�_���=�i�ij[@�P/P P^�

� 8�@ :&�i§¦� � j�r�n^_kl�j�c���=*�jE�� �Uf=]qc�h��Gf-�ih��*�jJ¢�*B��ioN]q����ESf- �]`r&�Nr�p�]�edjsp�]�\ibGbGh@u�c�b=r]��i��bvr�nNpqj�l�j7hNf-lkfS�G]`f©lk\ijKQ E fdp��dbdc{]�l�\^r q a�]�l�\�h^]`_k���^_�_k]�bd�Cx{�� iy�/�w�O�-�;y#�7|! G}5�y "d�-� �9|¥�-| � 1| �!4{�-� � y�4��C}�|!" 1�}�w�C}�1 �� 7 � $ 806 7 �� 8 �TG/GG�

� 8 7 � �ih^f fd�ih � f-c�l��"7��d| |¥}$w���X�q� 1 1� � 4{�d|C�Cy-� ����� � 4�}��9} ; �O�-� " 1� 1�S£ ]`pqj��=��=�j�a�¨bdc�eO��= ���08 �&G 7 �

� 8�L > c�j��db=c�� � �ihij�eO�3Q��G�G]�c�bd�^r�j��Gl4rxf-nin^]��i�©�^_�]��i�©r&�^p�lk]�n^p�j~f-o^_kl�c{f-��lk]�bd�3p�j��=j�p`_��H �67�wky 4{}�}��-���v� 1�y ��|! G} � �����Z�^�dbvpq�Nr�j �L^��=�b��O� 8�� � �^�

� 8�? %ij�hibdc�b��O��� �}{y�w�"@y#�&y{�9| � �m�-�¦}��$�i}$w� �m}$�9| 1� :��sf-hijsrx]q�&§�ckjs_k_��C8 �TG @��� 8 � ��� %ib�  ��£���J��ic��=fdckhU�Gfd�ihyo9�^[Z\ic��i��� :)��lk]��dj�rxf-c�edb��~p�bG��f=p�]���fdlk]�bd�@u�b=c�r�bdo^]`pqjc�bdo9bdlk_��$5�y=z�yd|C�!401*��� �:;&/i|¥y��Oy��my�/21 ��" 1$| }�� 1��8�� � ��¦[Ubmf-n^nNjsf-cs�

� 8�G �=b=_kjd> �^]���fd�Gl�� Q�hi�^f-c�hib =)j�o9bdl��¨fd�ih �)�i�d\ � �ic�c�fd�Gl ¤¥£�\=��lkjd�Wo�]�r*�^pql�f-�ij�bd�^_p�bv�sfdp`]q�sf-l�]qb=� f-�ih�r�fdn oi�N]�p�h^]��i�3�N_k]��i� �Nf-l��^c�f=p;u�jsf-l��^ckjs_�]���bd�^l�hib=ckc�j��G��]qc�bd�i¤r�j��Glk_�� H � �N���=|! �$�N|¥}$w�O�-| �Cy��9�d���¨y-���$}�wk}$� 4{}~y�� ��9| }���� �q�v}$�9|$;&/i| y-�9y �xy /21 � " 1 �|¥}0� 1� H lkf=p �v��@�P P/Pi�

� 8� ES]��\^f-jsp H _kfdckh*fd�ih :)�ihic�j�alJ�p`f-e=jd�j<;bd�ih^j��^_�f-l�]qb=�'64��bd�ihN]ql�]qb=�^fdpvhij��^_k]�l¥�&nic�bdn�¤f-�vf-l�]qb=� u�bdc@�G]`_k�^fdp l�c{f-�e�]��i�^����N|¥}�w�9�-| �Cy��O�-�� iy�/�w�9�d�¢y#� �¨y�� � /i|¥}�w �U� 1�Cy-�^�@ �!qM8zx $A? 6*@ �� 8 � � ��

� 8 � :)p`p`f-� �=j�n^_kbd�mf-�^h3E ]q�\^fdj�pjJZp`f-�eO�!E ]  �l��^ckj)r�bGhijsp�_¨u�b=c�]`r�fd�dj�c�j�nic�j�_kj��Gl�f-lk]�bd�U�[�j��\i�^]���f=p&B�j�n9bdc�l�: B � � �-¤¥§ NbJ;¤ ?�L^��§ B`Qg< :bB = :bB � §¨c�b � j���l�[�j��\i�N]q�sfdpTB�j$¤n9bdc�l�� � j�n^fdckl�r�j��Gl�bvu!<;b=r�ni�^l�j�c ov�s]qj��i��j=� N��N]q�=j�c{_k]�l¥��b=ui[�bdc�bd�Gl�bN�zESf-c��\ 8 � � �^�

@*8

Page 22: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

� @�P J¢� �4�^]�nNj�c�_xf-�^h � ¤¥[4� J;���i��� : c�bdo9bdl�j$ �n^p�bdc{f-l�]qb=� f-�ihgrxf-ninN]q�i� _kl�c{f-l�j��-�o^f=_�j�h bd� f�_�jsrxf-�Glk]�� \^]�j�c{f-c��\v�"bvu�_�nNf-lk]`fdp c�j�nic�j�_kj��Glkfdlk]�bd�^_�� 5)ydz{y-| �!4�13�-���;&/i|¥y��9y �xy /21���" 1$| }�� 1� �$ L*G�6 � 7 � 8 � �j8d�

� @*8 J�j�����f=rx]q� �^� �4�^]�n9j�c{_�fd�ih ���X[4� J;�G�^��� : �G�^fdp`]ql�f-l�]q�=jfdninic�b=f-�\�lkb ckb=oNb=lj$ �n^p�bdc{f-l�]qb=��fd�ih�rxf-ni¤¥p�j�fdck�^]��i�N� H ��7�w�y�4{}{}������v� 1Sy ��| �} � ������>�y�w�� 1 Gy��y��1¥���d|C�C�d�¨wk}{��1$y-�^���G�3�-����� /i� | � � 1$}���1�y-w � /21�Cy-�^�'n^f-�=j�_ 7 �/P 6TLTP�LN� �Ub=_�:)p�l�bv_�� < :&�8 � /G�� H Q Q Q)�

� @ @ �=bd\i� �^� �Uj�b=�^f-c�h f-�^h9�)�i�d\8%����)�^ckc{f-�Gl ¤¥£�\v��l�jd��E©bdoN]�p�jxckb=oNb=l¢p�bG��fdp`]���f-l�]qb=�ov��lkc�fd�e�]q�i���=j�b=r�j�l�c{]q�mo9j�fd��bd�N_�� � ����� ��w�����1�� 4�| �Cy�� 1@y�� 5�ydz{yd|C�!4017����� ;&/ �|¥y��m�-| �Cy��^� G*q 7 x $ 7 G � 6 7 @i���=�i�ij38 � �j8=�

� @ 7 �=bd\i� �N� ��j�b=�^f-c�hgf-�ih5�)fd�^_ �vf-��b=o oO� %ij�hij�cs� : ��bvr�n^�ilkfdlk]�bd�^f=p�pq� j��x�s]qj��vlr�j�lk\ibGh*u�bdc'p�fdck�=j$¤ _��sfdp�j���bd�i���ickc�j��Gl�rxf-n^n^]q�^�)f-�ih&p�bG��f=p�]���fdlk]�bd��� H � �=bd\i� �)b=p`pqj�c�¤o^fd�\fd�ih �¢f-� �4bGh^]�lk_k�\ij�eO�Gj�h^]�l�b=c�_���5�y=z�yd|C�!401�5�}01�}��-w 40 �� G}��4���N|! �$�N|¥}$w�O� �| �Cy��9�d� ��"��)�iy 1� / ��� ��b=�ihibd�U��@�P P/Pi��ovnic{]��i�dj�c�¤ �¨j�c{p�fd�^�

� @zL %ij��i� ����f-�ihdQ¨��f-�^�dj�p�b=_ Q��*ES]`p`]qbv_�� nilk]`rxfdpO�vpqb=o^fdpNnNbv_�j¢j�_klk]`rxf-l�]qb=�u�b=c���bd��¤_�]�_kl�j��vl _�j��^_�b=c�h^fdlkfmc�j��v]�_kl�c{f-l�]qb=��� H �"7�w�y�4���y#��� ���� �¨y-���$}�wk}$� 4{}my��65�y=z�yd|C�!401��� �:;&/i|¥y��m�-| �Cy��N��n^f-�=j�_ � 7 6y8�P/Pi��= f-�db��Gfi� �=fdn^f-�U�jESfs�y8 � �/?�� H Q Q Q"§¨c�j�_�_��

� @ ? �¢p`f-hN]�rx]�c �N� �U�^r�j�p`_�ev�v� ov�^j�\^f=_k]`_bE©�^ev\^bdn^fdhi\v�vfs�=�Nf-�ihYoG�i����f-�^�^� �)�G�Nfdrx]q�n^fdl�\ n^p`f-�i�^]��i�©]���_kj��^_kbdck¤¥o^fd_kj�hSlkj�c�c�f=]q��f-���v�N]�_�]ql�]qb=��� � ����� �iwk�-��1�� 4�|C�Cy-��17y��5�y=z{y-| �!4�1�� ;&/i| y �x�d|C�Cy-�^� �!q$L&x�$ L �/@ 6TL&G @��T:��i�N��8 � � Pi�

� @ � ����E f-� ��fs�v�=JZfs�=js_k]`f-��]��Gl�j�c�n9b=p`f-lk]�bd�U���&}�/�wk�d� �¨y�� � /i|¥�-| �Cy��N� L!q 7 x�$ L!8�? 6&L L*GG�8 � �/@i�

� @/G ����E f-� ��fs�v� H �^u�b=c�rxf-l�]qb=��¤¥o^fd_kj�h�b=o � j���lk]��dj;u��^�i��l�]qb=�^_�u�b=c'fd��l�]q�=j�h^fdlkf�_kj�p�j���lk]�bd����&}�/�wk�d� �¨y�� � /i|¥�-| �Cy��^�!L!q�L&x $A? �/P 6 � P�LN��8�� �/@i�

� @ �N� � B�b=�icke=jd� ; w$| <��-���q}�w�" � �}{y�wk}�� 1S��� � ;¢� �=y-w� | 7� 1� ) iu�b=ckh N)�^]��dj�c{_�]ql¥�§¨c�j�_�_���=�j�a �¨bdc�eO��= ����8 � /GG�

� @ � H b=fd�i�^]`_ B�j�eGp�j�]�lk]`_�� J��i��f=pq�=bdc{]qlk\^rx_�$=: _��ic��dj��=� [Uj��\i�^]���fdp�ckj�nNb=ckl��gES��>¢]`p�pN��N]q�=j�c{_k]�l¥�=� 8�� � i�

� 7 P H b=fd�i�^]`_ B�j�e�p�j�]�lk]`_��O> c�j��=bdck���)�ih^j�eO��fd�ih Q���fd�i�djspqbv_ E ]�p`]qbv_���E©�^p�lk]q¤ c�bdo9bdl4j� v¤n^p�bdc{f-l�]qb=� b=u)f-� �^�ieG�ib�a�� j��v��]�ckb=�^r�j��Gl���j��x��]�j��Glkpq��c�j�hi�i�s]q�^�3lk\ij�bvh^b=r�j�lkc��j�c�c�bdcs� H � H �*< : H �9j�h^]�l�b=c�� ��9| }�w�9�d|C�Cy-�9�-� iy����N| �¨y�� �$}$w�}$� 4�}����6; w$| � � 4��C�-� ��9| }�� �� �q�v}$� 4{}$�N�db=p��^r�j @���n^fd�djs_ 8 7 LTP 6C8 7 L&?�� = f-�=b��vfi� �vf-n^fd���j:��i�=�^_kl�8 � �&GG�*E©b=ck�vf-���f-�Nu�r�fd�i�©§¨�ioNp�]`_�\^j�c{_�� H �i�=�

� 7 8 H b=fd�i�^]`_�E��0B�j�e�pqjs]ql�]�_���> c�j��=bdck� �)�ih^j�eO��fd�ih+Q¨��f-�i�=j�p�b=_ ES]`p�]�b=_��rE©�^p�lk]q¤ c�bdo9bdl��bvp�p`f-o9bdc{f-l�]qb=�u�b=c�c�bdoi�^_kl�j� GnNpqb=c�fdlk]�bd��� H �"7Zwky�4{}{}������G��1�y#���$�N|¥}$w�O�-| �Cy��9�d� �¨y�� ��$}�wk}$� 4{}*��� 5�ydz{yd|C�!401*����� ;&/i|¥y��m�-| �Cy��^�!o�f-��%ic{f-�i�s]�_k��bN�!< :&��:)nic�]`p�@�P/P P^�

� 7 @ ��f-lk\^p�j�j��YB�b=rxfd�^]qeO� > j�b=r�j�lkc�]��*nic�bdoN]q�i�f-�ih3l�js_�l�]q�i�:6Sf@_k�ic��dj$�v��[Uj��\i�^]���fdpB�j�nNb=ckl �T?�¤ LT@��2� H Ey:b<Fo9�!B��il��=j�c{_ N)�^]��dj�c{_�]ql¥�v�!ovj�nilkj�r*oNj�c 8 � �/?��

@ @

Page 23: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

� 7 7 =)]��\ibvp�f=_ B�b��=��£�b=p`u�c�f=r J��ic��=f-c�h�� �¢]qj�l�j�c�%ib�  �df-�^hrovj�o^fd_klk]`f-��[Z\ic��i��� <;bvfd_klkfdp�^f���]��=f-l�]qb=��6�c�bdo9bdlmr�bdlk]�bd�ga�]�l�\g�i�i��j�c�lkf=]q�Gl¥�=� H � ;�;�; �����-������"��)�iy 1� / ��7Z�q�-�^�N���v�.>X� | 7 �� ��7 1��8�� � ��

� 7 L =)]��\ibvp�f=_ B�b��=��£�b=p`u�c�f=r J��ic��=f-c�h�� �¢]qj�l�j�c�%ib�  �df-�^hrovj�o^fd_klk]`f-��[Z\ic��i��� <;bvfd_klkfdp�^f���]��=f-l�]qb=�x¤¦r�b=o^]�p�j)c�bdo9bdl;�^f���]q�vf-l�]qb=��a�]�l�\@�i�i��j�c�lkfd]��Gl¥�x]��7hi�G�Nfdrx]q� j��G��]qc�bd�i¤r�j��Glk_�� H � � ���� �¨y�� �$}$wk}���4{} y��/5�y=z�yd|C�!4013��� �";&/i|¥y��m�-| �Cy��N� �)j�l�c�b=]�l��`E H �8 � � �^� H Q Q Q"§¨c�j�_�_��

� 7 ? [4�`ov�\ij�c{r�j�cs�cB�j���j��Gl7c�j�_k�^p�lk_7]�� fdcklm�vfdp`pqj�c�]�j�_�� 7�wky 4�}{}������G��1 y �S|! G} � �������P!q �&x $98 7 �L�6C8 7 � �i�ToGj�nilkj�r&o9j�c[8 � �T@��

� 7 � < � Q�� ov\^fd�i�ibd�U� � G} �3�d| �}0�m�-| �!4{�-��� �}�y-w�" y#� �¨y � � /��^�!4{�-| �Cy��N���\^fdnil�j�c�:ESfdl�\ijsr�fdlk]���f=p�[Z\ij�b=c��&bvu <;b=rxr*�i�^]���f-l�]qb=���jN��N]q�=j�c{_k]�l¥�&b=u H p�p`]q�^b=]`_'§�ckjs_k_��*8 � L �^�

� 7 G �v]�fd�vo9b ov\^]9f-�^h <�fdc�p�b&[�b=rxfd_�]C��> bGbGhxu�jsf-lk�ickjs_¨lkb�l�c{f-�eO� H � � ����� �¨y����$}�wk}���4{}y����¨y�� � /i|¥}�w �U� 1�Cy-���-��� 7��-| |¥}$w� 5�} 4{yk�d�^� | �Cy��N� ovjsf-lklkp�jd��£=:*� �d�i�^j 8 � ��LN�H Q Q Qg§¨c�j�_�_��

� 7 B��*oG]�r� E©b=o^]�p�j�c�bdo9bdl¨p�bv�sfdp`]q�sf-l�]qb=�xu�c�b=r pqjsf-c��ij�hmp`f-�ihNr�fdcke�_��!ESf=_�lkj�c � _¦lk\ij�_�]`_��E©�z>¢]�p`p�N��N]q�=j�c{_k]�l¥�=�jE©b=�Gl�c�j�fdp �!<�f-�Nf-h^f^� �=�^pq�=8 � � i�

� 7 � B���oG]`r/fd�ih >�� � �ihij�e9�`ESbdo^]`pqj�c�bdo9bdl pqbG��f=p�]���fdlk]�bd�©u�c�b=r p�j�fdck�^j�h©p�fd�ih^rxf-c�e�_��H �.7�wky 4{}{} �-���v� 1¢y � | �}�� ������� 5�� �¨y����$}�wk}���4{}�y�� ��9| }���� �q�v}$�9|05�ydz{yd| 1 �-��� � " 1 �|¥}0� 1�� � 5 � ����� �4]q��l�b=c�]`fi�!<�fd�^f-h^f^� ��lkbdo9j�c�8 � � i� H Q Q Q"§¨c�j�_�_��

� L/P B�� oG]`r f-�ihl>�� � �ihij�e9� ��jsf-c��^]q�^� f-�ih j���fdp��^f-l�]q�^� ��]�_k�^f=p�u�j�fdl��ic�j�_�u�bdc�nNbv_�jj�_klk]`rxf-l�]qb=��� H � 7�wky 4{}{} �-���v� 1my#��|! G} �O} ��}��N|! � ������$�N|¥}$w�O�-| �Cy��O�-� �¨y����$}�wk}���4{}y�� �¨y�� � /i|¥}$w ��� 1{�Cy-��� � ����������4j�c�e=��c{fi��> c�j�j���jd�i_kj�nil 8�� � �^� H Q Q Q"§¨c�j�_�_��

� L�8 ovlkj��dj��Yove�]�j��^f^� H �Glkj�c{f-��l�]q�=j�ckj���b=�^_�lkck�^��lk]�bd���]�f~�dj�bvr�j�l�c{]���nickb=o^]��i�^��7�wky 4{}{} � ����v� 1*y �&|! G} � ������ P�q �Tx $98 7 ��L 6 8 7 7 �/ovj�nilkj�r*oNj�c 8 � �/@��

� LT@ B�fd�ih^fdp`p < �0oGrx]qlk\�fd�ih3§¦j�l�j�c <;\ij�j�_kj�rxf-���� �3l�\ij�c�j�nic�j�_kj��Glkfdlk]�bd�Sfd�ih js_�l�] ¤rxf-l�]qb=�@bvu�_kn^f-l�]�f=pO�i�ihij�ckl�fd]��Gl¥�=� ��N|¥}�w�9�-| �Cy��O�-� iy /�w�9�-��y ��5�y=z{y-| �!4�1�5�}01�}{��w 40 =�?jq�L&x�$ ? � 6 � �� 8 � �i�

� L 7 B�]��\^f-c�hio9�Oov�ilkl�b=��f-�ihW:��ih^ckj�a >��0JZfdcklkb^�95�}������$y-w 4{}0�m}$�9| �}{��w�^���G�-�KE H [§¨c�j�_�_�� 8 � � ��

� L L �&� ���'['f-�vf-c�jd� �*� ����E©� �)j�c�r�bdlkl��¨fd�ih �&� � ]`f-bN� �¢]`_��Nfdp;n^p�fd��j~c�j���bd�=�^]ql�]qb=�u�bdc@f-�ilkbd�ibvr�b=�^_mc�bdo9bdl�_�� H � 7Zwky�4{}{}������G��1 y � | �} � ����� �$�N|¥}$w�O�-| �Cy��O�-� �¨y�� ��$}�wk}$� 4{}�y-� 5�y=z{y-| �!4�1&��� � ;&/i|¥y��m�-| �Cy��� � ��5 ;���� ����^n^f-�=j�_F@ ? 7 P 6j@ ? 7 ?��=§�]`_��sf-l ¤f�aZfs�=�*ESfs�=8�� 6*@ P 8�� � i� H Q Q Q <;b=r�ni�^l�j�c oGbv�s]qj�l¥�=�

� LT? :&�O['f-c{f-�Gl�bvp�f^� �� ��}�w 1�}87Zwkydz��q}�� � �}�y-w�"��`Q�p`_�j���]qj�c ov��]�j��i��j*§¨�^o^p�]`_k\ij�c{_`J¢� �����:)rx_kl�j�ckh^f=r�^[Z\ij =�j�l�\ij�c�p`f-�^h^_�� 8�� �i�

� L � <�fdrx]`p�p�b��N��[�fs�ip�bdcUfd�ih �¢f���]qh�^� �4c�]�j��vrxf-���TQX �n^p�bdc{f-lk]�bd��_kl�c{f-lkj��=]�j�_�u�bdc¦r�bdo^]`pqjc�bdo9bdlk_�� H � � ���� ��N|¥}�w�9�-| �Cy��O�-� �¨y�� �$}$wk}���4{}Sy-��5�ydz{yd|C�!401��-��� ;&/i| y �x�d|C�Cy-�^�:Zlkp`f-�Gl�fi� > :*��ESfs�=8�� � 7 � H Q Q Q"§¨c�j�_�_��

@ 7

Page 24: 0 $motionplanning/papers/sbp... · [Z\^]`_7a bdc egf-hihic j _ _kj _mlk\ij3n^ckb=o^pqjsrtbvuxwkydz{yd|~} $ 9 y wk d|C Cy- ^ [Z\^f-l ] _ lk\ij3l fd_ke bvu f- ^l b=rxfdlk] fdp`pq q}{

� L&G o9�v[Z\ick�^��� ��� %^b� O�vf-�ihx£��TJ;�^ck�vf-c�h���:�nic�bdoNf-o^]`p�]`_klk]���f-ninic�b=fd�\*lkb���bd�^���ic�ckj��Glrxf-ninN]q�i�f-�ih3pqbG��f=p�]���fdlk]�bd�Su�b=c¢r�bdoN]�p�j*c�bdo9bdl�_�� ��� 4� v���9}��}{��w�N���v�d� 7 8 $A@ ��6*? 7 �8 � � i�'f=p�_kbmf-nin9j�fdckj�h@]��e:)�il�b=�ib=r�b=�^_FB�bdo9bdl�_b?���@/? 7 6*@/G*8d�

� L ovj�o^fd_klk]`f-�*[Z\^ck�i�U�TJZfs�=js_k]`f-��p`f-�ihNr�fdcke4p�j�f-c��^]��i� u�b=c�r�bdoN]�p�j;c�bdo9bdl�pqbG��f=p�]���fdlk]�bd������ 40 =���O} U}{��w�N���v�d� 7 7 $ L!8d�C8 � � i�

� L � ovj�o^fd_klk]`f-�*[Z\^ck�i�U� �Uj�fdck�^]��i�)r�j�l�c{]q��¤ lkbdn9b=p�bd�=]���f=p-rxf-n^_'rxf-n^_'u�bdc¦]��ihibGbdc¦r�bdo^]`pqjc�bdo9bdlZ�^f���]q�vf-l�]qb=����; w$| � � 4��C�-� ��9| }���� �q�v}$� 4�}��in^fd�djs_`@*8�6�G&8=� �vf-�G�^fdc��y8�� � �^�

� ?�P ovj�o^fd_klk]`f-� [Z\ic��i���Z£�b=p`u�c�f=r J��ic��=fdckhU��fd�ih � ]�j�lkj�c�%^b�  � : c�j�f=p ¤¥lk]`r�jSfdp��dbd¤c{]qlk\^r u�b=c*r�bdo^]`p�jmckb=oNb=l&r�fdnin^]��i�©a�]�l�\�f-n^n^p�]���fdlk]�bd�^_&l�bSr&�Npql�] ¤¥ckb=oNb=l&f-�ih 7 hrxf-ninN]q�i�N� H � 7�wky 4{}�}��-���v� 1 y ��|! G} � ���� ��9| }�w�9�d|C�Cy-�9�d� �¨y�� �$}$wk}���4{}4y�� 5�y=z�yd|C�!401��� �:;&/i|¥y��m�-| �Cy��N�!oGf-��%ic{f-�^��]`_���b^�!< :&�!ESfs�K@�P/P Pi� H Q Q Q"§¨c�j�_�_��

� ?*8 §¦�'£�\^fd]�l�jf-�ih %��X§¦� %^j�c�c�]�jd�Y:��ilkbd�ibvr�bd�^_*j$ �n^p�bdc{f-lk]�bd��$ � c�]��dj���ov���i�i��j�c�¤lkf=]q�Gl¥�v� H �67�wky 4{}�}��-���v� 1�y �&|! G} �¨y�� �$}$wk}���4{}xy�� �¨y�� � /i|¥}$w ��� 1�Cy�������� 7��d| |¥}$w�5�} 4{y��-�N� |C�Cy-�^��n^fd�djs_ 7 7 � 6 7 L �^� �Ub=_K:)p`fdrx]qlkb=_�� < :&�gN o*:&� �d�^�iji8 � ��LN� H Q Q Q<;b=r�ni�^l�j�c ovbG��]�j�l¥�@§�ckjs_k_��

� ? @ §¦j�lkj�c;£�\^fd]�l�j=�); 40/�w�Cy /21&�m� 40 =���9}��$y�w&��/i|¥y��Oy��my�/21��s� 1�/^�-� }��$�O�qy�wk�d|C�Cy-�^�'§¨\ �l�\^j�_�]�_���E©�z>¢]�p`p�N��N]q�=j�c{_k]�l¥�=�jE©b=�Gl�c j�f=pC� �¢�� j�oNj��d� 8 � � i�

@zL