Up till now. Path and trajectory planning. Path and trajectory planning. Outline. What is the difference between path and trajectory?

Transcription

1 Path and trajectory plannng Lecture 5 Mkael Norrlö Up tll now Lecture Rgd body moton Representaton o rotaton Homogenous transormaton Lecture Knematcs Poston Velocty va Jacoban DH parameterzaton Lecture 3 Lagrange s equaton (Newton Euler) Parameter dentcaton Experment desgn Model structure Robot Moton Control Overvew Current and Torque Control Control Methods or Rgd and Flexble Robots Interacton wth the envronment Outlne Path vs trajectory Standard path plannng technques n ndustral robots Trajectory generaton an ntroducton to the problem More general path plannng algorthms Potental eld approach An ntroducton to Probablstc Road Maps (PRMs) Lab sesson: date/tme to be decded Path and trajectory plannng What s the derence between path and trajectory? Path: Only geometrc consderatons. The way to go rom cong a to b. Compare: knematcs versus dynamcs. Trajectory: Include tme,.e., consder the dynamcs.

2 Path plannng In ndustral robot applcatons two path plannng modes can be dented Change conguraton Path plannng In ndustral robot applcatons two path plannng modes can be dented Change conguraton In RAPID (ABB s programmng language) MoveJ p, v, z, tool; Perorm an operaton Perorm an operaton MoveL p, v5, z, tool; MoveC p3, p4, v5, ne, tool; Rapd, robot moton nstructons Lnear n jont space:movej Cartesan space:movel Crcle segment MoveC Va ponts Pont-to-pont (ne ponts n Rapd). Robot has to stop. p p p3 p4 Va ponts (zones n Rapd). Robot does not reach the programmed poston. p p4 p p3

3 z Geometrc descrpton Orentaton nterpolaton x y Inverse knematcs. q(lc) lc One possble parameterzaton s cubc splnes. See MSc theses, Path plannng or ndustral robots by Mara Nyström and Path generaton n 6-DOF or ndustral robots by Danel Forssman. Chap 5 n Modelng and Control o Robot Manpulators, Scavcco and Sclano The orentaton along the path s nterpolated to get a smooth change o orentaton. Gven ntal orentaton (as a quaternon), q and nal orentaton q compute q = q q - and nterpolate the angle a rom to q n q p (a) = <cos(a/), sn(a/) s > where (q,s ) s the angle-axs representaton o q The nterpolaton scheme s oten reerred to as slerp nterpolaton The trajectory generaton problem Optmal control! The trajectory generaton problem Optmal control! q The path s parameterzed n some ndex s Þ q r (s) whch ntroduces addtonal constrants. q q q

4 z z The trajectory generaton problem Trajectory generaton problem Resultng optmzaton problem Complexty: 6 jonts/actuators A robot can have > constrants Multple robots possble.5. v d = 5 mm/s z.5 v d = mm/s xx y. x.3 Trajectory generaton problem Dynamsk optmerng.5. z y v d = 5 mm/s v d = mm/s xx.3. x y.3. x O P T I M I Z A T I O N q(t) q(t) y v d = 5 mm/s v d = mm/s xx.3. x Tme [sec]

5 Soluton Dynamc scalng o trajectores The dynamc model can be rewrtten n the orm D( q )q&& + C( q,q & )q& + g( q ) =t 443 G( q )[ qq & & ] Let r = ct (tme scalng). The orgnal speed/acc dep torque s t = D( q )q& + G( q )[ qq & & ] I tme scalng s appled t = r& t + rd( & q( r ))q ( r ) and wth lnear tme scalng s s s t s = c t s Applcaton to a robot system A more general path plannng scheme Use more than one robot to ncrease the lexblty n an applcaton. Here wth applcaton arc weldng.

6 Conguraton space A complete speccaton o the locaton o every pont on the robot s a conguraton (Q). The set o all conguratons s called the conguraton space. Obstacles The workspace o the robot s W. The subset o W occuped by the robot s A(q). The conguraton space obstacle s dened as { qîq A( q ) Ç ¹ Æ} QO = O q + knematcs gve conguraton. wth O = ÈO The collson ree conguratons are Q ree = Q\ QO Collson detecton A number o packages exsts on the nternet: One example s rom the group team gamma at Berkley Another at Unversty o Oxord See also wkpeda An applcaton where collson detecton s used can be ound n MSc thess General ormulaton o the path plannng problem Fnd a collson ree path rom an ntal conguraton q s to a nal conguraton q. More ormally g : [, ] Q wth g () = q s and g () = q. ree Examples o methods: Path plannng usng potental elds Probablstc road maps (PRMs)

7 Artcal potental eld Idea: Treat the robot as a pont partcle n the conguraton space, nluenced by an artcal potental eld. Construct the eld U such that the robot s attracted to the nal conguraton q whle beng repelled rom the boundares o QO. In general t s dcult/mpossble to construct a eld wthout havng local mnma. Slde rom: Constructon o the eld U U can be constructed as an addton o an attractve eld and a second component that repels the robot rom the boundary o QO U(q) = U att (q) + U rep (q) Path plannng can be treated as an optmzaton problem, ndng the global mnmum o U(q). One smple approach s to use a gradent descent algorthm. Let t q ) = -ÑU( q ) = -ÑU ( q ) - ÑU ( q ) ( att rep Constructon o the eld U Comments In general dcult to construct the potental eld n conguraton space (the eld s oten based on the norm o the mn length to the obstacles) Easer to dene the eld n the robot workspace For an n-lnk manpulator a potental eld s constructed or each DH-rame The lnk between workspace and conguraton space s the Jacoban

8 The attractve eld Conc well potental (U att, (q) = o (q)-o (q ) ) Parabolc well potental (U att, (q) = ½ x o (q)-o (q ) ) Parabolc well potental wth upper bound U att, ì ï ( q ) = í ïdz î z o ( q ) - o ( q ), o( q ) - o ( q ) - d z, o ( q ) - o ( q o ( q ) - o ( q ) ) d > d The repulsve eld Crtera or the repulsve eld to satsy, Repel the robot rom obstacles (never allow the robot to collde) Obstacles should not aect the robot when beng ar away and F att, ( o ( q ) - o ( q )) ì - z ï ( q ) = í o ( q ) - o( q ï dz o ( q ) - o ( q î, ), ) o ( q ) - o ( q o ( q ) - o ( q ) ) d > d Path plannng obstacle ree path Notce: Includng only the orgn o the DH-rames does not guarantee collson ree path. (Addtonal ponts can however be added.) The repulsve eld One possble choce wth U F rep, rep, ì æ ö ï í ç - ( q ) = h, è r( o( q )) r ø ï î, ( q ) r( o ( q )) r r( o ( q )) > r æ ö h ç - Ñr( o( q )) r( o( q )) r è ø r ( o( q )) = I the obstacle regon s convex and b s the closest pont to o r( o( q )) = o( q ) - b, Ñr( x ) o( q ) - b = o ( q ) b x= o ( q ) -

9 Comment, the convex assumpton The orce vector has a dscontnuty Dstance uncton not derentable everywhere Can be avoded repulsve elds o dstnct obstacles do not overlap. Mappng the workspace orces nto jont torques I a orce s exerted at the end-eector J T v F =t The Jacoban can be derved n all the ponts o. Notce that the ull Jacoban can be used when mappng orces and torques rom workspace to torques n conguraton space. Path constructon n conguraton space Buld the path usng the resultng conguraton space torques and an optmzaton algorthm Escape local mnma usng randomzaton When stuck n a local mnmum execute a random walk Gradent descent algorthm Desgn parameters, a, z, h, r. Typcal problem: Can get stuck n local mnma. New problems: Detect when a local mnmum s reached Dene how the random walk should behave (how many steps, dene the random terms, varance, dstrbuton, )

10 A more systematc way to buld collson ree paths Probablstc Roadmap (PRM) Space  n orbdden space ree space The cost o computng an exact representaton o the conguraton space o a mult-jont artculated object s oten prohbtve. But very ast algorthms exst that can check an artculated object at a gven conguraton colldes wth obstacles. à Basc dea o Probablstc Roadmaps (PRMs): Compute a very smpled representaton o the ree space by samplng conguratons at random. Slde rom: Slde rom: Probablstc Roadmap (PRM) Probablstc Roadmap (PRM) Conguratons are sampled by pckng coordnates at random Conguratons are sampled by pckng coordnates at random Slde rom: Slde rom:

11 Probablstc Roadmap (PRM) Probablstc Roadmap (PRM) Sampled conguratons are tested or collson (n workspace!) The collson-ree conguratons are retaned as mlestones Slde rom: Slde rom: Probablstc Roadmap (PRM) Probablstc Roadmap (PRM) Each mlestone s lnked by straght paths to ts k-nearest neghbors Each mlestone s lnked by straght paths to ts k-nearest neghbors Slde rom: Slde rom:

12 Probablstc Roadmap (PRM) Probablstc Roadmap (PRM) The collson-ree lnks are retaned to orm the PRM The start and goal conguratons are ncluded as mlestones Slde rom: s g Slde rom: Probablstc Roadmap (PRM) Comments The PRM s searched or a path rom s to g In ndustral robotc applcatons the path plannng problem s very much let to the user New deas rom moble robotcs (potental eld algorthms, PRMs, etc. could be appled) Automatc plannng algorthms hghly complex s g Slde rom: The trajectory generaton problem can be solved by applyng optmal control technques Conceptually easy to solve the olne problem Dcult to mplement onlne

13 Comments Other plannng requrements n many applcatons Lab sesson Suggeston Jan 3 Tme 9-5 (?) Explore the possbltes n Rapd/RobotStudo Exercse based on the prevous lab but ncludng multple rames and also ncludng movng rames (addtonal mechancal unts) Projects Knematc redundancy Estmaton and control Modelng and Identcaton Path plannng and trajectory generaton Energy optmal Sensor control (conveyor trackng) Dagnoss ROS Robot operatng system Further develop the robot rom the exercses modelng and control Further explore the DH parameterzaton and the possblty to use t n RobotStudo