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