{"responseHeader":{"status":0,"QTime":8,"params":{"q":"{!q.op=AND}id:\"193337\"","hl":"true","hl.simple.post":"","hl.fragsize":"5000","fq":"!embargo_tdt:[NOW TO *]","hl.fl":"ocr_t","hl.method":"unified","wt":"json","hl.simple.pre":""}},"response":{"numFound":1,"start":0,"docs":[{"date_modified_t":"2010-04-09","ark_t":"ark:/87278/s63b6dqj","date_digital_t":"2009-08-24","source_t":"Original: University of Utah J. Willard Marriott Library Special Collections","doi_t":"doi:10.26053/0H-FD00-DTG0","setname_s":"ir_etd","subject_t":"Robots; Control systems; Wireless LANs","restricted_i":0,"department_t":"Mechanical Engineering","format_medium_t":"application/pdf","identifier_t":"us-etd2,118389","conversion_specifications_t":"Original scanned on Epson GT-30000 as 400 dpi to pdf using ABBYY FineReader 9.0 Professional Edition","date_t":"2007-05-16","mass_i":1515011812,"publisher_t":"University of Utah","description_t":"In this research, a computerized motion planning and control system for multiple robots is presented. Medium scale wheeled mobile robot couriers move wireless antennas within a semicontrolled environment. The systems described in this work are integrated as components within Mobile Emulab, a wireless research testbed. This testbed is publicly available to users remotely via the Internet. Experimenters use a computer interface to specify desired paths and configurations for multiple robots. The robot control and coordination system autonomously creates complex movements and behaviors from high level instructions. Multiple trajectory types may be created by Mobile Emulab. Baseline paths are comprised of line segments connecting waypoints, which require robots to stop and pivot between each segment. Filleted circular arcs between line segments allow constant motion trajectories. To avoid curvature discontinuities inherent in line-arc segmented paths, higher order continuous polynomial spirals and splines are constructed in place of the constant radius arcs. Polar form nonlinear state feedback controllers executing on a computer system connected to the robots over a wireless network accomplish posture stabilization, path following and trajectory tracking control. State feedback is provided by an overhead camera based visual localization system integrated into the testbed. Kinematic control is used to generate velocity commands sent to wheel velocity servo loop controllers built into the robots. Obstacle avoidance in Mobile Emulab is accomplished through visibility graph methods. The Virtualized Phase Portrait Method is presented as an alternative. A virtual velocity field overlay is created from workspace obstacle zone data. Global stability to a single equilibrium point, with local instability in proximity to obstacle regions is designed into this system.","rights_management_t":"© Daniel Montrallo Flickinger","title_t":"Motion planning and coordination of mobile robot behavior for medium scale distributed wireless network experiments","id":193337,"relation_is_version_of_t":"Digital reproduction of \"Motion planning and coordination of mobile robot behavior for medium scale distributed wireless network experiments\" J. Willard Marriott Library Special Collections TJ7.5 2007 .F58","publication_type_t":"thesis","parent_i":0,"type_t":"Text","dissertation_institution_t":"University of Utah","thumb_s":"/e6/37/e63757c220df84c6066e0a3d5efee54a1577a27c.jpg","oldid_t":"etd2 1291","author_t":"Flickinger, Daniel Montrallo","metadata_cataloger_t":"frb","format_t":"application/pdf","modified_tdt":"2017-07-12T16:48:12Z","dissertation_name_t":"MS","school_or_college_t":"College of Engineering","language_t":"eng","file_s":"/71/56/71569d7bcceda96e9f4f3c5cc3574b7883ca6160.pdf","format_extent_t":"62,117 bytes","created_tdt":"2012-04-23T00:00:00Z","permissions_reference_url_t":"https://collections.lib.utah.edu/details?id=1254926","_version_":1664094734725939200,"ocr_t":"DISTRIBUTED NETWORK Engineering MOTION PLANNING AND COORDINATION OF MOBILE ROBOT BEHAVIOR FOR MEDIUM SCALE DISTRIBUTED WIRELESS NETWORK EXPERIMENTS by Daniel Montrallo Flickinger A thesis submitted to the faculty of The University of Utah in partial fulfillment of the requirements for the degree of Master of Science Department of Mechanical Engineering The University of Utah December 2007 © Copyright © Daniel Montrallo Flickinger 2007 All Rights Reserved SUPERVISORY COMMITTEE APPROVAL satisfactory. Chair: Minor '-5-^ : San-ford Meek (/ THE UNIVERSITY OF UTAH GRADUATE SCHOOL SUPERVISORY COMMITTEE APPROVAL of a thesis submitted by Daniel Flickinger This thesis has been read by each member of the following supervisory committee and by majority vote has been found to be satisfactory. &IT? - air: Mark Mmor ..... I . b = ~fordMeek II , '-- 10 9 8 7 6 5 4 3 2 1 1 2 3 4 5 I I ~ final positiorr-- ,:' , , \", \" r---------~ , , 6 7 8 9 22 Figure 3.1. generator. Step 1 . Step 2 If the current line segment does not intersect any obstacle exclusion zones, If the current line intersects an obstacle exclusion zone, the zone corner point £ significant implementation, are segments Model 23 Step 1 Step 2 Step 3 • Figure 3.2. Path generation steps. the intermediate goal position. Step 3 Drive the robot to the intermediate goal position. Step 4 If the robot is not at the final goal position, return to the first step. The iterative waypoint method possesses several drawbacks, the most significant of which is the high elapsed time between motion start and arrival at the goal point. The requirement that robots pivot at each waypoint wastes time, and limits the maximum velocity attainable. There is no support for user-specified paths, only singular goal points. The method itself is limited in its scope and implementation, and only appropriate for a workspace modeled by rectangular regions. 3.2 User-specified Waypoint Model To allow faster movements, and more complex trajectory specifications, an additional motion model is created. Users choose multiple via points to specify a path for a robot, instead of choosing only a single goal point. Via points are connected by straight line segments comprising a path from a start to a goal position. Instead of commanding robots to pivot at waypoints, the path segments are filleted, with curve segments inserted between straight line segments. Waypoints defined in point to point motion become via points in the extended motion model. The resulting path of line segments and arcs allows continuous motion to workspace filleted y, given reference velocities v and u>. trajectories drift. t 1 r 6 8 9 x(m) 24 destinations. Different types of arcs can be used to build a path between the filleted segments, with varying curvature continuity properties. Two solutions to calculate reference trajectories may be used. These methods are compared in Figure 3.3. The kinematics based method uses the Cartesian robot state equation, (4.1), to calculate x, and ¢ given reference velocities v and w. Discontinuities in curvature at the interfaces between line and circular arc segments cause drift to occur. A closed-form solution to parameterizing reference trajectories is desired to eliminate this drift. -7 -8 -9 :[ -10 >- -11 'I '() -12 -13 5 7 Reference Trajectory (closed form) \"- \"\"\" Original Waypoints --Reference Trajectory (kinematics based) 10 11 12 Figure 3.3. Comparison of kinematic and closed form trajectory generators. A fast, closed form geometric solution is needed to create arcs for trajectory generation. Presented here is a method utilizing intersecting offset line segments to solve for the center of a circular arc. ab be represented by dotted lines. Two lines, denoted as dashed lines in these figures, represent lines offset by distance r from segments ab and be. The intersection of these offset lines corresponds with the arc center point, / . Rz(\\) | ; b d e c 25 3.2.1 Line Segment Filleting The radius of curvature is determined by specifying a reference velocity. Higher reference velocities require larger radius curves to prevent the loss of wheel traction. trajectory Given three input via points a, b, and c, as illustrated in Figure 3.4 and Figure 3.5, line segments ab and b-; are constructed. These segments are represented r ab b-;. f. Two unit vectors, perpendicular to the via point line segments are calculated, Ild!11 = sign(ab x b-;) . Rz (~) . ab/llabll, (3.1) and (3.2) where Rz (~) is a rotation about the z axis by ~; defined by a '- c '- '- Figure 3.4. Filleted arc, obtuse via point path angle. 0 -1 1 0 Pi = a + M I # l l , (3-4) P2 = b + v\\\\df\\\\, P3 = b + r-\\\\ef\\l p4 = e + f-llelll, (3.7) where r is the desired arc radius, corresponding to the offset distance. a ~ P3 \\ \\ \\ \\ \\ \\ \\ \\ .\\ \\ \\ \\ \\ \\ \\ b • \\ I I I I I I I I I '1. I . I I I I I • I P, Figure 3.5. Filleted arc, acute via point path angle. R, G) = [~ -~ ]. 26 c (3.3) Points are constructed to build line segments offset to the original line segments, PI = a + r . IIdlll, (3.4) p, = + r . IIdfll, (3.5) = b+r ·lIelll. (3.6) p, = c+ l' 'lIelll, (3.7) where r is the desired arc radius, corresponding to the offset distance. 2 7 T and L,= Pi P21 Pz Pa T M = r ( p i -- P 2 ) T 1 . (P3 --Pa)T = f = b. (3.10) 3.11) Otherwise, the resulting arc center point is calculated L\\ (Pl - P2) L2 (P3 - Pa) fy = M Ll (Pl - p2)y L2 (P3 - Pa)v (3.12) where M fx ' fy. (3.14) are constructed by, d = f-r-\\\\df\\\\, (3.15) = / - r - | | e / | | , r | | e / | | respectively. The angle Zdef, 7 is calculated by the following: acos((d - f)(e - f)) 7 ll( d - / ) H - | | ( c - / ) ir (3.17) 27 The intersection of the two offset line segments is determined by calculating the determinants of the points, such that, and and £, ~ I [~~ 11, £, ~ I [ ~:~ 11, M ~ I[ ~; =~: l~ 11· If M 0, this is a degenerate case, resulting in / ~ b. by and f~ [i:l (3.8) (3.9) 3. 10) (3.11 ) (3.13) The endpoints of the filleted arc, d and e, as shown in Figure 3.4 and Figure 3.5, d ~ f - T ' IIdf II , e ~ f - r . lIefll. (3.16) where ,. is the arc fillet radius, and IIdfll and l!efll are defined in (3.1) and (3.2) LdeJ, ; following: d f)ie f)) 7 ~ \"'I(I d'7 -\"-\"'f)-;;-II \"oi. 11-7e-(- ---';/\"'\"I)I The direction of the curve is needed for the closed form solution. The length of the shortened part of the two original line segments is given by, I \\\\(P d)\\\\ \\\\(b - e)\\\\. 3.3 Lines and Circular Arcs The baseline path generation method is the use of constant radius arcs. These paths are C° curvature continuous. The discontinuity in the change of curvature between line and arc segments requires that a robot must stop at each curve boundary point to satisfy kinematic constraints. practice, a robust controller can allow a robot to track a C° continuous path with bounded error. Constant radius circular arcs are desirable because of their geometric properties, such as endpoint tangency and the existence of straightforward closed form solutions for path parameterization. The arc radius is chosen in consideration of trajectory velocity requirements. As velocity is higher, arc radius must also be higher due to centripetal acceleration, given by, ac = - -, v r a m a x contact friction force of the robot, amax ^-g, (3.20) /j, g Substituting (3.20) into (3.19) yields (3.21) For example, given a friction coefficient fj, 0.4, and velocity v l.Om/s, s)2 (3.22) • 9.8m/s2 ) ' r (3.23) 28 l = II (b - d) II = II (e) II· (3.18) CO curvature In Co parameterization. requirements. ac --, r (3.19) where is the linear velocity of the robot, and is the radius of the circular arc. The maximum allowable lateral acceleration, am ax is related to the wheel ground amax = f-l. g, (3.20) where f-l is the friction coefficient, and is acceleration due to gravity. Substituting v2 r=--. f-l.g f-l = = 1.0m/(1.0m/s)2 r = ....,------:---....\"..,- (0.4) . (9.8m/82 ) , = 0.26m. 3.21 ) v = Vr'V9- (3-24) Spirals Polynomial spiral arcs are chosen as replacements to constant radius circular arcs for greater curvature continuity. These curves are represented by curvature as a function of arc length, k(s) = a0 • s° + ai • s1 + ... + an • sn, k 1 , a* n of a closed form solution increases the computational complexity. 3.5 Quintic Splines manipulation of control polygons when generating these curves allows for these parameters to be controlled. The design, specification, and parameterization of quintic splines is discussed in this section. 29 In accordance with these constraints, an arc fillet radius of 0.25 meters is chosen for most trajectories. Velocity can likewise be constrained by arc radius, especially with curves such as Cornu spirals and splines. Solving (3.21) for v yields, v Jr· /1' g. (3.24) 3.4 Polynomial Spirals Polynomial spiral arcs are chosen as replacements to constant radius circular arcs for greater curvature continuity. These curves are represented by curvature as a function of arc length, (3.25) where K is the curvature in meters- I, ai is from a list of coefficients, and is the order of the curve. An arbitrary order polynomial is constructed, and its coefficients are solved to meet boundary conditions given by the filleting of two intersecting line segments. The two endpoints of the curve must coincide with the endpoints of the adjoining trimmed line segments, and the curvature at each endpoint must be zero. The lack To create a curve in Cartesian space, the curvature from polynomial equation describing the spiral must be used, along with a velocity profile, to solve the robot kinematics, as discussed in Section 4.2. Quintic splines may improve curvature continuity. Splines can take the place of constant radius circular arcs or polynomial spirals to build curve segments that have specific boundary conditions of position, velocity, acceleration, and curvature. The An example of a filleted arc replaced with a quintic spline is given in Figure 3.6. The associated curvature profile is shown in Figure 3.7. The first and second _ q 21 1 1 1 i 1 1 -0.2 0 0.2 0.4 0.6 0.8 1 1.2 x ( m e t e r s) C2 constant radius circular arc is shown for comparison. The change in curvature is minimized, but the maximum curvature is increased. curvature, respectively. The second derivative of curvature for a quintic spline is continuous, therefor guaranteeing C2 continuity in curvature. This minimizes the wheel acceleration required for a robot to track this type of trajectory, improving performance over the line-arc trajectories discussed in Section 3.3. Given a series of disjointed line segments, resulting from the arc filleting method ........ .C../.). .Q.....) Q.) -E- >- 1 0.8 0.6 0.4 0.2 o --- Quintic Spline • Circular Arc --- Control Polygon •• •• •• • •• • • •• • • • • • •• •• •• • • •• • • • • •• • •• -0.2~----~----~----~----~----~----~----~ -0.2 0 0.2 0.4 0.6 0.8 1 1.2 x (meters) Figure 3.6. Example of a quintic spline. 30 derivatives of this curve are continuous, resulting in 0 2 curvature continuity. A Figure 3.8 and Figure 3.9 show the first and second derivatives of curvature, 0 2 31 4 I 1 1 1 1 1 1 1 1 r 3.7. discussed in Section 3.2.1, quintic splines are created instead of circular arcs. A control polygon is created from the continuity requirements designed in to the path [44]. For a quintic spline, the control polygon has six points. The first and sixth points are the endpoints of the arc. The second point and third points control the first and second curvature derivatives of the arc. Both endpoints of every arc adjoin a straight line segment, with zero curvature and curvature derivatives. To ensure zero curvature and curvature derivatives at the boundaries of the arc, the first and second, and fourth and fifth segments of the control polygon must be colinear with each other. 4,-----,----,,----,,-----,----,,----,-----,-----,-----,-----, 3.5 3 2.5 - ~ I(J) L-Qa>) 2 g ~ 1.5 0.5 O~~--L---~L-__ ~ ____ ~L-__ ~ ____ ~ ____ ~ ____ ~ ____ ~ __ ~ __ a 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 Figure Curvature profile of quintic spline example. ensure 151 •-i 1 1 1 1 1 r _15 I i i i i i i i i i I 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 t 44]. 2D 'I :s c: o u Q) (/) 32 15,-----,-----.-----.-----.------.-----.-----.-----.-----,,---~ _15L-----L-----~----~----~-----L-----L-----l----~ ____ ~L_ __ ~ o 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 Figure 3.8. Derivative of curvature of quintic spline example. 3.5.1 Parameterization Splines are parameterized for trajectories using a subdivision algorithm, such as DeCastlejau's [44J. With uniform sampling in parametric time, data point spacing in the trajectory parameteric data varies significantly. This causes reference trajectories to have too many data points in some areas, and too few in others. An iterative method may be used to parameterize a curve with uniform spacing in real time, as opposed to parametric time, as obtaining a closed form solution is not a trivial task. A C2 continuous spline path of chained quintic Bezier curve segments is defined by giving control points. Each Bezier segment is a parametric curve, as given by 33 100 t C(t) [x(t),y(t)], 3-26) G 0,1] i) i = 1... achieved by using De Casteljau's algorithm. This algorithm evaluates points in the interior of a Bezier curve by subdivision. C'(0) = 5 ( P 2 - P 1 ) , (3.27) and C\"(0) = 20((P! - P2 ) + (P3 - P2 ) ) . (3.28) The symmetric construction is used at the end point. 100,----.----.----,----,----.----,,----,----.----.----, ~- \"0 C o u (]) CJ) ~ -150 -200 -250~--~----L---~----~--~----~--~L----L----L----- o 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 Figure 3.9. Second derivative of curvature of quintic spline example. C(t) = [x(t), y(t)] , (3.26) with t E [0, 1] as a polynomial of P( i) where = 1 ... 6. Evaluation of position and derivatives at a given position along the curve is The first and second derivatives of a quintic Bezier curve at the start point are, (3.28) The symmetric construction is used at the end point. 34 t f~l /• \\C'(0)\\/v{t). d=\\C{t + dt)-C(t)\\. velocity. To solve this problem, is adaptively refined to match to the desired step velocity within a close tolerance of d ~ f. A bisection refinement algorithm is used, as described in This refinement is repeated for each point along the curve, typically needing only one or two iterations because the velocity of the curve changes smoothly. (j){t) , velocity is defined for the boundaries of each curve. The desired velocity at each time increment will be interpolated from these values. Two additional parameters are needed for trajectory specification, and co(t), which are the linear and angular velocities. t K(t) = (x'y\"-y'x\")/((x'2 + y'2)^).. (3.31) through 1 ' 2 vmax u u{t) = v(t)-K[t). The desired linear velocity v(t) of the robot is a function of the curvature at a specific point, described below. The parametric step in along the curve that is necessary to produce that given geometric time step I-I is initially estimated from the length of the initial tangent vector: dt = I ·IC'(O)I/v(t). (3.29) The chord length distance to the next point along the curve is computed by d = IC(t + dt) - C(t) I. (3.30) The parametric speed along the curve changes independently from the desired dt d v(t)/ I. refinement The following Cartesian states x(t), y(t) and ¢(t) are calculated for each time t. To calculate ¢, the nonholonomic constraint, (4.2), is applied. A tangential v(t) w(t), Curvature at each time is defined by Velocity along the curve is computed from curvature through ( ) Vmax v t = 1 K,(t)2 , + 2 3.31 ) (3.32) where Vmax is defined as the maximum desired velocity. The rotational velocity w is calculated through velocity and curvature by w(t) = v(t) . \"\"(t). (3.33) In 35 The trajectories built by components in this chapter are sent to a controller, which then creates commands to be sent to the robots. In Chapter 4, various motion controllers are designed and discussed. control of the robots in Mobile Emulab allows for motion control schemes to be tested and implemented fully in software. Accurate localization of robots enables the use of state feedback control. These attributes maximize the design flexibility of the system, but also are partially responsible for the constaints discussed in Section 1.3. The use of software control in this instance detaches the motion controller from the robot hardware, increasing the sensitivity of the system to imperfect communication channels. presented interface for the Garcia robots. Motion is restricted to straight line segments and zero radius pivots. This works well for line segment paths with no arcs. regulates a robot to a single goal point, and uses continual state feedback from the localization system discussed in Subsection 2.5.3. The implementation of this controller was used to test feedback control on the testbed, and establish that a trajectory tracking controller would ultimately be feasible on the system. with these trajectories, is discussed in Section 4.2. A state feedback trajectory tracking controller is introduced in Section 4.4, which supersedes the waypoint motion controller within Mobile Emulab. CHAPTER 4 MOTION CONTROL Given a path plan, such as a goal point or trajectory, a controller is required to drive a robot towards its objective. The availability of computing hardware for The initial implementation of robot motion control in Mobile Emulab, presented in Section 4.1, made use of the vendor provided application programming interface A posture stabilizing controller is presented in Section 4.3. This controller To increase robot velocities, and decrease the time in motion, more advanced . reference trajectories are examined in Chapter 3. The robot kinematics, as associated trajectory • Odometry idicates that the robot has traveled by the set distance • A wheel has stalled • A proximity sensor is triggered • A cliff sensor is triggered • The primitive is aborted move pivot. to Section 7.1 for more information on the implementation of robot coordination using primitives. null 37 4.1 Primitive Motion The initial effort into robot motion on Mobile Emulab is point to point motion, accomplished using vendor supplied motion commands. These commands, termed primitives, provide the basic elements needed to move in straight lines, pivot, and turn. The primitives take as input length and angle measurements, and execute low level motion commands using odometry as a reference. The robot application programming interface has methods to allow for the configuration of parameters related to motion primitives. For example, there are settings for maximum wheel velocity, wheel stall threshold, and wheel acceleration. These parameters are tuned depending on operating conditions and performance requirements. When motion primitives are executed, termination is triggered by set boundary conditions. For a move primitive, which moves a robot a set distance in a straight line, the motion is terminated under the following conditions: distance stalled triggered triggered aborted The relevent motion primitives in this research are and With these two motions, a robot can be sent to any single goal posture in Cartesian space. Refer To achieve faster and more accurate goal posture attainment, state feedback control is used. This relies on the primitive, which allows instantaneous wheel velocities to be commanded. Kinematic control may be implemented directly. The following sections in this chapter discuss the design of kinematic controllers 38 to accomplish posture stabilization, path following, and trajectory tracking, all The robots used for Mobile Emulab are exclusively differentially steered wheeled mobile robots. Each robot has two independently controlled large wheels, on sides opposite along the longitudinal axis of the robot, sharing the same transverse axis. A passive two degree of freedom roller wheel is used to statically support the rear of the robot. A unicycle kinematic model is used for this class of robot. This model may be visualized rolling disk on a flat plane. 4> are used to denote Cartesian position regulation, X r • cos(4>) y v • sin(4>) J . u) There are three system states, but only two inputs. This class of wheeled mobile robot has a nonholonomic kinematic constraint given by, x • sin(4>) + y • cos{4>) = 0. ( 4.2) This describes the constraint that the robot may only move linearly along its longitudinal axis, (input or in rotation, (input ui). An arbitrary set of states, xi2/ii] may not necessarily be reached from another initial arbitrary set of states, [xoyofo] by a single straight line motion. utilizing the null primitive. 4.2 Robot Kinematics as a fiat 4.2.1 Cartesian Kinematic System Three robot position states, x, y, and ¢ are used to denote Cartesian position and orientation within a known reference frame. In the case of posture regulation, the origin of the coordinate system coincides with the goal posture. Otherwise, the origin is arbitrarily chosen as a known datum point in the workspace. The robot state equations in Cartesian space are [ X] [ v . cos ( ¢) ] ~ = v.sin(¢2 ( 4.1) The system inputs, v and w denote linear and rotational velocities respectively. X· sin(¢) + . cos(¢) = O. 4.2) v), w). [XIYl¢l] XOYO¢2] transformation e 9 = a \\ A 2 + 2/2 atan2(-y, -x) 9-6 ( 4.3) y, 6 a r e the Cartesian states, and e, 9, and a are the Polar 9, a, y, 4>, and system velocity inputs v and u> is given in Figure 4.1. The axes O signify e 9 = CO a v • cos{a) V sin(a) V e sin(a) ( 4.4) 39 4.2.2 Polar Kinematic System To overcome some of the drawbacks associated with the Cartesian system representation, a Polar representation is used. A Cartesian to Polar state transformation is given by where x, y, and ¢ are the Cartesian states, and e, e, and 0: are the Polar states. A kinematic diagram of the Polar states e, e, and 0:, Cartesian states x, y, and ¢, and system velocity inputs v and w is given in Figure 4.1. The axes 0 signify the goal reference frame, to which the wheeled robot is stabilized. For the posture stabilization problem, the polar state equations are [ ~] [ v . cos(o:) ] e + W v· sm(a) . e r\" sin (a) LtC v·-- e ~~--------~-----+Xr o ............ ........ e .... ..... Figure 4.1. Polar kinematic diagram for posture stabilization. Cartesian coordinates are added to the system. These state equations are given by, ' x vr • cos(r _ vr • sin(6r) (4.5) which is in the same form as (4.1). With the addition of these reference posture states, the Polar states given in (4.3) become, e e a ^{x- xr)2 {y-yr)2 atan2(-(y - yr), -{x - xr)) - 6r 9-d> + 6r ( 4.6) Figure 4.2 illustrates the kinematics of the trajectory tracking problem. The reference frame is indicated by a grey set of wheels, while the actual robot is indicated by the black set. The reference trajectory is given by the dashed line, and the actual trajectory is a solid black line. The Polar states e, 8, and a are given, along with their Cartesian components and system inputs (4.6). substituting (4.1), (4.5) and (4.6), (4.6) becomes e 6 = a v • cos(a) + vr • cos(8) sin(a) sin(a) 1 e ~ Vr • e ~ r sin(a) _ v . sm(Q) _ 1 Robot Trajectory r s. Or )e Polar kinematic diagram for trajectory tracking. 40 For the trajectory tracking problem, three states for a reference trajectory in [ ~ ] ~ [ ~:: ~~~i:~~ ] , [ e] [ J(x - Xr)2 + (y - Yr)2 ] () = atan2(-(y - Yr), -(x - xr)) - cPr a e - cP + cPr e, As presented in [32], polar state equations are defined. Differentiating (4.6) and [ ~ ] [ V. ~i~e(~~s~a~r ~ ~~:~fO~(:~ ] a v . a) V . sin(a) _ ). ere 'Y e Reference Traje!o~ ~ ~ ~~~~ .... --~X Figure 4.2. (4.7) I I constraints based on control and traction requirements. In this subsection, the dimensional constraints of the robots, and how they relate to the robot kinematics are discussed. produce commands in the form of velocities. Specifically, the control laws presented in this research produce the inputs v and uj for the systems described in the previous subsections. The low level control systems on the robot perform velocity tracking on a per wheel basis. This requires that wheel velocities be calculated for the v and uj velocities output from the motion controllers. The system inputs v and tu can be calculated from the individual wheel velocities by, V l + V r (a c\\ ^ = g ' (4 8) V r ~ V l I a a\\ w = \" 7 r - (49) Vl vr, r l r ,i _ But i (4.10) vL v - - v + ^ = v+\\u\\-R 41 The two polar systems defined in (4.4) and (4.7) are used for the design and simulation of the posture stabilizing controller and trajectory tracking controller, respectively. In simulation, these differential equations are solved directly, while in implementation the Cartesian to Polar transformations are used to directly calculate the Polar states. 4.2.3 Kinematic Constraints The Garcia robots used in Mobile Emulab have specific kinematic constraints related to their dimensions and configurations. While a differentially steered wheeled mobile robot is capable of a zero radius turn (e.g., a pivot), there exist curvature kinematics The nonlinear controllers discussed in this chapter, as kinematic motion controllers, w w w VL +VR V=--- 2 VR -VL W=--- 2 ·R (4.8) ( 4.9) By solving (4.8) and (4.9) for VL and VR, the individual left and right wheel velocities are calculated by, [ ~~ ] [ ~ ~ ~ ] , (4.10) where R = 0.0889 meters, as given by Table 2.1. At maximum wheel velocity, Vmax = V + Iw I . R (4.11) Letting v m a x 1.0 meters per second, consider that iomax vmax - JR. This results in u)max ~ 5.6243 radians per second. 8 is compared to 8 calculated by the Polar state equation (4.4). Without correction for discontinuities in the Cartesian to Polar conversion, 8 may be ±7r from the actual state value. To prevent discontinuities, unwrapping must be performed, which accounts for the periodicity of the two-dimensional arctangent trigonometric function used to calculate 8. The discontinuity correction, termed phase angle unwrapping is discussed in detail in Section 7.4.3. 4.3 Posture Stabilizing Controller linearizing controller for the purpose of posture stabilization. Using a controller developed in [26], a nonholonomic system is regulated to a single equilibrium point coincident with a goal posture. The development of this controller is undertaken as an intermediate step in the development of robust motion control for Mobile Emulab. The ultimate goal for the motion controller is to support full trajectory tracking. The establishment of posture stabilization through a nonlinear state feedback controller is used to test the feasibility of the use of this class of motion control on the testbed. control law developed here is implemented on Mobile Emulab, with state feedback coming from the visual localization system. From the polar state equations (4.4), a control law for a state feedback controller, [U,UJ]t = a,8), 8 V = V1+V2 = ~Xe2 + ^ ( a 2 + h82) (4.13) is chosen, where e is the error distance vector, and \\fh • 82]T is the alignment error vector, and where 0 and 0. 42 Vmax = Wmax = (vmax - v) / R. Wmax ~ The Polar state () as calculated by the Cartesian to Polar conversion in (4.3) e e ±n e. This section presents the development of a smooth, state feedback linearizing testbed. The controller and robot simulation are implemented using MATLAB. The [u, w]T = g(e, a, e), (4.12) is desired to drive states e, e and a to zero. A control law is designed via Lyapunov analysis [45]. A quadratic form Lyapunov candidate function, 1 2 1 (2 2) V VI + V2 2Ae + 2 a + he , ( 4.13) chosen. [a, Vh . e2F A > h > o. 43 V = V1 + V2, (4.14) V1 = Xe-e, (4.15) V2=(aa + heey (4.16) The control laws v = ('jcos a) • e, (4-17) cos asin a . , ., . uj = ka + -f (a + M), a 7 > > 0. V\\ Vi Xevcos(a). Vi ~ Xecos(a) (>ycos(a)) Vi = Xe2cos2(a)j. 4.21) Vi = -(Xsin2{a))e2 < 0. into (4.16). ( sin(a)\\ , „ / sin(a)\\ , J V2 = al-uj + v--^j+hOlv-^J, / sin(a) (a h0)\\ JN 2 = a[-oj + v ^ J-\\, 4.24 \\ a e J ( f f / \\ •> sin(a) (a + h • 9)\\\\ , A V2 = a{-u+\\hcos{a)e)-^± }-\\\\ , ( rycos{a)sin{a) , , A ,A V2 = a f -UJ + ^ {a + h-6)\\. (4.26) Taking the derivative of (4.13) results in, which is split into two parts: Vi = Ae· e, The control laws v (Icos a) . e, k cos as'ln a ( h()) W= a+, a+, ( 4.14) ( 4.15) (4.16) ( 4.17) (4.18) are chosen, to be substituted into (4.19) and (4.23). The controller gains are constrained by , > 0 and k > O. Considering V1 first, the state equations (4.3) and velocity control law (4.17) are substituted, resulting in, V1 = Aevcos(a). Vi = Aecos(a) (Icos(a)) e, (4.19) ( 4.20) (4.21 ) ( 4.22) The state equations (4.3) and control laws (4.17), (4.18) are also substituted Starting with, V'- ( Sin(a)) h()(Sin(a)) 2 - a -w+v + v , e e ( 4.23) results in, V· ( sin(a) (a + h())) 2 = a -w+v , ( 4.24) V2 a ( -w + ((Icos(a)e) Sin~a) (a eh . ())) ) , ( 4.25) V.2 = w + ,cos(a)sin(a) (a + h . ())) . a (4.26) V2 -ka2 < 0. (4.27) The derivate of the original Lyapunov function from (4.14) is assembled in (4.28). This is an abuse of the terminology, as the derivative is only negative semidefinite, instead of negative definite. The final result is, V = - A (7 • cos2(a)) e2 - ka < 0. (4.28) in situations where the high torques required to quickly reverse, stop, and move forward may reduce the battery life of the robots. Avoidance of path cusps may also decrease the amount of time required for posture stabilization. Furthermore, it is aesthetically pleasing to testbed users when robots proceed along more direct paths to their destinations. v = vmaxtanh [ ) , UJ = - ^--. (4.30) e are developed. A hyperbolic tangent function is used to provide a smooth saturation of v m a x . Posture regulation provides a control schema that bridges the gap between the waypoint path controller, and the higher performance trajectory tracking controller in Mobile Emulab. The posture regulating controller may be used with the existing baseline waypoint path generator, providing faster motion and decreased transit time during experiments. Trajectory Tracker To support continuous robot movement in Mobile Emulab, a suitable trajectory 44 . 2 V2 = ka :::; O. ( 4.27) ( 4.28) The control law (4.17) and (4.18) is further altered to permit forward motion only, eliminating cusps in the resulting path The avoidance of cusps is desirable A voidance Furthermore, destinations. The modified control laws, = vmaxtanh (~) Vmax ( 4.29) and W = (sin(a)) (1 + h~) + f3a . e are developed. A hyperbolic tangent function is used to provide a smooth saturation of vmax . Posture regulation provides a control schema that bridges the gap between the waypoint path controller, and the higher performance trajectory tracking controller in Mobile Emulab. The posture regulating controller may be used with the existing baseline waypoint path generator, providing faster motion and decreased transit time during experiments. 4.4 Kinematic State Feedback ~ajectory ~acker trajectory tracking controller is selected from the research. The controller needs to be capable of commanding a differentially steered robot to follow a specified parametric path at a high speed, and with minimal error. A kinematic controller is required to take advantage of the existing development tools and robot capabilities provided by the robot manufacturer. The design of a control law to use for trajectory tracking is presented in Section 4.4.1. The design of a dynamic extension to this controller is discussed in Section 4.4.2. Parameter and gain specification of the control system is given in Section 7.4.2. The stability analysis of the discrete system is discussed in Section 6.3.4. are discused in [32]. as, ki • • ke • tanh(e - ry/2 • ke) 4- vr • e • cos(9) • ke + vT • kr • (sin(8) + ^ • e) v = • ke + kr • sin(a) ( 4.31) where ke is defined as ke y/(-cos(26), kr = rV2-sin(29). ( 4.32) ( 4.33) The optimized control law governing rotational velocity is given by, u = k2 • tanh{9 + a) + 29 + 6r. (4.34) 45 4.4.1 Control Law A nonlinear control law to track a robot to a reference frame along a circular path manifold are developed using Lyapunov based techniques. This controller presented in this section is capable of solving the posture stabilization, path following, and trajectory tracking problems simultaneously. The controller development and background, along with more details about the design of the circular path manifold [The control law is unaltered, and implemented directly in the motion control system for Mobile Emulab. The optimized control law for linear velocity is given kl . e · ke . tanh(e - rV2· ke) + Vr . e· cos(e) . ke + Vr . kr . (sin(e) + ~r • e) v = r e ·ke+kr·sin(o:) , ke ke = V( - cos(2e), kr = rV2 . sin(2e). v = -kv(v - vr) + vr, ( 4.35) to - - ku{uj - uor) + oJr. (4.36) response in the presence of noisy state feedback. The system parameters for the trajectory tracking controller are given in Table 4.1. These parameters are referenced in the simulations and experiments presented in Chapter 6 and Chapter 8. With the completion of the design of trajectory generation and motion control systems, one final component is required to create a complete robot coordination and control system for Mobile Emulab. An obstacle avoidance system is needed to coordinate the motion of multiple robots, and to avoid obstacles present in the robot workspace. The next chapter presents an obstacle avoidance system, which is intended for reactive motion planning. After the presentation of the obstacle avoidance system, the results of all robot motion components in simulation are presented. r e Small perturbance to prevent discontinuity fci Controller gain, v k2 UJ kv Dynamic extension gain, v ku> Dynamic extension gain, UJ 46 4.4.2 Dynamic Extension A dynamic extension is utilized to bound the output of the control law (4.31) and (4.34). The dynamic extension is defined by new states, (4.36) introduced to decrease steady state error and improve boundedness. This extension to the system effectively acts as a low pass filter, which improves the controller 4.4.3 Controller Parameters Table 4.1. Motion controller parameters Gain Description Path manifold radius E kl k2 Controller gain, W kv kw W AVOIDANCE environment trajectories curvature. differential appear -ultimately CHAPTER 5 OBSTACLE AVOIDANCE In the iterative goal point progression model, obstacle avoidance is accomplished through a modified visibility graph method, as discussed in Section 3.1. This method works well for point to point motion and posture stabilization, but a better model is needed for path following and trajectory tracking. A method of smooth and continuous path generation for mobile robots maneuvering in a planar environment with multiple obstacles is presented in this chapter. The method is based upon construction of nonlinear dynamic phase portraits where key mathematical features of their underlying differential equations are manipulated in order to provide a novel trajectory generator resolving a number of known issues in the literature. Termed the Virtualized Phase Portrait Method (VPPM), this algorithm uses a planar velocity field instead of a scalar potential field. This provides trajectories devoid of oscillations, eliminates problems with local minima, and results in more direct control over bounded and smooth velocity and path curvature. In the VPPM, a single vector of equations is constructed to describe the goal point and obstacles. The goal point appears in the phase portrait differential equations as a globally asymptotically stable node that attracts the robot directly to its desired goal point along a minimum length straight line path. Obstacles appear in the equations in order to deflect the robot from this ideal path as the robot approaches the obstacles. Due to the smooth saturation nature of the goal point and obstacle proximity effects, the trajectory velocity and curvature are ultimately smooth and bounded. Obstacle proximity effects also allow multiple obstacles to be considered simultaneously by one set of equations. In cases where groups of obstacles could trap or hinder the robot path, a bounding volume hierarchy is 101----| 1 ' i -| r • 1 • 1-• 1™-: r 10 - •• L : I , L : : I I J ! : , I : S : ! : ' -10 -8 -6 -4 -2 0 2 4 6 8 10 meters environment. 48 employed to combine obstacles. This allows concave regions to be filled. A trajectory generated by VPPM through a workspace with moderately placed obstacles is shown in Figure 5.1. This illustrates key components of the velocity field, such as the goal sink, obstacle exclusion zones, obstacle regions, and the initial position. The obstacles in this example are placed manually, but could represent the configuration space of a circular robot in an obstacle filled workspace known a priori. 10,-----r-----r-----,-----,-----,-----,-----,-----'\"-----r----~ 8 - l I 6 -- I l I 4 -- I l I I 2- ~ I e! I * 0 ~ E I I f I -2 . \",,>\\ , \" l -. \\ I ~, \\ I I -4 , ~ l l ~ --I I '. ~ I -6 ~ I l I I -8 I l I I I I I I I -10 -10 -8 -6 -4 -2 0 2 4 6 8 10 meters Figure 5.1. Robot trajectory simulated in a cluttered environment. Given the size and placement of obstacles and goal point location within the robot workspace, a virtual velocity field, equations 9ft2. the goal attractor qgoai, and obstacle repulsion fields where i = 1,... ,n, and n is the number of obstacles present. globally attracts a robot to a specific Cartesian position within the workspace. A globally asymptotically stable equilibrium point is placed anywhere in 3?2. The goal is asymptotically stable, not exponentially stable due to the usage of a saturation function to prevent undesirably strong velocity fields in areas distant from the sink. proximity to the goal, the field strength weakens, by linearly decreasing the approach velocity of the robot near its objective to provide globally asymptotic convergence. As shown in Figure 5.2, all differential field lines are oriented towards the goal point. vectors oriented towards a single configured goal point. The goal attractor function could be substituted with a reference trajectory tracked by a nonlinear controller, or a goal posture regulator which drives a robot to a desired position and orientation. Pgoai saturation level fi determines the magnitude of the velocity field converging to the goal point. A polar form saturation function orients the differential field vectors towards the goal point. The usage of two Cartesian saturation functions results in vectors converging to the major axes in outlying areas, with trajectories traveling 5.1 Velocity Field Phase Portrait Method 49 (5.1) is generated. The field is used to create a trajectory from an initial point to the final goal point. For simplicity within the scope of this article, the field equations are restricted to ~2. The velocity field equation (5.1) shows the superposition of ggoal, fields gi, = 1, ... present. 5.1.1 Goal Sink In this model, a single goal sink for each robot is considered. This goal sink R2. In the goal point. The goal attractor function is a saturation function creating a field with all attract or orientation. A single Cartesian position, Pgoal is configured for the goal attractor. The f.-l traveling 50 5 4 3 0 -1 -2 -5 -5 0 Field of a single goal sink at the origin. undesirable The scalar polar distance magnitude, e=\\\\q- Pg0ai 6 = Atan2(q-Pgoal), 5~'~'~'~\\~\\~\\~\\~\\~'~~'~/~/~/-/~/~~~/~~~ \"'\\\\\\\\\\'~\"IIII/// '\" '- \\ \\ \\ \\ , , 111111/// \"\"\\\\\\\\\\'IIIII//~/ \"\"'\\\\\\\\\"III//~// \"\"\"\\\\\\\"II///~// \"\"\"'\\\\\"II//~;~~ ~~\"\"'\\\\III/;~;'~~ ~~~\"\"'\\II/~\"'~~~ ~~~~~~\"'I~;'~~~~~~ 2 1 o -1 -3 -4 .............................................................. ~~~~~~~~~t',~~~~~~~ ~~~~,~~/It\\\",,~~~~ ~~~~~~~111\\\"\",'~~ ~~~~//1111\\\\\"','\" //~///1111\\\\\\\"\"\" /////11111\\\\\\\\\"\", ~///111111\\\\\\\\\"\", ///11111 ,,, \\ \\ \\ \\, '\" ~//11111'1'\\\\\\\\\\'\" -5~~~~~~~~~~~--~~~~~~~ -5 o 5 Figure 5.2. along them to reach the equilibrium point at the goal. This behavior is undesirable in the interests of optimizing trajectory length. magnitude, e = II q - Pgoal II , (5.2) and angle, e = Atan2(q - Pgoal ), (5.3) 51 are assembled to form the goal attractor field, Qgoai = - A t • tanh(e) • Rz{9) 1 0 Rz{9) Rz{0) = cos(9) -sin(9) sin{9) cos(9) Obstacle field overlays are created by defining a set of Cartesian center coordinates. Values describing the two-dimensional size, orientation, and strength of the desired region of repulsion are also created. To define obstacle regions in the workspace, let Pi G 3?2, i 1,..., n be the center positions, and 0 -1 1 1 1 1 1 1 1 1 1 t t t t t t t I t t t 1 t t 1 t 1 1 • . l . l . . f . . . t . . . t . . l . l . . i . . t . .H i i i i i i i i i . i i i i i t i i i , i t i i i i i i i 1 1 1 1 1 1 1 1 1 t t t t t i t i t t t t i t t t t t u j . . . i . i . i . . i . i . i i i i i i i i i i i i i i i i i i i i t i i i i i t i -3 -2 -1 0 1 meters Figure 5.4. obstacle with no secondary rolloff function. used as secondary rolloff functions. With the addition of support for oriented obstacles, a new secondary rolloff function is needed. VPPM is extended with a new secondary rolloff function, a. This new extension not only provides support for oriented obstacles, but preserves continuity of the vector field, and improves interactions between closely spaced obstacles. rolloff a, a 3~------~--------~------~--------~--------~------~ 2 1 .. j .. I ... 1. .. 1 ... 1 .. j ... 1 ... 1 ... 1. . 1 1 1 t t t t t t t t t 1 1 1 1 1 1 1 1 1 o .. 1. . 1.. t .. 1. 1 .. 1. . 1.. t .. 1. ,~ J ! 4 J ~ J ! ! I + , i l , l l ~ J ~ ~ .. ,. . -I\"' .......... -.II - • I' . . ,. . . T ··t·· -2 .I .. ! ... 1 ... 1 ... 1. .. 1 .. ! .. .I ... 1 .. t t t t t t t t 1 1 1 1 1 1 1 f 1 .1,. , .. t .. 1. . 1. . 1.. 1. t. J, ,J ,~ J J J J J J t , + , l ! I I I j f J . I .. ~ . .. ~ ....... '.f .. .f • . l\"' ',\" 'T' _3~-------L ________ ~ ______ ~ ________ J-______ ~ ________ ~ 53 o 2 3 An obstacle with no secondary rolloff function. (J\". The secondary roll off function, (J\", is a scalar value calculated by solving a cubic polynomial parametric trajectory envelope around the obstacle. The current 54 q x, y a. Ra = #z(0-0 + 7r). TV to lie between 0 and -. Any other orientations can be normalized to this range by switching the dimensions of the obstacle region. Using this rotation matrix, and the dimensional parameters for each individual obstacle, the vertices of the oriented bounding box are calculated, -1 0 Vbh ottomleft Ra* vh bottomright Vtopleft = Vtopright Ra* Ra* Ra* 0 -1 1 0 0 1 -1 0 - 0 1 1 0 - 0 1 a;/2, * ai/2, * aj/2. bisection line one or two vertex points above the bisection line. The bottom left vertex is always below the bisection line, and can be discarded from consideration for the secondary rolloff function. Likewise, the top right vertex is always above the bisection line, and is always considered in the secondary rolloff function. One of the remaining vertexes may lie above the bisection line, and this is algorithmically determined in order to get the vertexes comprising the control points of the secondary rolloff function envelope. configuration is transformed into obstacle local coordinates. For a given X, a value in f) is calculated. The magnitude of the trajectory envelope at that specific point is then used as the value for (5. The obstacle local coordinate system rotation matrix is given by (5.7) The obstacle region is developed as an oriented bounding box, in a local coordinate system with an origin coincident to its center. The angle of orientation is normalized 7f to lie between 0 and \"2' Any other orientations can be normalized to this range by switching the dimensions of the obstacle region. Using this rotation matrix, and the dimensional parameters for each individual obstacle, the vertices of the oriented bounding box are calculated, ~ottomleft = RfJ * [ - ~ _ ~ ] * ad2, ~ottomright = RfJ * [~ _ ~ ] * ad2, = RfJ * [ - ~ ~] * ad2, Vtopright = RfJ * [~ ~] * ad2. (5.8) (5.9) (5.10) (5.11) The term is used to denote the x axis of the local coordinate system. For any oriented bounding box given the above stated parameters, there can be only remaining determined The control points of an obstacle are determined by the one or two vertices above the bisection line, plus two more points along the bisection line to control the size -Wi 0 w2 0 w\\ u>2 are calculated, and used to determine the secondary rolloff envelope wi u>2 correspond to obstacle approach the Gi = Vtopleft- Gi = Vtopright- (°-14) If the bottom right vertex is above the bisection line, - Vbottomright- orientation angle is -, both vertexes will lie directly on the bisection line, and will not be control points. The bottom left vertex is always below the bisection line, and therefor never considered as a control point. The final control point is given by, Pj Vf point, t x, 55 of the obstacle approach and departure regions. These outlying control points are determined based on the velocity of the approaching trajectory, which in turn is influenced by the goal and local obstacle field strengths. Two scalar distance values, WI and W2 are calculated, and used to determine the secondary rolloff envelope function control points. The values WI and W2 correspond to obstacle approach and departure distances, respectively. The first control point is [ -WI] GI = 0' (5.12) in the local obstacle coordinate system. If the top left obstacle vertex is above the bisection line, the next control point becomes, (5.13) The top right vertex is always above the bisection line, which leads to its inclusion as a control point: (5.14) If the bottom right vertex is above the bisection line, Gi = Vbottomright. (5.15) Due to the symmetric properties of oriented bounding boxes, either the top left or bottom right vertex can exclusively lie above the bisection line. If the orientation 7f \"4' (5.16) For each segment of the trajectory envelope, four parameters are given. The vectors Pi and Pf represent the initial and final control points, while Vi and vf denote the derivatives which control the orientation at each control point. is calculated given X, which also determines the active segment. For each segment, t starts at zero, and ends at one. The cubic trajectory coefficients for each segment h = Pu (5.17) k2 = vu (5.18) k3 = 3*{Pf-Pi)-{2*Vi + vf), (5.19) and h = - 2 * (Pf - Pi) + {Vi + vf). (5.20) Using these coefficients yields y = k4-t3 + k3-t2 + k2-t + k1. (5.21) An example secondary rolloff function envelope for an obstacle oriented at zero is given in Figure 5.5. Four control points are used, and Wi and w2 are both arbitrarily set to one. Note that the top curve is the calculated envelope, and that the bottom curve is reverse-symmetric. 1 0.5 o -0.5 -1 -1.5 1 -0.5 0 0.5 1 1.5 56 are given by and U sing these coefficients yields ( 5.17) (5.18) (5.19) (5.20) WI W2 reverse-1.5 '----_~ _ ____':_ __ __'_____-----'---_---,JL--_~ _ ____':_ __ - 2 -1.5 -1 2 Figure 5.5. Secondary rolloff function, obstacle angle 0 7r/w*i\\, (5.22) attractor, 1 - 0.5- o - 0 . 5 - -1 - - 2 -1 0 1 1.5 2 7r/57 In Figure 5.6, the same values are used, except that the obstacle is now oriented 7r / 4. In this case, we only need three control points. The reverse symmetry properties are more prominent in this example. The value for W2 is significantly decreased, and the orientation is changed to 7r /6 in Figure 5.7. A smaller exclusion zone around the obstacle is required in regions The partial obstacle field equation, (5.22) is constructed from the primary and secondary rolloff functions, multiplied by the obstacle field strength parameter. Field lines are aligned perpendicular to the goal attractor to keep obstacle repulsion fields from influencing the approach velocity of a robot in proximity to an obstacle, which helps prevent local minima from forming. Otherwise, field lines radiating directly from the center of an obstacle region can create local minima on the side furthest from the goal at tract or , while accelerating trajectories towards the goal near the side closest to the goal attractor. 1.5 0.5 Ol---~ - 0.5 -1.5'----_---'-_-----' __ ---'-----_--\"-__ --'---_---'--__ '----_---.J -1.5 -0.5 o 0.5 Figure 5.6. Secondary rolloff function, obstacle angle 7r / 4 1 function, 7r/(5.23) scalar partial field and field orientation parameters are then used to calculate the final oriented obstacle differential field, cos(C) sin(Q (5.24) 58 1.5 .... . .... . . .. :-. - 1.5 ~---'~\"--''--:---'-;!;-'''--;;-:-'----';:-;c'---'--:-'---~;-',,-,,-,! - 2 -1.5 -1 -0.5 0 0.5 1.5 2 Figure 5.7. Secondary rolloff function , obstacle angle 1[/6 The orientation of the field is determined by 7r ( = O±- 2' dependent on whether the current point is above or below the bisection line. The . [ cos(() 1 q, = p. sin(O . tested in simulation to verify their characteristics and performance. All simulations presented here use MATLAB and Simulink. undertaking the task of implementation in to the Mobile Emulab system. The simulation results presented in this chapter provide a baseline to establish desired behavior, which can be used to compare and verify results gathered through experimentation on real hardware. 6.1 Trajectory Generation generating segmented paths, curves, and parameterization to create final reference trajectories required by the motion controller. This standard trajectory is designed to thouroughly test all generators for robust- CHAPTER 6 SIMULATION In this chapter, the design, implementation, and results of simulations for trajectory generation, motion control, and obstacle avoidance are presented. The design of all components to be implemented into Mobile Emulab are first rigorously Several simulation applications are used to evaluate the performance of the different aspects of motion planning and control. The simulations are used to speed the development of algorithms, and verify the planned features before undertaking The trajectory generators discussed in Chapter 3 are evaluated in simulation in this section. Applications are created in the MATLAB programming language to test all aspects of the trajectory generators before integration within RMCD. This method is chosen to enable rapid development and evaluation of algorithms for For simulation and verification of the segmented trajectory generators, a standard reference trajectory is needed. A series of viapoints resulting in a path with curves with high and low angles, plus multiple segments that must be orthoganal. Trajectories Section 3.3. Paths generated in this manner are C° continuous, with a discontinuity in curvature at the boundaries of lines and arcs. A closed form parametric trajectory is realized, given a list of via points. In Figure 6.2, the resulting paths from two trajectory generators are presented. The path created from simulation is generated from the MATLAB application. The 60 ness, and correctness. Figure 6.1 gives an example of a standard testing trajectory, colloquially termed the double paperclip. 6.1.1 Line-Arc Trajectories Line-Arc trajectory generation by the path planning software is discussed in CO trajectory -7r-----~----~------~----~----~------~----~----~ -8 -9 -10 -11 -12 -13 -14~--~----~----~----~----~--~----~--~ 4 5 6 7 8 9 10 11 12 Figure 6.1. Example trajectory with quintic spline curves. Simulation 6 8 x (meters) RMCD r 1 I 6 8 x (meters) Emulab, as discussed in Section 2.5. To test for conformability, the two plots are compared. 6.3. A linear velocity ramp is applied at the beginning and end of the path to create a trajectory. A constant velocity is otherwise applied. curvature continuity of the path results in the abrupt changes in angular velocity. This path could not be followed exactly by a differentially steered mobile robot without stopping at each discontinuity point and executing a zero radius turn. A kinematic controller with first order filtering can approximately track a C° continuous path with a minimal amount of error. 6.1.2 Spline Trajectories discussed in Section 3.5 is given in Figure 6.1. The fillet radius is 0.25 meters, P2 - Pi = 0.05, as developed in Section 3.5.1. The radius and control 61 -7 -7 -8 -8 -9 -9 (j) Qi -10 (j) Qi -10 Q) Q) .s -11 .s -11 >. >. -12 -12 -13 . -13 -14 -14 4 10 12 4 10 12 Figure 6.2. Trajectory generation comparison, path. path in the plot labeled RMCD is generated from the implementation on Mobile The velocity profiles from the two trajectory generators are compared in Figure Figure 6.4 gives a comparison of the angular velocities. The zero order curvature CO An example of the standard test trajectory generated using quintic splines, as and where P2 - PI = 0.05, as developed in Section 3.5.1. The radius and control polygon points are chosen arbitrarily in this section, for illustration purposes. 62 Simulation RMCD 50 100 150 200 250 300 0 Time (seconds) Trajectory generation comparison, velocity. 0.5 Simulation -0.5 L 100 200 Time (seconds) 300 RMCD o o 200 Time 300 Figure 6.4. Trajectory generation comparison, angular velocity. 62 Simulation RMCD ~ ~lo ~lo c c (0) 8 Ql 0.1 Ql 0.1 U) U) QU)) Q~l a; a; .s .s .5' 0.05 .-5' 0.05 0 0 CD CD > > 0 0 50 100 150 200 250 300 Time (seconds) Figure 6.3. 0.5 ~'o ~'o c c 0 0 () () Q) Q) U) CJ) CJ) CJ) c C 0 .:=- 0 'u 'u 0 0 1) 1) > > ~ m ::::J \"3 OJ OJ C c « « -0.5 0 200 300 0 100 200 300 Time (seconds) Time (seconds) Figure 6.4. velocity. trajectories 6.2 Posture Stabilization Controller of resulting 63 The feasibility of using splines for parametric trajectory generation is demonstrated, as the path shown in Figure 6.1 is similar to the path shown in Figure 6.2, albeit without any lower order discontinuities in curvature. The same via point data is used for both simulations. This demonstrates that spline-based trajectories can be used in Mobile Emulab, requiring minimal modification to path data input requirements. Controller Simulations of the posture stabilization controllers discussed in Chapter 4 are created to test the stability and performance of the designs. Before integration of these controller in to Mobile Emulab, the controllers must be verified to respond as desired. The results presented in this section explore the behavior of the two posture stabilization controllers under different initial conditions, parameters, and gains. The simulations presented here do not account for the sampling frequency of the state feedback on the real system. Noise characteristics are not modeled, and robot dynamics consist mainly of the saturation velocity and acceleration of the controller command signals. The goal of these simulations is to establish the feasibility of these controllers to perform as expected while being used to control real robots. 6.2.1 Simulation Development The posture stabilizing controller discussed in Section 4.3 is shown in Figure 6.5, as implemented in Simulink. Logging facilities are in place to capture position, state, controller, and wheel velocity data from the simulation. Both Cartesian and polar inputs for initial conditions are accepted. An alternate version of the posture stabilizing controller is included in this simulation. This version allows only forward motion, avoiding cusps in the resulting paths. The block diagram for this controller is given in Figure 6.6. 1.2 g a m m a a l p h a CT>H e cos sin O f u m ax tanh U r XTJ3 o m e g a e t a a Figure 6.5. Posture stabilizing controller. omega Figure 66..66.. PPoossttuurree ssttaabbiilliizziinngg ccoonnttrroolllleerr,, aalltteerrnnaattee vveerrssiioonn ((ffoorrwwaarrdd mmoottiioonn oonnllyy)).. to accept initial conditions in either Polar or Cartesian coordinates, and output of the resulting trajectory, Polar states, and controller output velocities. Both posture regulators are included, and can be chosen at run time. the main simulation. 6, 9. xyS]. ] -1.0, -7r/66 A Simulink block diagram of the main simulation application of the posture stabilizing controller is shown in Figure 6.7. Included in this application are blocks of The robot polar kinematics block diagram, shown in Figure 6.8, takes input from the controller, runs through a robot dynamics model, and then directly implements the polar state equations (4.3). The initial state values come from the values"}]},"highlighting":{"193337":{"ocr_t":[]}}}
*