- Email: [email protected]
The trajectory generation algorithm for the cable-suspended parallel robot—The CPR Trajectory Solver
Accepted Manuscript The trajectory generation algorithm for the cable-suspended parallel robot—The CPR Trajectory Solver Ljubinko Kevac, Mirjana Filipovic, Aleksandar Rakic
PII: DOI: Reference:
S0921-8890(16)30599-1 http://dx.doi.org/10.1016/j.robot.2017.04.018 ROBOT 2838
To appear in:
Robotics and Autonomous Systems
Received date : 27 September 2016 Revised date : 21 April 2017 Accepted date : 27 April 2017 Please cite this article as: L. Kevac, et al., The trajectory generation algorithm for the cable-suspended parallel robot—The CPR Trajectory Solver, Robotics and Autonomous Systems (2017), http://dx.doi.org/10.1016/j.robot.2017.04.018 This is a PDF file of an unedited manuscript that has been accepted for publication. As a service to our customers we are providing this early version of the manuscript. The manuscript will undergo copyediting, typesetting, and review of the resulting proof before it is published in its final form. Please note that during the production process errors may be discovered which could affect the content, and all legal disclaimers that apply to the journal pertain.
- 0 $- #'%( . (** +)(' #'! -,"(* "))& ) & -+# & (!#( +#(! (#/ +,#-2 ) &!+ .& /+ %+&$ & %,( + &!+ + # (()/-#)( (- + ) "))& ) & -+# & (!#( +#(! (#/ +,#-2 ) &!+ .& /+ %+&$
& %,( + &!+ + # &$. #(%)% / # - +,
#*$ ' #&#)(.# #"$&).*#((,-#-.- (#/ +,#-2) &!+ )&!#( &!+ + # '#+$( #&#*)/# *.*#(+,
& %+ ' * %# "))& ) & -+# & (!#( +#(! (#/ +,#-2 ) &!+ .& /+ %+&$ & %,( + &!+ + # +%# - +,
+,* , " +$ -)+2 )&/ + #, *+) .+ #( #( -"#, ** + 0"# " #, ., -) ! ( +- ,'))-" + + ( -+$ -)+2 ) ,2,- '4, ' + 0"# " ", -,% -) ')(#-)+ ( -+ % -" ) $ - #( + & -#' "/ ! ( +- - , ) ).+*+#'#-#/ -+$ -)+# ,*+#'#-#/ ,0"# "+ "), ( 2-" +$ -)+2 )&/ + .+#(! -" ! ( +-#)( ) ' +4, )'*& 1 + + ( -+$ -)+2 )+ -+$ -)+2 ! ( +-#)( -" +$ -)+2 )&/ + ., , -" %()0& ! ).- -" .++ (- *),#-#)(, ( / &) #-2 )+# (--#)(,) -" ' +( ) $ -( -" (#- #( ,-" !)&*),#-#)(( / &) #-2)+# (--#)() -" ' + " +$ -)+2 )&/ + ")), , )( ) -" ! ( +- *+#'#-#/ , )+ #(- + )(( -#(! -" .++ (- ( !)& *),#-#)(, ) -" ' + - + )'*& -#(! -" "), ( *+#'#-#/ -" +$ -)+2 )&/ + ,- &#," , -" ( 0 *),#-#)(, ( / &) #-2 )+# (--#)(, ) -" ) $ - "#, *+) ,, #, + * - 2 &# &&2 .(-#& -" + & -#' ) $ - ')(#-)+#(! ( -+ %#(! -,% #, )'*& - )+ -" *.+*), ) (&23#(! ( .,#(! -" #( &!)+#-"' 0 "/ ,2(-" ,#3 *+)!+' * %! +$ -)+2 )&/ + 2 .,#(! -"#, *+)!+' * %! -" ,#'.&-#)( 1* +#' (-, ) -" ' +4, -+$ -)+2! ( +-#)( )+-" *.+*), ) -" ) $ -')(#-)+#(!( -+ %#(!+ *+ , (- 2.,#(!-" +$ -)+2)&/ +')-#)(.-)()'2) ,2,- '4, ' +#,#( + , " ' +",-,%-) )&&)0( ')(#-)+-" ")-# &&2')/#(!) $ - /(* + ,2,- '+ &-#' -+$ -)+2*&((#(!) $ -')(#-)+#(! 1.
',*( - ,#('
& -,.,* ( +&& &) )-,,2,- ',+ )'*& 1+) )-,2,- ',0"# "., & , )+')-#)( -+(, + " , & , + )(( - -0 ( -" &) -))& ' + - ++# + ( -.-)+ ,. ,2,- ' ')-)+-! +-0#( " ,2,- ', )+ # + (- *.+*), , "/ ( #( ( ,")0( #(
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
-
+1%&0" 1&,+4"4&))!"#&+"1%"1/ '" 1,/6$"+"/ 1&,+ )$,/&1%*-1%"/ '" 1,/6,)3"/1%"* &+ ,+1/& 21&,+ ,# 1%" - -"/ ,/ 1%" -2/-,0"0 ,# + )60&+$ +! 1"01&+$ 1%" -/,-,0"! )$,/&1%* 4" % 3" 0")" 1"!1%"0601"*0%,4+&+&$ 4%"/"4"0%,4 3&"4,#1%" %,0"+0601"*4&1% *"/ %&00601"*4 0-/"0"+1"!&+ &+!"1 &)"% 3"1"01"!1%" )$,/&1%*,+1%"0601"* ,+)6 21&1 + "20"!4&1% +6(&+!,#1%"0601"*4&1% *"/ "$4&1%1%"-0601"*
$%&"' " ' & #( "#' ' ' #' " - &+&'!& % ("%-#"&'% " &+&'!& * % %'% , + !%-& " " ' #(% ($$% $# "'& # $% $ $-& $ *#% &$ #%' $(%$#&#%) '+ % "'%** ' &&+&'!&& !$ + &+&'!
(% &+&'!-&!%'% ""!#" '#% " #' +!#) "# ' ) * ' ' ) *#' !%'% '#%+' ' ) *## '-&$#& ' #"&
" ,$ .),3)&0 ,-)&0 -." *,) & ') , , ( .,$ .),3! ( ,.#)() ." -3-. '5- ' ,1"# ""-.-%.) )&&)1( ')(#.),." ) $ .1"# "#-#(." ' ,5--#!".# - &)- - *)--# & .)." (.,&*)#(.) ." -#-) ." ' ,5--#!".1"# "#-) ." -"* ) *3,'# ), -)&0#(! ."#- *,) & ' 1 "0 /- ." , -/&.- ,)' 1"# " !/,(. .". . " ')' (. ." *)-#.#)(( 0 &) #.3),# (..#)() ." ' ,#-%()1(, %#(! "#*#-/- ),' -/,#(!*)-#.#)() ." ' ," ., %#(! "#*#--'&& 0# .".#-.. " .)." ' ,( , &3-#.-*)-#.#)( %.) )'*/. , )(.,)&-3-. '"#-., %#(! 0# )/& #( &/ '!( .# #( ,, #)- , +/ ( 3# (.# # .#)(, & .),-)(,),)." ,.3* ) ., %#(!. "()&)!3/.*)-#.#)(( 0 &) #.3),# (..#)() ." ) $ ., %()1()(&3. ,.#(')' (.- &-)#.#-( .".." ) $ .#- &13- #( ' ,5- -#!". #." ." - --/'*.#)(- ." ,$ .),3 )&0 , (-/, - -'))." ( )(.#(/)/- , , ( .,$ .),3 ) ." ' ,5- ').#)( #( .#' ( -* 1"#& ." ) $ .5- ').#)( .,$ .),3 ( ").# ), ." */,*)- ) #'*& ' (..#)( ) ." .-% ) ) $ . )&&)1#(! 1 "0 *, -/' .". ." -3-. ' " -/ # # (. ( ,!3 , -)/, - -) #. )/& )&&)1 ." ) $ .5- "#!" -. 0 &) #.3 &-) /,#(!." #'*& ' (..#)() ." .-%." -3-. '"-*)--# #&#.3) & ,.#(!( & ,.#(! ." ' , * ( #(!)(." ) $ .5- 0 &) #.3 "(! /,#(!." ' ,5-.-% 2 /.#)( "#- ' (- .".." ' ,5-0 &) #.3( -.) )&&)1." ) $ .5-').#)(.(3')' (. ), #(!.)." )( #.#)(- ) ." ,$ .),3 )&0 ,5- /( .#)(#(! &-) & . (-#)( ), - 1" , ", . ,#4 - ." ),, -*)( #(! & '/-.&13- &#'#. -).".." 3-.#- 3 ( ,!3, -)/, -) ")- ( ./.),- - ( "0 *, -/' .". ." ) $ . ')0 - )(&3 . ." &)1 -. *&( ) ." ' ,5- 1),%-* "#-*&( #-& && - p 0 #(#!-( "#-*&( #-)(& 0 & .,.#(!*)-#.#)() ." ' ,#- #( 3." )* ,.),-).".." ' ,#- )0 ." /,, (.*)-#.#)( ) ." ) $ . ..".')' (.." )* ,.), "))- -').#)( #, .#)() ." ' ,
. ,1, - ." )* ,.), -.,.- ." *,)!,' 1"# " 3 #(#.#)( -.,.- 1#." ." & ,.#)( *,) -- *,) -- ,)' 4 ,) .) ." -.,.#(! , , ( 0 &) #.3 ) ." -3-. '5- ' ," -.,.#(! , , ( 0 &) #.3 ) ." ' , #- *, -/' 0 ,! 0 &) #.3 ) ." ) $ . /,#(! ." #'*& ' (..#)( ) ." .-% ' ,5- , , ( 0 &) #.3 ( "(! * ( #(! )( ." ) $ .5- ').#)( ( 0 &) #.3 0,#.#)(- . , ." -.,.#(! ')' (. ." ,$ .),3 )&0 , .% - )(.,)& ) ." ' ,5-').#)(" ')(#.),#(!( ., %#(!-3-. ') ." ) $ .#- )'*& . &3/.)()')/- /,#(! ." ) $ . ., %#(! *,) -- - )( ." /,, (. *)-#.#)(- ( 0 &) #.3 ),# (..#)(- ) ." ' ,( ) $ .." ,$ .),3)&0 , "))- -)( ) ." *,#'#.#0 - ,)'." . - ( #( .". -"#)(." ' , )&&)1-." ) $ ."#-*,) --#-, * . /(.#&." )* ,.), # -.)-.)*." ) $ .., %#(!.-% ..".')' (.." ,$ .),3)&0 ,-.,.-." ' , & ,.#)(*,) -- *,) --( #.-&)1- )1(." ' , ,)'." , , ( .)4 ,)0 &) #.3 " ,$ .),3)&0 , (/- )( ) )/,*, #( *,#'#.#0 .,$ .),# -*,#'#.#0 -1"# ", -.), #( . - (#!1 -")1.", *,#'#.#0 -1"# " ( /- 3." ,$ .),3)&0 , /,#(! ." ) $ . ., %#(! - )( ." /,, (. *)-#.#)(- ( 0 &) #.3 ),# (..#)(- ) ." ) $ . ( ' ," )/,."*,#'#.#0 #--.,#!".&#( #( - These primitives are defined in the local coordinate system, but the CPR Trajectory Solver can use the .,(- ),'.#)( '.,#2( '*." '. (3*&( ) ." !&) & )), #(. -3-. ' ) ." ' ,5- workspace. The A C C and D E C processes can be used by the CPR Trajectory Solver not only by the operator but also at the moments when the object suddenly changes its velocity and/or when the object is
($$!$'($( $&&$( "&.'' (( '%&')"( ( A C C and D E C processes can only be executed via P4 primitive.
)& %& " ( *' ( &'( %& " ( * ( '$# %& " ( * # ( ( & %& " ( * & " ( *' +& $'# '$ ( ( ( - $*& !! ( %&')" %$' ( $#' # *!$ (- $& #(( $#' $ ( "&#$ ( )& # ( (' ,)( $# $&( ' $!& (-$%&'#(( $# %& " ( *' - + !! ,%! # ( &$) $!!$+ # (+$!' ! '& ' #&! !$+ $ ,)( $# $ ( & ($&- $!*& $& #- $ ( & "#( $# %& " ( *' & " ( * ' #&!!- $"%$' $ !"#(' *# #! $ #( $( $# $( $# $( $# $( $# $ #(
$' ( $#$( "&(( "$"#($!'(%& " ( *.'$"%!( $# "& %' $#"$* # !$# '(& ( ! ## ( & ($&-$!*&'(! ' ' ( #+%$' ( $##$& #(( $#$( $ ( '$#( (#)&&#(%$' ( $#$( "& ( #'"&.' $!%$ #(# $$''( %& " ( * $( $#!$# ( '(& (! # $##( # ( &( $#$"&.'"$( $#+ ( &( $#$$ (.'"$( $# "& %' $# "$* # !$# '(& ( ! # )#( ! ( & ' %$' ( $# $* ( !'( #$+#%$' ( $#$( $ ( '(%$ #($&,)( $#$)&&#(%& " ( *#( &'(%$ #($&( #,(%& " ( *
! #&!'& %( $#$%& " ( *'- ' (#'# # %& " ( *'-& #+ ( &( ##)"&$'(& (! #'#&' Table 2 gives the conditions when and how the CPR Trajectory Solver chooses on of primitives P1, P2 and P3 from its data base. Primitives are defined in Fig. 2 in local coordinate system. & " ( *
)&
"&#$ ($& #(( $#
(.'%$' ( $# #&!( $#($( "&
! ! !& & # !% ! ! % !
! !& & # !% ! ! ! ! # ! !
! !& & # !% ! ! ! ! # ! !
! ! !
! !# -
" !! ! %
! "!" ! % ! !! $ ! !$ & ! $ in Fig. 3. Detailed presentation of the CPR Trajectory Solver is given with the flow chart in Fig. 4. At any moment, the system checks if the operator has activated the CPR Trajectory Solver. If the operator has not activated the CPR Trajectory Solver, the whole system is in idle state. When the operator activates the logic by pressing the ST A R T button, the CPR Trajectory Solver used for object tracking and reference trajectory generation is started. The process of reference trajectory generation repeats itself and the camera is able of monitoring and tracking the object. However, the structure of the system is much more complex. The system consists of the following blocks: C PR System, PD Controller and the C PR T rajectory Solver. Also, the system contains a Safety button which can be used by the operator in case of an emergency. Reference trajectory of camera in 3D space is defined in real time. Based on these variables and the real
trajectories of the external coordinates xc , y c , z c , by using the PD Controller the control signals are determined and the C PR System with the camera performs the real trajectory by following the generated reference trajectory. 2.1 Description of general form of the CPR Trajectory Solver The topic of this paper is definition of the CPR Trajectory Solver and it will be explained in detail: a) Starting the program. This procedure is performed by the operator based on his/her visual estimation of the current position and velocity orientation of the object. Before starting the object monitoring and tracking process, the operator places the camera at the position so that the object is close to the cent # b) A cceleration process. The operator starts this process by pressing button ST A R T. With this command, the CPR Trajectory Solver starts the A C C process and P4 primitive. Camera starts to move in direction of c ! # ! ! # !
#
e velocity is executed. c) T he procedure and conditions of choosing the primitive. Next 0.2 [s], the camera keeps moving (P4 primitive) in the same direction as during the acceleration and in that period it establishes the current position and velocity orientation of the object " O bject of interest. At that moment, the CPR Trajectory Solver also knows the current position and velocity orientation of the camera. Based on these data, the CPR Trajectory Solver defines the next goal point of the camera in its workspace. In the next step, the CPR Trajectory Solver decides if it is needed for the camera to change its velocity magnitude.
# ! ! A C C and D E C processes and primitive P4. Afterwards, the CPR Trajectory Solver chooses the next primitive from its data base. Then, it chooses the plane in which the primitive should be executed and transforms the primitive to the coordinates of the global Cartesian coordinate system. By using the chosen primitive (P1 " P4), the CPR Trajectory Solver guides the camera towards its goal position. d) E xecution of the new primitive. The chosen primitive executes over certain number of sampling intervals. During the execution of the primitive, the CPR Trajectory Solver constantly reviews the position and velocity orientation of the camera relative to the goal point of the primitive. The chosen primitive is executed until the camera reaches its goal point. The CPR Trajectory Solver establishes this by constantly checking if camera has finished the previously defined primitive, i.e. reference trajectory. In the period of execution of any primitive, the position and velocity orientation of the object are unknown. The procedures c) and d) of the CPR Trajectory Solver are executed cyclically during the tracking and monitoring the object in real time. e) Stopping the program. This routine is started by the operator by pressing E N D button. In this way, the CPR Trajectory Solver runs the D E C process along P4 primitive which is in the direction of
# D E C process. The camera slows down from the reference to zero velocity and the tracking and monitoring process is over. As it can be seen in Fig. 4, after execution of each primitive or choosing the A C C or D E C processes, the CPR Trajectory Solver checks if the operator has activated the E N D button and in that case the
whole process of object tracking and reference trajectory generation is stopped. When the operator activates the E N D button, the CPR Trajectory Solver waits for the current primitive to finish and after its execution it starts the process of stopping the object monitoring and following. At any moment, execution of the primitive can be stopped when the operator activates the Safety button.
Figure 4. Flow chart of the CPR Trajectory Solver.
2.2 An example of using the CPR Trajectory Solver In order to facilitate understanding of the CPR Trajectory Solver, we will show a detailed description of one example !( ! ! ! $ ( ! ! % sequentially defined, see Figs. 1, 5, and 6. Also, refer to the Table 3. This task includes: 1) Starting the C PR system with the camera at the point I c and moment t c 0[ s] . Based on ! ( !! !( # !% !! he/she defines line t1 which is parallel to the estimated direction of the object's motion. Line t1 is positioned on plane p a which is parallel to the plane p 0 . The operator places the camera at the position so that the object is close to the central point of the ! ( ! ! ! ! ! % # % !ST A R T button, see Fig. 4. The camera starts its motion along line t1 , starting from point I c . From that moment, the CPR ! % # ! ! ! ! ( ! ! ! "! " General description of this process is given in item a) of subsection 2.1. 2) A cceleration from point I c towards point II c moving along line t1 . In this period, camera of the % ! ! & ! ! ! ! ( # !% v c along line t1 (P4 primitive). The CPR Trajectory Solver uses the A C C process. This interval is orange coloured in Figs. 1 and 5. General description of this process is given in item b) of subsection 2.1. 3) Detection of the cur rent position of the object at point I o at moment , and detection voI o . From point II c , the camera moves with a velocity v c and during the period of 0.2 [s] (P4 primitive), it detects the current position and velocity orientation of the object ' point I o , see Figs. 1, 5, 6 and Table 3. At that moment the camera is at point II co . Based on the current position and the velocity orientation of the object and the camera, the CPR Trajectory Solver defines a new goal position and velocity orientation of the camera and that is the point III c ( x III c x I o , y III c y I o , z III c z II co ) . Also, the CPR Trajectory Solver decides that there is no need for ( # !% !" ! ! ! ! ! ! III c , i.e. the direction of the line t 2 which belongs to the plane p a , is parallel to the motion of the object, i.e. it is parallel to the line o1 which is on plane p 0 . The motion orientation of the camera at the point III c is the same as the motion orientation of the object at the point I o . The motion orientation of the object is defined in comparison to the direction of x, y axes of the global coordinate system: x negative and y positive. Based on these data, the CPR Trajectory Solver chooses one of the primitives via which the camera of the CPR system traverses from point II co to the next goal point, III c . General description of primitive choosing process is given in item c) of subsection 2.1. The chosen primitive is defined in Fig. 2a) (primitive P1) and the CPR Trajectory Solver has decided that it should be performed in horizontal plane p a ! ( $ ! # $ " General description of primitive execution process is given in item d) of subsection 2.1. 4) Detecting the cur rent position of the object at point II o at the moment and
voII o . From point to point , on line t 2 , the CPR Trajectory Solver has a task to recognize the position II o and velocity orientation of the object voII o which is on line o 2 . Also, it has a task to choose one of the primitives from its database. The CPR Trajectory Solver concludes that the object is at the same point as it was at the moment ,
while the object is moving in the opposite direction. Because of this special situation, the CPR Trajectory Solver is designed so that the camera can traverse to the next goal position in the vertical #& &'"'# !("" " !'# !("" " #"& !" CPR Trajectory Solver chooses the descension trajectory, because the z position of point III co is ! #% ' %# # " #"##
!# !' %! " !(" & goal point IVc ( xIVc xII o , y IVc y II o , z IVc z II co ) which is on line and belongs to plane p c . Also, #
!# !' %! " ## #! " ! !(" % #' #$ For camera to move from point III co to point IV c , the CPR Trajectory Solver chooses the appropriate primitive which will be performed in plane p c . The logic chooses the P1 primitive which was defined in Fig. 2a). See item c) of 2.1 subsection. The interval of the trajectory from point III c to point IV c is black coloured in Figs. 1 and 5. Primitive execution is generally described in subsection 2.1 under item d).
$! #!# !' ! # 5) Detecting the cur rent position of the object at point III o at the moment and
defined by line o3 . $! # !(" # ! point IV c to point IV co along line t 3 , the CPR Trajectory Solver establishes position III o and velocity orientation of the object voIII o which belongs to line o3 . It defines the next goal point of the camera V c which is on line t 4 and plane p d . Also, the CPR Trajectory Solver decides that there is no need for !("% #' #$ Based on these data, like it was described in item c) of subsection 2.1, ! !("! "" ! # IV co to point V c the CPR Trajectory Solver chooses the primitive P2 which needs to be executed in plane p d like it is described in item d) of subsection 2.1. The interval of the trajectory from point IV c to point V c is red coloured in Figs. 1 and 5. 6) Detecting the cur rent position of the object at point IV o at the moment and
detecting the velocity orientation of the object
defined by line o 4 . After completing the previous
primitive, the CPR Trajectory Solver leads the camera along line t 4 towards the point V co by using the c) process from subsection 2.1. During that period, it recognises new position IV o and velocity orientation of the object v oIVo - line o 4 . Now, the CPR Trajectory Solver %&" !$%& $-%sight and because of that it reacts and chooses the ascension trajectory of the camera which is executed in vertical plane p e . Based on these information, the CPR Trajectory Solver defines the next goal point of the camera VI c ( xVI c x IVo , yVI c y IVo , zVI c zVco ) , which is positioned along line t 5 and in plane . Line t 5 is parallel to the line. %"& $ &"$+" ($ %&&&$ %!"!"$ $-%( " &+ magnitude change. By knowing the current position and velocity orientation of the motion of the camera and object, the CPR Trajectory Solver chooses the next primitive which will be used for moving the camera from point V co to the next goal point VI c . The chosen primitive, defined in Fig. 2b), is P2 primitive. The interval of the trajectory from point V c to point VI c is light blue coloured in Figs. 1 and 5. See item d) in subsection 2.1. 7) Motion of the camera from point VI c to point VII c along line t 5 in plane. Camera is moving from point VI c to point VI co and the CPR Trajectory Solver, by using the process c) from subsection 2.1, establishes next position V o and velocity orientation of object voVo . However, the operator decides to stop the tracking and monitoring of the object before the camera reaches point VI co . It is the moment when the operator gives a command for stopping the camera by pressing the E N D button. At that moment, the #$"%% " $-% $& "! % %&$& & )% %$ ! & $" %'%& "! 2.1. The camera finishes its motion at point VII c ( xVII c , yVII c , zVII c ) and monitoring process ends. The period of the trajectory from point VI c to point VII c is violet coloured in Figs. 1 and 5. In this example, we have shown the sequential generation of a complex reference trajectory in real time. This is only one of the randomly selected possible combinations of primitives P1 P4 which are used by the CPR Trajectory Solver. Everything is conditioned by the sequential detection of chaotic motion of the object at certain moments and by the current motion of the camera. 3.
"$&#'$#"%"( & "!"& ! & & "$ ' & "!%"& &$ &"$+%" ($"$ !$& "!"&$$!&$ &"$+"&"%! %+%& -% $ !&!"$$ & " & &$ !)(%+!&% ,&#$"$ # ! % #$"$ # "!% %&% " %($ '&' +"!!&'! &% &$ &"$+%" ($"$!$& "!"&$$!&$ &"$+ %%'$"'& ! %%"!& &&& %+%& -% $!%&" "! &"$!&$ &" & ) "(% "& + ) & ! & $-% % & ! $ & '$"'& ! "$ !$& "! " & +! %"&"%! %+%& !'$"'& !"$!$& "!"&"!&$" %&$'&'$'$ !& $-%$$!&$ &"$+" ") ! &%%'$"'& !%$ '&' + +"'# !&!+ " !&&+$$+&"& "($ !& "'$%" # !&& "!"& " $-%&% "! &"$ !!&$ !& "& "!"&" & !)& " ")%)) #$%!& &$%' &%"&% ' & "! *#$ !&% ) & & %+%& '$ !&$ !! "! &"$ !&" &% %!"$!$& !& %*#$ !&%)( '%&#$"$ # % ' & "!$%' &%" %")")&$$!!$ ( '%"& $&% !""$ !&%*+
!-"' %!' " ' ! ! ' &" %" ! *!&' ' ' % '"%," )%''&#"& ' "!"' " ' x o y o ! z o %"'& ! ! '&) " ', "% !'' "! vo "! ,'&$(!' " !'& t o !&' '(%' "!"' "' "! &%"(!&
(% "& ' "!+,- The CPR trajectory solver is activated by the operator by pressing ST A R T button at the moment 0[s] and by placing the camera at point I c . From that moment up to the camera of the CPR system gradually accelerates towards the starting reference velocity v c0 . This part of the trajectory is labelled as A C C-P4O R in Fig. 6. In Figs. 1 and 5 this part is orange coloured. All the simulation results can be followed through Table 3 as well. From point II c , the CPR Trajectory Solver takes the control over object tracking and monitoring. From Fig. 6, we can see periods in which the CPR Trajectory Solver establishes the current position and velocity orientation of object and defines goal position of the camera and chooses the primitive. These periods are: P4B R, P4B L, P4R E, P4L B and P4V I. Also, we can se#% "&"#% ' )&.+(' "! P1B R, P1B L, P2R E and P2L B. Below these periods, colourings of these trajectories in Figs. 1 and 5 are labelled. Detected positions of the chaotically moving object are labelled in the same image at the level of the works#.& "*&' # ! During execution of primitives P4B R, P4B L, P4R E, P4L B and P4V I, the CPR Trajectory Solver decided that there is no need for camera to change ! '(" '&) " ',&"' %.&) " ', &'%#-" & #* can be seen in Fig. 7a). Fig. 7b) & "*& ) ' "! '*! % ! %%! '% '"% & " %.& "' "! ! %'& ! coordinate system.
%" "*#& $) &$ "*#! #$ From Fig. 6, we can see that execution of certain primitive requires a certain amount of time. Time of (%$ $!" $&# $ ) $*#& $) $#!"#% $$ ( & $) object is , where # "*# ( & $)" ## of the chaotic motion of the object, for definition of the most unfavourable situation was assumed: the object moves $' !"& %# ) %"" $ ) $$ $*# ! #$ & #$"$ $# # "*# tracking of the object is ensured. Results of object tracking are smoothly interconnected aforementioned primitives, which can be seen in Figs. 1, 5, and 6, which " ! ( "" $"$ ") $ "*# $ ! ( reference trajectory of the camera is determined sequentially, based on the positions and velocity orientations of the camera and object at certain moments. We will once more emphasize that all the labels of positions and velocities are the same in Figs. 1, 5 and 6 and in Table 3 as well. A new procedure for creating the reference trajectory of the CP#)#$ *# " $ " $ tracking and monitoring in real time is presented. This procedure is named the CPR Trajectory Solver. #$"$'$!"#% !$ $$$ #)#$ *# " #$ ' $'# & within the came"*##$$$ '#$! $ "*#' " #! "$ ") &"# continuous information about the position and orientation of the camera during the task execution. However, position and velocity orientation of the object are not continuously available to the CPR Trajectory Solver. These data are available to the CPR Trajectory Solver only at sequential moments of their determination. During the implementation of the task, the motion of the object is completely chaotic. At the moment of recognizing the position and orientation of the object, the CPR Trajectory Solver defines the goal position and velocity orientation of the camera. After, it determines if the camera needs to change its velocity magnitude. Then, it chooses one of the four primitives (P1, P2, P3 and P4) " $#$# #$! # !" $&*#(%$ #!" $&#$ %# " $" $ $ "*#%"" $ ! #$ # #! ! ("" $"$ ") of the camera is formed sequentially at the specified moment and it is built of primitives which were chosen by the CPR Trajectory Solver from its data base. The complex reference trajectory is smooth in time and space. For confirmation of the defined logic and generation of the corresponding simulation
0B;0<4809>=A03,@01:<80/,9:@07;<:2<,8;,.6,20"#%$!900B,8;70:1>30.,80<,F=.:8;70B reference trajectory was generated and shown through a simulation experiment. The generated smooth reference trajectory defined in real time demonstrated the controllable behaviour of the camera and good tracking of a chaotic motion of the object. The CPR Trajectory Solver has been tested on a chosen CPR system in ideal conditions without external influences and disturbances. The results presented in this paper are the basic results and they open up the possibilities for further development of the CPR Trajectory Solver for more complicated application conditions. %34= <0=0,<.3 A,= =?;;:<>0/ -C >30 494=>4:9 $.409.0 ,9/ %0.39:7:24.,7 0@07:;809> :@0<9809> :1 >30 #0;?-74. :1 $0<-4, >3<:?23 >30 1:77:A492 >A: ;<:50.>= <,9> %#- 8-409>,77C49>0774209>=0<@4.0<:-:>=:1,9>3<:;:8:<;34..3,<,.>0<4=>4.=-C 43,57:"?;499=>4>?>0 &94@0<=4>C :1 072<,/0 $0<-4, ,9/ <,9> !- %30 /C9,84.= :1 3C-<4/ =C=>08= :1 .:8;70B =>?<0 -C $ & 9=>4>?>0 072<,/0 ,9/ ,.?7>C :1 0.3,94.,7 924900<492 &94@0<=4>C :1 4= $0<-4,(0,<02<,>01?7>:"<:1< ,>4.,#$>0@,9:@4.0/<431<:8 ,>308,>4.,79=>4>?>0072<,/0 1:< >30 307;1?7 .:9=?7>,>4:9= /?<492 >34= <0=0,<.3 ,9/ A0 ,<0 2<,>01?7 >: :?< 1:<80< .:770,2?0 )4@6: $>464.1:<34=307;/?<492>34=<0=0,<.3 [1]
Kawamura, S., Tanaka, W.S., Pandian, S.R.: Development of an ultrahigh speed robot falcon using wire drive system. In: IEEE International Conference on Robotics and Automation, pp. 1764E 1850 (1995). * + 7-?=#:=>078,9 ,2,7,64=%30 $%<:-:.<,90 #:-:>$C=> E [3] S. Behzadipour & A. Khajepour: Time-optimal trajectory planning in cable-based manipulators. IEEE Trans. Robot. 22(3), 2006, 559E563. [4] A. Gasparetto & V. Zanotto, A technique for time-jerk optimal planning of robot trajectories. Robot. Comput. Integr. Manuf. 24, 2008, 415E426. [5] A. Capua, A. Shapiro & S. Shoval, Motion Planning Algorithm for a Mobile Robot Suspended by Seven Cables. 2010 IEEE Conference on Robotics Automation and Mechatronics (RAM), DOI: 10.1109/RAMECH.2010.5513143. [6] M. Manubens, D. Devaurs, L. Ros & J. Cortes, A motion planning approach to 6-D manipulation with aerial towed-cable systems. International Micro Air Vehicle Conference and Flight Competition (IMAV2013),17-20 September 2013, Toulouse, France. [7] H. Fqihi, M. Saad, K. Benjelloun, M. Benbrahim & M. Nabil Kabbaj, Tracking Trajectory of a Cable-Driven Robot for Lower Limb Rehabilitation. World Academy of Science, Engineering and Technology International Journal of Electrical, Computer, Energetic, Electronic and Communication Engineering Vol:10, No:8, 2016. [8] H. Merabti, B. Bouchemal, K. Boucherma & K. Belarbi, Trajectory tracking and obstacle avoidance for cable driven robot by Nonlinear Model Predictive Control. 15th international conference on Sciences and Techniques of Automatic control & computer engineering STA'2014, Hammamet, Tunisia, December 21-23, 2014. *+ :<,C08 %:?<,54D,/03 ,B48?8 :1 =;,>4,7 .,-70 <:-:> 1:< , ;<0/01490/ ><,50.>:349>30A:<6=;,.0?=492.7:=0/7::;:;>48,7.:9><:7 ,;;<:,.3 :?<9,7 :1 9>0774209> ,9/ #:-:>4.$C=>08=%30:4:9=':7?80 ==?0 ?7C ",20= - * + :<,C08 ,8/,/C9,84.7:,/-.,<C:1.,-70-=?=;09/0/;,<,7707 8,94;?7,>:<=9>0<9,>4:9,7 :?<9,7:1/@,9.0/ ,9?1,.>?<492%0.39:7:2C':7?80 ==?0-
- [11] P. Tempel, F. Schnelle, A. Pott & P. Eberhard, Design and Programmin for Cable-Driven Parallel Robots in the German Pavilion at the EXPO 2015, Machines, DOI: 10.3390/machines3030223, 2015. [12] Richard Thomas Hale. Object-oriented Cable Camera System. Richard Thomas Hale, assignee. Patent US20120300079 A1. 29 Nov. 2012. [13] M. Filipovic, A. Djuric & Lj. Kevac, The rigid S-type cable-suspended parallel robot design, modelling and analysis. Robotica, Available on CJO 2014 doi:10.1017/S0263574714002677. [14] M. Filipovic, A. Djuric & Lj ! " ! work used for modeling aerial robots, Applied Mathematical Modelling (2014), DOI: http://dx.doi.org/10.1016/j.apm.2014.09.019.
No. t c [s] Camera position at t o [s] Camera position at t c [s] t o [s] ( xc , y c , z c )
1.
0
I c (0.60, 1.05,
( xc , y c , z c )
0
-0.30) 2.
0.15
II c (0.61, 1.04,
I c (0.60, 1.05,
Current postion
orientation direction of line t i
Primitive
x positive, y negative - line t1
P4
x negative, y positive- line t 2
P1
x positive, y negative - line t 3
P1
x positive, y V c (0.83, 0.85, x positive, y negative - line negative - line t 4 o3 -0.35)
P2
x positive, y negative - line o4
x positive, y negative - line t 5
P2
x positive, y negative - line t 6
P4
position
( xo , y o )
Current orientation direction of line - o i
unknown
unknown
II c (0.61, 1.04,
( xc , y c , z c )
-0.30) 0.35
-0.30)
II co (0.65, 1.00,
-0.30)
-0.30) I o (0.74, 1.00,
x negative, y positive - line o1
III c (0.74, 1.00,
-2.00) 3.
1.42
III c (0.74, 1.00,
1.62
-0.30)
III co (0.71, 1.04,
-0.30)
II o (0.74, 1.00,
-0.30)
x positive, y negative line o2
IV c (0.74, 1.00,
-2.00) 4.
2.19
IV c (0.74, 1.00,
2.39
-0.35)
IV co (0.78, 0.97,
-0.35)
III o (0.83, 0.85,
-0.35)
-2.00) 5.
2.96
V c (0.83, 0.85,
3.16
-0.35)
V co (0.88, 0.84,
-0.35)
IVo (1.03, 0.79,
VI c (1.03, 0.79,
-2.00) 6.
3.87
VI c (1.03, 0.79,
-0.30)
4.07
VI co (1.08, 0.77,
-0.30)
Vo (1.22, 0.82,
-0.30)
x positive, y positive - line o5
VII c (1.10, 0.77,
-2.00)
-0.30)
Ljubinko o Kevac is a research assistant at Innovation centre of Sc chool of Eleectrical Eng gineering, Universiity of Belgraade, Serbia. He is doing Ph.D. at School of Elec ctrical Enginneering, Univ versity of Belgradee, Serbia. Hee obtained B.S. B and M.S S. from the University U off Belgrade. H His research interests include ccable roboticcs, control th heory and maathematical modelling. m
Mirjana Filipovic is i an associiate research h professor at Mihajlo Pupin Insttitute, Unive ersity Of he obtained B.S. B at the Faaculty of mechanical eng gineering, Unniversity of Belgrade, B Belgradee, Serbia. Sh Serbia aand M.S. and d Ph.D. at School of Eleectrical Engiineering, University of B Belgrade, Serrbia. Her research h interests inccludes cable robotics, con ntrol theory, mechanics and a mathemaatical modellling
Aleksan ndar Rakic is i an assistant professo or at Schoo ol of Electrical Engineeering, University of Belgradee, Serbia. He H obtained d B.S., M.S . and Ph.D. at the School of Eleectrical Eng gineering, Universiity of Belgrrade, Serbia.. His researcch interests includes co ontrol theoryy, nonlinear systems, control ssystems and mathematica al modelling g
Highlights
Smooth reference trajectory of camera intended for object’s chatoic motion tracking The CPR Trajectory Solver which secures system’s autonomy One novel software package: CPRTS Simulation experiment which confirms theoretical presumptions
PII: DOI: Reference:
S0921-8890(16)30599-1 http://dx.doi.org/10.1016/j.robot.2017.04.018 ROBOT 2838
To appear in:
Robotics and Autonomous Systems
Received date : 27 September 2016 Revised date : 21 April 2017 Accepted date : 27 April 2017 Please cite this article as: L. Kevac, et al., The trajectory generation algorithm for the cable-suspended parallel robot—The CPR Trajectory Solver, Robotics and Autonomous Systems (2017), http://dx.doi.org/10.1016/j.robot.2017.04.018 This is a PDF file of an unedited manuscript that has been accepted for publication. As a service to our customers we are providing this early version of the manuscript. The manuscript will undergo copyediting, typesetting, and review of the resulting proof before it is published in its final form. Please note that during the production process errors may be discovered which could affect the content, and all legal disclaimers that apply to the journal pertain.
- 0 $- #'%( . (** +)(' #'! -,"(* "))& ) & -+# & (!#( +#(! (#/ +,#-2 ) &!+ .& /+ %+&$ & %,( + &!+ + # (()/-#)( (- + ) "))& ) & -+# & (!#( +#(! (#/ +,#-2 ) &!+ .& /+ %+&$
& %,( + &!+ + # &$. #(%)% / # - +,
#*$ ' #&#)(.# #"$&).*#((,-#-.- (#/ +,#-2) &!+ )&!#( &!+ + # '#+$( #&#*)/# *.*#(+,
& %+ ' * %# "))& ) & -+# & (!#( +#(! (#/ +,#-2 ) &!+ .& /+ %+&$ & %,( + &!+ + # +%# - +,
+,* , " +$ -)+2 )&/ + #, *+) .+ #( #( -"#, ** + 0"# " #, ., -) ! ( +- ,'))-" + + ( -+$ -)+2 ) ,2,- '4, ' + 0"# " ", -,% -) ')(#-)+ ( -+ % -" ) $ - #( + & -#' "/ ! ( +- - , ) ).+*+#'#-#/ -+$ -)+# ,*+#'#-#/ ,0"# "+ "), ( 2-" +$ -)+2 )&/ + .+#(! -" ! ( +-#)( ) ' +4, )'*& 1 + + ( -+$ -)+2 )+ -+$ -)+2 ! ( +-#)( -" +$ -)+2 )&/ + ., , -" %()0& ! ).- -" .++ (- *),#-#)(, ( / &) #-2 )+# (--#)(,) -" ' +( ) $ -( -" (#- #( ,-" !)&*),#-#)(( / &) #-2)+# (--#)() -" ' + " +$ -)+2 )&/ + ")), , )( ) -" ! ( +- *+#'#-#/ , )+ #(- + )(( -#(! -" .++ (- ( !)& *),#-#)(, ) -" ' + - + )'*& -#(! -" "), ( *+#'#-#/ -" +$ -)+2 )&/ + ,- &#," , -" ( 0 *),#-#)(, ( / &) #-2 )+# (--#)(, ) -" ) $ - "#, *+) ,, #, + * - 2 &# &&2 .(-#& -" + & -#' ) $ - ')(#-)+#(! ( -+ %#(! -,% #, )'*& - )+ -" *.+*), ) (&23#(! ( .,#(! -" #( &!)+#-"' 0 "/ ,2(-" ,#3 *+)!+' * %! +$ -)+2 )&/ + 2 .,#(! -"#, *+)!+' * %! -" ,#'.&-#)( 1* +#' (-, ) -" ' +4, -+$ -)+2! ( +-#)( )+-" *.+*), ) -" ) $ -')(#-)+#(!( -+ %#(!+ *+ , (- 2.,#(!-" +$ -)+2)&/ +')-#)(.-)()'2) ,2,- '4, ' +#,#( + , " ' +",-,%-) )&&)0( ')(#-)+-" ")-# &&2')/#(!) $ - /(* + ,2,- '+ &-#' -+$ -)+2*&((#(!) $ -')(#-)+#(! 1.
',*( - ,#('
& -,.,* ( +&& &) )-,,2,- ',+ )'*& 1+) )-,2,- ',0"# "., & , )+')-#)( -+(, + " , & , + )(( - -0 ( -" &) -))& ' + - ++# + ( -.-)+ ,. ,2,- ' ')-)+-! +-0#( " ,2,- ', )+ # + (- *.+*), , "/ ( #( ( ,")0( #(
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
-
+1%&0" 1&,+4"4&))!"#&+"1%"1/ '" 1,/6$"+"/ 1&,+ )$,/&1%*-1%"/ '" 1,/6,)3"/1%"* &+ ,+1/& 21&,+ ,# 1%" - -"/ ,/ 1%" -2/-,0"0 ,# + )60&+$ +! 1"01&+$ 1%" -/,-,0"! )$,/&1%* 4" % 3" 0")" 1"!1%"0601"*0%,4+&+&$ 4%"/"4"0%,4 3&"4,#1%" %,0"+0601"*4&1% *"/ %&00601"*4 0-/"0"+1"!&+ &+!"1 &)"% 3"1"01"!1%" )$,/&1%*,+1%"0601"* ,+)6 21&1 + "20"!4&1% +6(&+!,#1%"0601"*4&1% *"/ "$4&1%1%"-0601"*
$%&"' " ' & #( "#' ' ' #' " - &+&'!& % ("%-#"&'% " &+&'!& * % %'% , + !%-& " " ' #(% ($$% $# "'& # $% $ $-& $ *#% &$ #%' $(%$#&#%) '+ % "'%** ' &&+&'!&& !$ + &+&'!
(% &+&'!-&!%'% ""!#" '#% " #' +!#) "# ' ) * ' ' ) *#' !%'% '#%+' ' ) *## '-&$#& ' #"&
" ,$ .),3)&0 ,-)&0 -." *,) & ') , , ( .,$ .),3! ( ,.#)() ." -3-. '5- ' ,1"# ""-.-%.) )&&)1( ')(#.),." ) $ .1"# "#-#(." ' ,5--#!".# - &)- - *)--# & .)." (.,&*)#(.) ." -#-) ." ' ,5--#!".1"# "#-) ." -"* ) *3,'# ), -)&0#(! ."#- *,) & ' 1 "0 /- ." , -/&.- ,)' 1"# " !/,(. .". . " ')' (. ." *)-#.#)(( 0 &) #.3),# (..#)() ." ' ,#-%()1(, %#(! "#*#-/- ),' -/,#(!*)-#.#)() ." ' ," ., %#(! "#*#--'&& 0# .".#-.. " .)." ' ,( , &3-#.-*)-#.#)( %.) )'*/. , )(.,)&-3-. '"#-., %#(! 0# )/& #( &/ '!( .# #( ,, #)- , +/ ( 3# (.# # .#)(, & .),-)(,),)." ,.3* ) ., %#(!. "()&)!3/.*)-#.#)(( 0 &) #.3),# (..#)() ." ) $ ., %()1()(&3. ,.#(')' (.- &-)#.#-( .".." ) $ .#- &13- #( ' ,5- -#!". #." ." - --/'*.#)(- ." ,$ .),3 )&0 , (-/, - -'))." ( )(.#(/)/- , , ( .,$ .),3 ) ." ' ,5- ').#)( #( .#' ( -* 1"#& ." ) $ .5- ').#)( .,$ .),3 ( ").# ), ." */,*)- ) #'*& ' (..#)( ) ." .-% ) ) $ . )&&)1#(! 1 "0 *, -/' .". ." -3-. ' " -/ # # (. ( ,!3 , -)/, - -) #. )/& )&&)1 ." ) $ .5- "#!" -. 0 &) #.3 &-) /,#(!." #'*& ' (..#)() ." .-%." -3-. '"-*)--# #&#.3) & ,.#(!( & ,.#(! ." ' , * ( #(!)(." ) $ .5- 0 &) #.3 "(! /,#(!." ' ,5-.-% 2 /.#)( "#- ' (- .".." ' ,5-0 &) #.3( -.) )&&)1." ) $ .5-').#)(.(3')' (. ), #(!.)." )( #.#)(- ) ." ,$ .),3 )&0 ,5- /( .#)(#(! &-) & . (-#)( ), - 1" , ", . ,#4 - ." ),, -*)( #(! & '/-.&13- &#'#. -).".." 3-.#- 3 ( ,!3, -)/, -) ")- ( ./.),- - ( "0 *, -/' .". ." ) $ . ')0 - )(&3 . ." &)1 -. *&( ) ." ' ,5- 1),%-* "#-*&( #-& && - p 0 #(#!-( "#-*&( #-)(& 0 & .,.#(!*)-#.#)() ." ' ,#- #( 3." )* ,.),-).".." ' ,#- )0 ." /,, (.*)-#.#)( ) ." ) $ . ..".')' (.." )* ,.), "))- -').#)( #, .#)() ." ' ,
. ,1, - ." )* ,.), -.,.- ." *,)!,' 1"# " 3 #(#.#)( -.,.- 1#." ." & ,.#)( *,) -- *,) -- ,)' 4 ,) .) ." -.,.#(! , , ( 0 &) #.3 ) ." -3-. '5- ' ," -.,.#(! , , ( 0 &) #.3 ) ." ' , #- *, -/' 0 ,! 0 &) #.3 ) ." ) $ . /,#(! ." #'*& ' (..#)( ) ." .-% ' ,5- , , ( 0 &) #.3 ( "(! * ( #(! )( ." ) $ .5- ').#)( ( 0 &) #.3 0,#.#)(- . , ." -.,.#(! ')' (. ." ,$ .),3 )&0 , .% - )(.,)& ) ." ' ,5-').#)(" ')(#.),#(!( ., %#(!-3-. ') ." ) $ .#- )'*& . &3/.)()')/- /,#(! ." ) $ . ., %#(! *,) -- - )( ." /,, (. *)-#.#)(- ( 0 &) #.3 ),# (..#)(- ) ." ' ,( ) $ .." ,$ .),3)&0 , "))- -)( ) ." *,#'#.#0 - ,)'." . - ( #( .". -"#)(." ' , )&&)1-." ) $ ."#-*,) --#-, * . /(.#&." )* ,.), # -.)-.)*." ) $ .., %#(!.-% ..".')' (.." ,$ .),3)&0 ,-.,.-." ' , & ,.#)(*,) -- *,) --( #.-&)1- )1(." ' , ,)'." , , ( .)4 ,)0 &) #.3 " ,$ .),3)&0 , (/- )( ) )/,*, #( *,#'#.#0 .,$ .),# -*,#'#.#0 -1"# ", -.), #( . - (#!1 -")1.", *,#'#.#0 -1"# " ( /- 3." ,$ .),3)&0 , /,#(! ." ) $ . ., %#(! - )( ." /,, (. *)-#.#)(- ( 0 &) #.3 ),# (..#)(- ) ." ) $ . ( ' ," )/,."*,#'#.#0 #--.,#!".&#( #( - These primitives are defined in the local coordinate system, but the CPR Trajectory Solver can use the .,(- ),'.#)( '.,#2( '*." '. (3*&( ) ." !&) & )), #(. -3-. ' ) ." ' ,5- workspace. The A C C and D E C processes can be used by the CPR Trajectory Solver not only by the operator but also at the moments when the object suddenly changes its velocity and/or when the object is
($$!$'($( $&&$( "&.'' (( '%&')"( ( A C C and D E C processes can only be executed via P4 primitive.
)& %& " ( *' ( &'( %& " ( * ( '$# %& " ( * # ( ( & %& " ( * & " ( *' +& $'# '$ ( ( ( - $*& !! ( %&')" %$' ( $#' # *!$ (- $& #(( $#' $ ( "&#$ ( )& # ( (' ,)( $# $&( ' $!& (-$%&'#(( $# %& " ( *' - + !! ,%! # ( &$) $!!$+ # (+$!' ! '& ' #&! !$+ $ ,)( $# $ ( & ($&- $!*& $& #- $ ( & "#( $# %& " ( *' & " ( * ' #&!!- $"%$' $ !"#(' *# #! $ #( $( $# $( $# $( $# $( $# $ #(
$' ( $#$( "&(( "$"#($!'(%& " ( *.'$"%!( $# "& %' $#"$* # !$# '(& ( ! ## ( & ($&-$!*&'(! ' ' ( #+%$' ( $##$& #(( $#$( $ ( '$#( (#)&&#(%$' ( $#$( "& ( #'"&.' $!%$ #(# $$''( %& " ( * $( $#!$# ( '(& (! # $##( # ( &( $#$"&.'"$( $#+ ( &( $#$$ (.'"$( $# "& %' $# "$* # !$# '(& ( ! # )#( ! ( & ' %$' ( $# $* ( !'( #$+#%$' ( $#$( $ ( '(%$ #($&,)( $#$)&&#(%& " ( *#( &'(%$ #($&( #,(%& " ( *
! #&!'& %( $#$%& " ( *'- ' (#'# # %& " ( *'-& #+ ( &( ##)"&$'(& (! #'#&' Table 2 gives the conditions when and how the CPR Trajectory Solver chooses on of primitives P1, P2 and P3 from its data base. Primitives are defined in Fig. 2 in local coordinate system. & " ( *
)&
"&#$ ($& #(( $#
(.'%$' ( $# #&!( $#($( "&
! ! !& & # !% ! ! % !
! !& & # !% ! ! ! ! # ! !
! !& & # !% ! ! ! ! # ! !
! ! !
! !# -
" !! ! %
! "!" ! % ! !! $ ! !$ & ! $ in Fig. 3. Detailed presentation of the CPR Trajectory Solver is given with the flow chart in Fig. 4. At any moment, the system checks if the operator has activated the CPR Trajectory Solver. If the operator has not activated the CPR Trajectory Solver, the whole system is in idle state. When the operator activates the logic by pressing the ST A R T button, the CPR Trajectory Solver used for object tracking and reference trajectory generation is started. The process of reference trajectory generation repeats itself and the camera is able of monitoring and tracking the object. However, the structure of the system is much more complex. The system consists of the following blocks: C PR System, PD Controller and the C PR T rajectory Solver. Also, the system contains a Safety button which can be used by the operator in case of an emergency. Reference trajectory of camera in 3D space is defined in real time. Based on these variables and the real
trajectories of the external coordinates xc , y c , z c , by using the PD Controller the control signals are determined and the C PR System with the camera performs the real trajectory by following the generated reference trajectory. 2.1 Description of general form of the CPR Trajectory Solver The topic of this paper is definition of the CPR Trajectory Solver and it will be explained in detail: a) Starting the program. This procedure is performed by the operator based on his/her visual estimation of the current position and velocity orientation of the object. Before starting the object monitoring and tracking process, the operator places the camera at the position so that the object is close to the cent # b) A cceleration process. The operator starts this process by pressing button ST A R T. With this command, the CPR Trajectory Solver starts the A C C process and P4 primitive. Camera starts to move in direction of c ! # ! ! # !
#
e velocity is executed. c) T he procedure and conditions of choosing the primitive. Next 0.2 [s], the camera keeps moving (P4 primitive) in the same direction as during the acceleration and in that period it establishes the current position and velocity orientation of the object " O bject of interest. At that moment, the CPR Trajectory Solver also knows the current position and velocity orientation of the camera. Based on these data, the CPR Trajectory Solver defines the next goal point of the camera in its workspace. In the next step, the CPR Trajectory Solver decides if it is needed for the camera to change its velocity magnitude.
# ! ! A C C and D E C processes and primitive P4. Afterwards, the CPR Trajectory Solver chooses the next primitive from its data base. Then, it chooses the plane in which the primitive should be executed and transforms the primitive to the coordinates of the global Cartesian coordinate system. By using the chosen primitive (P1 " P4), the CPR Trajectory Solver guides the camera towards its goal position. d) E xecution of the new primitive. The chosen primitive executes over certain number of sampling intervals. During the execution of the primitive, the CPR Trajectory Solver constantly reviews the position and velocity orientation of the camera relative to the goal point of the primitive. The chosen primitive is executed until the camera reaches its goal point. The CPR Trajectory Solver establishes this by constantly checking if camera has finished the previously defined primitive, i.e. reference trajectory. In the period of execution of any primitive, the position and velocity orientation of the object are unknown. The procedures c) and d) of the CPR Trajectory Solver are executed cyclically during the tracking and monitoring the object in real time. e) Stopping the program. This routine is started by the operator by pressing E N D button. In this way, the CPR Trajectory Solver runs the D E C process along P4 primitive which is in the direction of
# D E C process. The camera slows down from the reference to zero velocity and the tracking and monitoring process is over. As it can be seen in Fig. 4, after execution of each primitive or choosing the A C C or D E C processes, the CPR Trajectory Solver checks if the operator has activated the E N D button and in that case the
whole process of object tracking and reference trajectory generation is stopped. When the operator activates the E N D button, the CPR Trajectory Solver waits for the current primitive to finish and after its execution it starts the process of stopping the object monitoring and following. At any moment, execution of the primitive can be stopped when the operator activates the Safety button.
Figure 4. Flow chart of the CPR Trajectory Solver.
2.2 An example of using the CPR Trajectory Solver In order to facilitate understanding of the CPR Trajectory Solver, we will show a detailed description of one example !( ! ! ! $ ( ! ! % sequentially defined, see Figs. 1, 5, and 6. Also, refer to the Table 3. This task includes: 1) Starting the C PR system with the camera at the point I c and moment t c 0[ s] . Based on ! ( !! !( # !% !! he/she defines line t1 which is parallel to the estimated direction of the object's motion. Line t1 is positioned on plane p a which is parallel to the plane p 0 . The operator places the camera at the position so that the object is close to the central point of the ! ( ! ! ! ! ! % # % !ST A R T button, see Fig. 4. The camera starts its motion along line t1 , starting from point I c . From that moment, the CPR ! % # ! ! ! ! ( ! ! ! "! " General description of this process is given in item a) of subsection 2.1. 2) A cceleration from point I c towards point II c moving along line t1 . In this period, camera of the % ! ! & ! ! ! ! ( # !% v c along line t1 (P4 primitive). The CPR Trajectory Solver uses the A C C process. This interval is orange coloured in Figs. 1 and 5. General description of this process is given in item b) of subsection 2.1. 3) Detection of the cur rent position of the object at point I o at moment , and detection voI o . From point II c , the camera moves with a velocity v c and during the period of 0.2 [s] (P4 primitive), it detects the current position and velocity orientation of the object ' point I o , see Figs. 1, 5, 6 and Table 3. At that moment the camera is at point II co . Based on the current position and the velocity orientation of the object and the camera, the CPR Trajectory Solver defines a new goal position and velocity orientation of the camera and that is the point III c ( x III c x I o , y III c y I o , z III c z II co ) . Also, the CPR Trajectory Solver decides that there is no need for ( # !% !" ! ! ! ! ! ! III c , i.e. the direction of the line t 2 which belongs to the plane p a , is parallel to the motion of the object, i.e. it is parallel to the line o1 which is on plane p 0 . The motion orientation of the camera at the point III c is the same as the motion orientation of the object at the point I o . The motion orientation of the object is defined in comparison to the direction of x, y axes of the global coordinate system: x negative and y positive. Based on these data, the CPR Trajectory Solver chooses one of the primitives via which the camera of the CPR system traverses from point II co to the next goal point, III c . General description of primitive choosing process is given in item c) of subsection 2.1. The chosen primitive is defined in Fig. 2a) (primitive P1) and the CPR Trajectory Solver has decided that it should be performed in horizontal plane p a ! ( $ ! # $ " General description of primitive execution process is given in item d) of subsection 2.1. 4) Detecting the cur rent position of the object at point II o at the moment and
voII o . From point to point , on line t 2 , the CPR Trajectory Solver has a task to recognize the position II o and velocity orientation of the object voII o which is on line o 2 . Also, it has a task to choose one of the primitives from its database. The CPR Trajectory Solver concludes that the object is at the same point as it was at the moment ,
while the object is moving in the opposite direction. Because of this special situation, the CPR Trajectory Solver is designed so that the camera can traverse to the next goal position in the vertical #& &'"'# !("" " !'# !("" " #"& !" CPR Trajectory Solver chooses the descension trajectory, because the z position of point III co is ! #% ' %# # " #"##
!# !' %! " !(" & goal point IVc ( xIVc xII o , y IVc y II o , z IVc z II co ) which is on line and belongs to plane p c . Also, #
!# !' %! " ## #! " ! !(" % #' #$ For camera to move from point III co to point IV c , the CPR Trajectory Solver chooses the appropriate primitive which will be performed in plane p c . The logic chooses the P1 primitive which was defined in Fig. 2a). See item c) of 2.1 subsection. The interval of the trajectory from point III c to point IV c is black coloured in Figs. 1 and 5. Primitive execution is generally described in subsection 2.1 under item d).
$! #!# !' ! # 5) Detecting the cur rent position of the object at point III o at the moment and
defined by line o3 . $! # !(" # ! point IV c to point IV co along line t 3 , the CPR Trajectory Solver establishes position III o and velocity orientation of the object voIII o which belongs to line o3 . It defines the next goal point of the camera V c which is on line t 4 and plane p d . Also, the CPR Trajectory Solver decides that there is no need for !("% #' #$ Based on these data, like it was described in item c) of subsection 2.1, ! !("! "" ! # IV co to point V c the CPR Trajectory Solver chooses the primitive P2 which needs to be executed in plane p d like it is described in item d) of subsection 2.1. The interval of the trajectory from point IV c to point V c is red coloured in Figs. 1 and 5. 6) Detecting the cur rent position of the object at point IV o at the moment and
detecting the velocity orientation of the object
defined by line o 4 . After completing the previous
primitive, the CPR Trajectory Solver leads the camera along line t 4 towards the point V co by using the c) process from subsection 2.1. During that period, it recognises new position IV o and velocity orientation of the object v oIVo - line o 4 . Now, the CPR Trajectory Solver %&" !$%& $-%sight and because of that it reacts and chooses the ascension trajectory of the camera which is executed in vertical plane p e . Based on these information, the CPR Trajectory Solver defines the next goal point of the camera VI c ( xVI c x IVo , yVI c y IVo , zVI c zVco ) , which is positioned along line t 5 and in plane . Line t 5 is parallel to the line. %"& $ &"$+" ($ %&&&$ %!"!"$ $-%( " &+ magnitude change. By knowing the current position and velocity orientation of the motion of the camera and object, the CPR Trajectory Solver chooses the next primitive which will be used for moving the camera from point V co to the next goal point VI c . The chosen primitive, defined in Fig. 2b), is P2 primitive. The interval of the trajectory from point V c to point VI c is light blue coloured in Figs. 1 and 5. See item d) in subsection 2.1. 7) Motion of the camera from point VI c to point VII c along line t 5 in plane. Camera is moving from point VI c to point VI co and the CPR Trajectory Solver, by using the process c) from subsection 2.1, establishes next position V o and velocity orientation of object voVo . However, the operator decides to stop the tracking and monitoring of the object before the camera reaches point VI co . It is the moment when the operator gives a command for stopping the camera by pressing the E N D button. At that moment, the #$"%% " $-% $& "! % %&$& & )% %$ ! & $" %'%& "! 2.1. The camera finishes its motion at point VII c ( xVII c , yVII c , zVII c ) and monitoring process ends. The period of the trajectory from point VI c to point VII c is violet coloured in Figs. 1 and 5. In this example, we have shown the sequential generation of a complex reference trajectory in real time. This is only one of the randomly selected possible combinations of primitives P1 P4 which are used by the CPR Trajectory Solver. Everything is conditioned by the sequential detection of chaotic motion of the object at certain moments and by the current motion of the camera. 3.
"$&#'$#"%"( & "!"& ! & & "$ ' & "!%"& &$ &"$+%" ($"$ !$& "!"&$$!&$ &"$+"&"%! %+%& -% $ !&!"$$ & " & &$ !)(%+!&% ,&#$"$ # ! % #$"$ # "!% %&% " %($ '&' +"!!&'! &% &$ &"$+%" ($"$!$& "!"&$$!&$ &"$+ %%'$"'& ! %%"!& &&& %+%& -% $!%&" "! &"$!&$ &" & ) "(% "& + ) & ! & $-% % & ! $ & '$"'& ! "$ !$& "! " & +! %"&"%! %+%& !'$"'& !"$!$& "!"&"!&$" %&$'&'$'$ !& $-%$$!&$ &"$+" ") ! &%%'$"'& !%$ '&' + +"'# !&!+ " !&&+$$+&"& "($ !& "'$%" # !&& "!"& " $-%&% "! &"$ !!&$ !& "& "!"&" & !)& " ")%)) #$%!& &$%' &%"&% ' & "! *#$ !&% ) & & %+%& '$ !&$ !! "! &"$ !&" &% %!"$!$& !& %*#$ !&%)( '%&#$"$ # % ' & "!$%' &%" %")")&$$!!$ ( '%"& $&% !""$ !&%*+
!-"' %!' " ' ! ! ' &" %" ! *!&' ' ' % '"%," )%''&#"& ' "!"' " ' x o y o ! z o %"'& ! ! '&) " ', "% !'' "! vo "! ,'&$(!' " !'& t o !&' '(%' "!"' "' "! &%"(!&
(% "& ' "!+,- The CPR trajectory solver is activated by the operator by pressing ST A R T button at the moment 0[s] and by placing the camera at point I c . From that moment up to the camera of the CPR system gradually accelerates towards the starting reference velocity v c0 . This part of the trajectory is labelled as A C C-P4O R in Fig. 6. In Figs. 1 and 5 this part is orange coloured. All the simulation results can be followed through Table 3 as well. From point II c , the CPR Trajectory Solver takes the control over object tracking and monitoring. From Fig. 6, we can see periods in which the CPR Trajectory Solver establishes the current position and velocity orientation of object and defines goal position of the camera and chooses the primitive. These periods are: P4B R, P4B L, P4R E, P4L B and P4V I. Also, we can se#% "&"#% ' )&.+(' "! P1B R, P1B L, P2R E and P2L B. Below these periods, colourings of these trajectories in Figs. 1 and 5 are labelled. Detected positions of the chaotically moving object are labelled in the same image at the level of the works#.& "*&' # ! During execution of primitives P4B R, P4B L, P4R E, P4L B and P4V I, the CPR Trajectory Solver decided that there is no need for camera to change ! '(" '&) " ',&"' %.&) " ', &'%#-" & #* can be seen in Fig. 7a). Fig. 7b) & "*& ) ' "! '*! % ! %%! '% '"% & " %.& "' "! ! %'& ! coordinate system.
%" "*#& $) &$ "*#! #$ From Fig. 6, we can see that execution of certain primitive requires a certain amount of time. Time of (%$ $!" $&# $ ) $*#& $) $#!"#% $$ ( & $) object is , where # "*# ( & $)" ## of the chaotic motion of the object, for definition of the most unfavourable situation was assumed: the object moves $' !"& %# ) %"" $ ) $$ $*# ! #$ & #$"$ $# # "*# tracking of the object is ensured. Results of object tracking are smoothly interconnected aforementioned primitives, which can be seen in Figs. 1, 5, and 6, which " ! ( "" $"$ ") $ "*# $ ! ( reference trajectory of the camera is determined sequentially, based on the positions and velocity orientations of the camera and object at certain moments. We will once more emphasize that all the labels of positions and velocities are the same in Figs. 1, 5 and 6 and in Table 3 as well. A new procedure for creating the reference trajectory of the CP#)#$ *# " $ " $ tracking and monitoring in real time is presented. This procedure is named the CPR Trajectory Solver. #$"$'$!"#% !$ $$$ #)#$ *# " #$ ' $'# & within the came"*##$$$ '#$! $ "*#' " #! "$ ") &"# continuous information about the position and orientation of the camera during the task execution. However, position and velocity orientation of the object are not continuously available to the CPR Trajectory Solver. These data are available to the CPR Trajectory Solver only at sequential moments of their determination. During the implementation of the task, the motion of the object is completely chaotic. At the moment of recognizing the position and orientation of the object, the CPR Trajectory Solver defines the goal position and velocity orientation of the camera. After, it determines if the camera needs to change its velocity magnitude. Then, it chooses one of the four primitives (P1, P2, P3 and P4) " $#$# #$! # !" $&*#(%$ #!" $&#$ %# " $" $ $ "*#%"" $ ! #$ # #! ! ("" $"$ ") of the camera is formed sequentially at the specified moment and it is built of primitives which were chosen by the CPR Trajectory Solver from its data base. The complex reference trajectory is smooth in time and space. For confirmation of the defined logic and generation of the corresponding simulation
0B;0<4809>=A03,@01:<80/,9:@07;<:2<,8;,.6,20"#%$!900B,8;70:1>30.,80<,F=.:8;70B reference trajectory was generated and shown through a simulation experiment. The generated smooth reference trajectory defined in real time demonstrated the controllable behaviour of the camera and good tracking of a chaotic motion of the object. The CPR Trajectory Solver has been tested on a chosen CPR system in ideal conditions without external influences and disturbances. The results presented in this paper are the basic results and they open up the possibilities for further development of the CPR Trajectory Solver for more complicated application conditions. %34= <0=0,<.3 A,= =?;;:<>0/ -C >30 494=>
Kawamura, S., Tanaka, W.S., Pandian, S.R.: Development of an ultrahigh speed robot falcon using wire drive system. In: IEEE International Conference on Robotics and Automation, pp. 1764E 1850 (1995). * + 7-?=#:=>078,9 ,2,7,64=%30 $%<:-:.<,90 #:-:>$C=> E [3] S. Behzadipour & A. Khajepour: Time-optimal trajectory planning in cable-based manipulators. IEEE Trans. Robot. 22(3), 2006, 559E563. [4] A. Gasparetto & V. Zanotto, A technique for time-jerk optimal planning of robot trajectories. Robot. Comput. Integr. Manuf. 24, 2008, 415E426. [5] A. Capua, A. Shapiro & S. Shoval, Motion Planning Algorithm for a Mobile Robot Suspended by Seven Cables. 2010 IEEE Conference on Robotics Automation and Mechatronics (RAM), DOI: 10.1109/RAMECH.2010.5513143. [6] M. Manubens, D. Devaurs, L. Ros & J. Cortes, A motion planning approach to 6-D manipulation with aerial towed-cable systems. International Micro Air Vehicle Conference and Flight Competition (IMAV2013),17-20 September 2013, Toulouse, France. [7] H. Fqihi, M. Saad, K. Benjelloun, M. Benbrahim & M. Nabil Kabbaj, Tracking Trajectory of a Cable-Driven Robot for Lower Limb Rehabilitation. World Academy of Science, Engineering and Technology International Journal of Electrical, Computer, Energetic, Electronic and Communication Engineering Vol:10, No:8, 2016. [8] H. Merabti, B. Bouchemal, K. Boucherma & K. Belarbi, Trajectory tracking and obstacle avoidance for cable driven robot by Nonlinear Model Predictive Control. 15th international conference on Sciences and Techniques of Automatic control & computer engineering STA'2014, Hammamet, Tunisia, December 21-23, 2014. *+ :<,C08 %:?<,54D,/03 ,B48?8 :1 =;,>4,7 .,-70 <:-:> 1:< , ;<0/01490/ ><,50.>:
- [11] P. Tempel, F. Schnelle, A. Pott & P. Eberhard, Design and Programmin for Cable-Driven Parallel Robots in the German Pavilion at the EXPO 2015, Machines, DOI: 10.3390/machines3030223, 2015. [12] Richard Thomas Hale. Object-oriented Cable Camera System. Richard Thomas Hale, assignee. Patent US20120300079 A1. 29 Nov. 2012. [13] M. Filipovic, A. Djuric & Lj. Kevac, The rigid S-type cable-suspended parallel robot design, modelling and analysis. Robotica, Available on CJO 2014 doi:10.1017/S0263574714002677. [14] M. Filipovic, A. Djuric & Lj ! " ! work used for modeling aerial robots, Applied Mathematical Modelling (2014), DOI: http://dx.doi.org/10.1016/j.apm.2014.09.019.
No. t c [s] Camera position at t o [s] Camera position at t c [s] t o [s] ( xc , y c , z c )
1.
0
I c (0.60, 1.05,
( xc , y c , z c )
0
-0.30) 2.
0.15
II c (0.61, 1.04,
I c (0.60, 1.05,
Current postion
orientation direction of line t i
Primitive
x positive, y negative - line t1
P4
x negative, y positive- line t 2
P1
x positive, y negative - line t 3
P1
x positive, y V c (0.83, 0.85, x positive, y negative - line negative - line t 4 o3 -0.35)
P2
x positive, y negative - line o4
x positive, y negative - line t 5
P2
x positive, y negative - line t 6
P4
position
( xo , y o )
Current orientation direction of line - o i
unknown
unknown
II c (0.61, 1.04,
( xc , y c , z c )
-0.30) 0.35
-0.30)
II co (0.65, 1.00,
-0.30)
-0.30) I o (0.74, 1.00,
x negative, y positive - line o1
III c (0.74, 1.00,
-2.00) 3.
1.42
III c (0.74, 1.00,
1.62
-0.30)
III co (0.71, 1.04,
-0.30)
II o (0.74, 1.00,
-0.30)
x positive, y negative line o2
IV c (0.74, 1.00,
-2.00) 4.
2.19
IV c (0.74, 1.00,
2.39
-0.35)
IV co (0.78, 0.97,
-0.35)
III o (0.83, 0.85,
-0.35)
-2.00) 5.
2.96
V c (0.83, 0.85,
3.16
-0.35)
V co (0.88, 0.84,
-0.35)
IVo (1.03, 0.79,
VI c (1.03, 0.79,
-2.00) 6.
3.87
VI c (1.03, 0.79,
-0.30)
4.07
VI co (1.08, 0.77,
-0.30)
Vo (1.22, 0.82,
-0.30)
x positive, y positive - line o5
VII c (1.10, 0.77,
-2.00)
-0.30)
Ljubinko o Kevac is a research assistant at Innovation centre of Sc chool of Eleectrical Eng gineering, Universiity of Belgraade, Serbia. He is doing Ph.D. at School of Elec ctrical Enginneering, Univ versity of Belgradee, Serbia. Hee obtained B.S. B and M.S S. from the University U off Belgrade. H His research interests include ccable roboticcs, control th heory and maathematical modelling. m
Mirjana Filipovic is i an associiate research h professor at Mihajlo Pupin Insttitute, Unive ersity Of he obtained B.S. B at the Faaculty of mechanical eng gineering, Unniversity of Belgrade, B Belgradee, Serbia. Sh Serbia aand M.S. and d Ph.D. at School of Eleectrical Engiineering, University of B Belgrade, Serrbia. Her research h interests inccludes cable robotics, con ntrol theory, mechanics and a mathemaatical modellling
Aleksan ndar Rakic is i an assistant professo or at Schoo ol of Electrical Engineeering, University of Belgradee, Serbia. He H obtained d B.S., M.S . and Ph.D. at the School of Eleectrical Eng gineering, Universiity of Belgrrade, Serbia.. His researcch interests includes co ontrol theoryy, nonlinear systems, control ssystems and mathematica al modelling g
Highlights
Smooth reference trajectory of camera intended for object’s chatoic motion tracking The CPR Trajectory Solver which secures system’s autonomy One novel software package: CPRTS Simulation experiment which confirms theoretical presumptions