Post on 02-Jan-2020
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