Autonomous Navigation of an IGVC Course with an ...Faculty of Engineering Autonomous Navigation of...
Transcript of Autonomous Navigation of an IGVC Course with an ...Faculty of Engineering Autonomous Navigation of...
FacultyofEngineering
AutonomousNavigationofanIGVCCoursewithanAckermannSteeredRobot
MTE544–Homework#2
December4,2017
LiorLustgarten–20471328
PavelShering-20523043
Part1–DefinitionofAckermannMotionModel
ThemotionmodelforAckermannsteeredrobot isderivedusingthemathshowinthe lecturenotes.ThetheoryisreinstatedbelowusingFigure1.Themotionismodeledbytrackingtherearwheel.
Figure1Ackermannmotionmodelwithvariablesdefined
TheaugmentedparametersaresummarizedinTable1below.
Table1Augmentedmodelparameters
Parameter
Rearwheelspeed v=3.0[m/s]
Steeringangle d=10-t[deg]
Steeringanglelimit +/-30[deg]
Framelength L=30[cm]
Updaterate 10Hz
Therobotstatesandinputsaredefinedbelow.
VehicleState Inputs𝑥"𝑥#𝑥$
= 𝑥𝑦𝜃
𝑢"𝑢# = 𝑣𝛿
MotionModel
Fromdynamics,definepositionoftherearwheelas:
𝑥,𝑦, = 𝑥,-" + 𝑣, cos 𝜃,-" 𝑑𝑡
𝑥,-" + 𝑣, sin(𝜃,-") 𝑑𝑡
Thefrontwheelpositionisrepresentedas:
𝑥8,:𝑦8,: = 𝑥 + 𝐿𝑐𝑜𝑠 𝜃
𝑦 + 𝐿𝑠𝑖𝑛(𝜃)
Thus,thedynamicsofthefrontwheelaredescribedbelow:
𝑥:,,𝑦:,, =
𝑥:,,-" + 𝑣,:𝑐𝑜𝑠 𝜃,-" + 𝛿, 𝑑𝑡
𝑦:,,-" + 𝑣,:𝑠𝑖𝑛(𝜃,-" + 𝛿,)𝑑𝑡
TheMATLABimplementationoftheAckermannmodelisshownintheAppendixB.
Kinematicmodel
Figure2Ackermannrobotkinematics
InstantaneousCenter of Rotation (ICR) is used to define robot’s rotation.Defining𝜔 = BC and
tan 𝛿 = FC.Thenthefollowingisderived:
𝜔 =𝑣𝑅=𝑣𝑡𝑎𝑛 𝛿
𝐿
𝜃, = 𝜃,-" +𝑣, tan 𝛿,
𝐿𝑑𝑡
Summarizingthefinalmotionmodelbelow:
∴𝑥",,𝑥#,,𝑥$,,
=𝑥,𝑦,𝜃,
=𝑥,-"𝑦,-"𝜃,-"
+
𝑣, cos 𝜃,-" 𝑑𝑡𝑣, sin 𝜃,-" 𝑑𝑡𝑣, tan 𝛿,
𝐿𝑑𝑡
DisturbanceAddition
ThekinematicmodelissubjectedtoGaussiandisturbanceofstandarddeviationof0.02monx,y,and1degreeontheq.Themodelupdatesat10Hz.
𝑥",,𝑥#,,𝑥$,,
= 𝑔 𝑥,-", 𝑢, + 𝜀,
UsingMATLABthedisturbanceisdefinedas:
𝜀, =
𝑛𝑜𝑟𝑚𝑟𝑛𝑑(0, 0.02)𝑛𝑜𝑟𝑚𝑟𝑛𝑑(0, 0.02)
𝑛𝑜𝑟𝑚𝑟𝑛𝑑(0,𝜋180
)
SimulationoftheMotionModel
AsimulationiscreatedinMATLABtotestthemotionmodel’sresponsetotheprovideinputofv = 3.0 [m/s] and d = 10 –t [degrees].Thesteeringangleislimitedto+/- 30 [degrees].Thex,ypositionoftherobotisplottedforeachtimesteponaCartesianplaneoverthecourseofthe20secondsimulation.Inordertoviewthemotionoftherobotintheplot,therobotspositionisplottedfortherearwheelasgreenasterixisandthesteeringangleofthefrontwheelisshownasgreenvector.Theheadingoftherobotisshownasaredvectorthelengthofwhichisproportionaltothevelocityoftherobotatthatpoint.
Therobot’struemotionisshowninFigure3andFigure4,respectively,fortheinputsabove.
Figure3Simulationofrobot'struemotionwithoutdisturbancefor20[s]
Figure4Simulationofrobot'struemotionwithdisturbancefor20[s]
Part2–CarrotControllerandRectangleTracking
AcarrotcontrollerisdevelopedtomanipulatetheAckermannmotion.Thecontrollerworksbydefininga“carrot”,apointondesiredtrajectorylineafixeddistance,r,aheadoftheclosestpointonthelinetothecurrentrobotpositionasshowninFigure5below.Pcontrolisfurtheraddedtothesteeringtoalign the heading of the robot to the direction of the carrot point along the desired trajectory. ThecontrollerMATLABimplementationisshownintheAppendixB.Essentially,thecarrotiscalculatedbytheprojectionofrobot’scurrentpositionofdistancer(fromtherearwheel)ontothedesiredlinesegmenttrajectory.
Figure5Carrotcontroller
ThecontrollerisimplementedinthesimulationwiththeresultsshowninFigure6andFigure7withradiusof 1.7 [m]. The radius is chosen experimentally to achieve turning without outside overshoot of thetrajectory,thusallturnsoftherobotwillhappenontheconcavesideofthetrajectory.Thesimulationisruntotrackarectanglewithdimensions20mby5mataspeedof3m/s.Thecontrollergain,p,ischosenexperimentallytobe0.5toachieveaccuratetrajectorytracking.
Figure6Carrotcontroller(r=1.7[m])withp-controlrectangletrajectorytrackingwithoutdisturbance
Figure7Carrotcontroller(r=1.7[m])withp-controlrectangletrajectorytrackingwithdisturbance
Part3–PathFindingandIGVCMapNavigation
TheProbabilisticRoadmap(PRM)approachwasusedtodetermineastraight-linepaththroughtheIGVNmapshowninFigure8.Multipledifferenttechniquesareusedtogenerateapaththroughtheprovidedmap.Thecarrotplannerwasthenusedtoguidetherobotalongthispath.
Thesecondstep inPRMisaddingedgestothemapandpruningthemtocreatethegraph.Edgesareadded based on the nearest 20 points to each node. This parameter is not investigated as a changevariable,howeverdefinitelywillmakeadifferencewhenthenumberofsampledecreases.TheedgesarethenremovedusingBresenham’sLineAlgorithm.Ifanedgepassedoveranoccupiedcellonthemapitwasdeleted.
Finally,theA*shortestpathalgorithmwasusedtofindthelowestcostroutealongthegraphfromthestartnodetotheendnode.
Figure8IGVNMap
RandomPRMSampling
ThefirststepinPRMwasgeneratingsamples.Initiallythesamplesweregeneratedrandomlyandremoved if theywere on an occupied cell in themap. However, the number of samples required tosufficientlysamplethebottleneckedareasofthemapisveryhigh.Theinitialparametersusedarelisted:
#samples sigma
200 30
Randomsamplegenerationperformsunreliablyintermsofbuildingacompletepathfromthestarttoendpoint.Thefailurerateisapproximatelyoneinfour.However,ifpassedthepathgeneratedisshowninFigure9.
Figure9RandomPRMsamplingpathgenerationsucceededwith200samplesandsigmaof30
Figure10displaysA*failingtofindtheshortestpathfromstarttoend,asnoconnectionsexistandthusanincreaseinsamplesisrequiredfortherandomgenerationtechnique.However,thesamplingmethodisthenchosentobeimprovedtoLavalle.
Figure10RandomPRMsamplepathgenerationfailedwith200samples,sigmaof30
LavallePRMSampling
Lavalle samplingworksbyplacingapointqat random just asbeforeusing randomsampling,howevernowanotherpointq’isplacewithaGaussiandistributionawayfromq.Thealgorithmchecksifonlyoneofqorq’isinfreespace,ifthat’struethenkeeptheonethatfree,ifnotthenretainnone.ThisprioritizespointsnearwallswiththeGaussiandistributionsigmadeterminingthedistancefromobjects.Figure 11 shows how Lavalle prioritizes the node placement near objects and avoids free space. ThistechniqueisfurtherinvestigatedwithBridgesamplingmethod.
Figure11LavallePRMsamplingwith200samplesandsigmaof30
BridgeSampling
Abridgesamplingapproachwasthenusedtoonlychoosesampleswhich fell inbetweentwosampleswhichwereontopofobstaclesontheoccupancygrid.Bridgesamplingyieldedmanysamplesinthebottleneckregionsbetweenthecircles(pylons)onthemap.Figure12andFigure13showtheresultsofbridge samplingpathgeneration.Thedrawback to thebridge samplingwas that therewas sampledeprivationonothermoreopenregionsofthemap.Asaresult,therewouldsometimesbe large,un-sampled gaps in the areas between the bottleneck regions. To account for this, a hybrid approachbetweenbridgesamplinganduniformrandomsamplingwasemployedwhichallowedforgoodsamplecoverageoftheoverallmapwithspecialattentionpaidtothebottlenecks.
Figure12BridgePRMsamplingwith150samplesandsigmaof30
Figure13BridgePRMsamplingwith150samplesandsigmaof50
BridgeandRandomPRMHybridSampling
Duetoasuchuniquemapstructurethevarioussamplingtechniquesdescribedabovearecombined.Specifically,theBridgemethodalongwithamodifiedLavalle.TheBridgemethodallowstogeneratenodesintightspacesseeninthefourcornersofFigure19whentherobothastogothroughapathofcylindricalobstacles.TheLavallealgorithmismodifiedtoperformexactlytheoppositeofitsintent,andisreferredtoastheAntiGaussiantechniquetoplacenodestowardsthecenteroftheopenspaceinsteadofnearobstacles.However,therandomsamplingisinvestigatedagainsttheAntiGaussiantechniquetodeterminewhichperformsbesttoplacenodesinopenspace.Therandomtechniqueis
usedinFigure19comparedtotheAntiGaussiantechniqueinFigure16.It’sclearfromruntimethattherandomsamplingaspartofthehybridisbetter[91.7seconds(Figure15)]comparedto[95.6seconds(Figure17)]foranti-Gaussiantechnique.
However,totrulycomparethecapabilitiesofRandomSamplingvsAntiGaussianaspartofthehybridalgorithmalotoftestsneedtoberunwiththesameparametersofsigmaandratioofsamplesbetweenBSandRS.ThesampletestsareshowninFigure19toFigure26locatedintheAppendixA.Thedatagatheringprocessislongandthusisnotperformedinthisreport.
Figure14BridgeandRandomPRMhybridsamplingwith150BSand400RSandsigmaof30
Figure15Robotruntimeof91.7[sec]with150BSand400RSandsigmaof30forBridgeandRandomPRMhybridsampling
Figure16BridgeandAntiGaussianPRMhybridsampling150RSand100RSsigmaof35
Figure17Robotruntimeof95.6[sec]with150RSand100RSsigmaof35forBridgeandRandomPRMhybridsampling
OccupancyGridPadding
Itisassumedthattherobotshouldnottouchthewalls.However,duetothestraight-linepathfoundbyPRMinconjunctionwiththerobotmotionmodelthereisnoguaranteethattherobotwillavoidbarriers.ThePRMcanreturnalistofwaypointswhichcausetherobottoveeroffthepathbecauseoftheturningradiusandcontactanobstacle.Tomitigatethis,theoccupancygridispaddedby0.4masshowninFigure18.Bydilatingthematrixoftheoccupancygridbyadiskwithradius4,anewoccupancygridiscreatedwhichforcesthepathfindingalgorithmtoleavea0.4mtolerancewhenpruningedgesandremovingsamples.
Figure18PaddingoftheIGVNMap
Part4–LimitationsofApproachandResolutions
SeveralassumptionsaremadeinthisassignmenttosimplifytheproblemofnavigatingtherobotthroughtheIGVCauto-navigationcourse.Theseassumptionslimitthesolutiontoveryspecificandlikelyunrealisticusecases.
Inthisapproach,themapusedforsimulationisthesameastheoccupancymapusedforpathplanning.Effectivelythismeansthatthemapisperfectrepresentationoftherealcourse.Inreality,intheIGVC,thevehiclewould create themap, perhaps using SLAM. The resultingmapwill be imperfect and requireupdating as the robot navigates the course. To bemore accurate and allow the approach to includeimperfectmaps,acontinuouslyupdatingSLAMapproachcouldbeimplementedinsimulation.
Additionally,inthisnavigationapproach,therobot’struepositioninsimulationisusedforthecontroller.Thismeans that the controller is usingperfect errordatawithnonoise. This is a simplificationandalimitationasanyreal-worldrobotwouldreceiveimperfectpositiondata.IntheIGVC,abasestationisnotpermittedforpositioncalibrationtomakethisharder.TheapproachcouldbemodifiedtoincludenoisypositionmeasurementsfromasimulatedGPS.AparticlefilterorKalmanfiltercouldbeusedtodetermineanaccuratestate.
Finally,thisapproachdoesnotconsiderthedynamicsoftherobot.Itissimplifiedbykeepingtherobotvelocityconstantalongtheentirepath.IntheIGVC,itwouldbeidealtoprioritizewiderturnsandmorestraightdrivingtoincreasethespeedofcoursecompletion.Intheresultsofthisassignmentitcanbeseenthattherobotusuallyturnsatitssharpestturnradiustonavigatethesharpcornersprovidedbythepathplanner.Thiscouldbemodifiedbyaddingacost tosharpcorners in thegraphoptimizationstep.Thecontrollercouldalsobemademorecomplextoincludeanunderstandingofwhenitcancutacornerandhowsharpitneedstoturnbasedontheoccupancygridinsidethecorner.
AppendixB–SampleTestsforcomparingRandomSamplingvsAntiGaussianaspartoftheHybridAlgorithmforpathgeneration
Figure19BridgeandRandomPRMhybridsamplingwith50BSand200RSandsigmaof30
Figure20Robotruntimeof95[sec]with50BSand200RSandsigmaof30forBridgeandRandomPRMhybridsampling
Figure21BridgeandAntiGaussianPRMhybridsampling50RSand200RSsigmaof30
Figure22Robotruntimeof96.4[sec]with50BSand200RSandsigmaof30forBridgeandAntiGaussianPRMhybridsampling
Figure23BridgeandRandomPRMhybridsamplingwith100BSand150RSandsigmaof50
Figure24Robotruntimeof99.2[sec]with100BSand150RSandsigmaof50forBridgeandRandomPRMhybridsampling
Figure25BridgeandAntiGaussianPRMhybridsampling100RSand150RSsigmaof50
Figure26Robotruntimeof94.5[sec]with100RSand150RSsigmaof50forBridgeandAntiGaussianPRMhybridsampling
AppendixB–MatlabCodeCarrotController%% distanceToCarrot % A function to calculate the location of the carrot and determine % whether the carrot is past the given line segment % % Input: % p1 - beginning point of the line segment % p2 - end point of the line segment % x - position of the vehicle % % Output: % carrot_outside - A boolean of whether the carrot is past the end of the % desired destination % % carrot_point - A coordinate value of the carrot the robot is following function [carrot_outside carrot_point] = get_carrot(p1, p2, x, r) carrot_outside = 0; line_segment = p2 - p1; p1_to_x = x - p1; % Compute the distance to the line as a vector, using the projection projection = p1 + (dot(line_segment,p1_to_x) / norm(line_segment)^2) * line_segment; carrot_point = projection + r * line_segment ./ norm(line_segment); if (norm(carrot_point - p1) > norm(line_segment)) carrot_outside = 1; end end
Pcontrol%% p - controller % Input: % target - [x y] of the target point % x - [x y theta] of the current robot pos % prev_u - [v theta_wheel] previous inputs % % Output: % u - [v theta_wheel] current input function [u] = p_control(target, x, prev_u) p = 0.5; error_threshold = 0.1; theta_ref = atan2(target(2)-x(2), target(1)-x(1)); theta = x(3); theta_error = theta_ref - theta; if (theta_error > pi) theta_error = theta_error - 2 * pi; elseif (theta_error < -pi) theta_error = theta_error + 2 * pi; end if (abs(target-x(1:2)) < error_threshold) u(1) = 0; else u(1) = prev_u(1); end u(2) = p * theta_error; if u(2) > pi/6 u(2) = pi/6; elseif u(2) < -pi/6 u(2) = - pi/6; end end s