Circuit Cellar, the Magazine for Computer Applications. Reprinted by permission. For subscription information, call (860) 875-2199, or www.circuitcellar.com. Entire contents copyright ©2004 Circuit Cellar Inc. All rights reserved.
FEATURE ARTICLE by Rich LeGrand Closed-Loop Motion Control for Mobile Robotics
Rich recently came up with two closed-loop drive train designs for mobile robots. All it took were some inexpensive permanent magnet motors and a simple feedback scheme. In this article, he covers everything from PID control and tuning to trajectory generation and opera- tional space control for two robot bases. He also explains the software.
The mechanical components that result, robots that use them are typically to the motor, the robot takes much make up robots aren’t getting much slow. Large gear reductions make some longer to stop. With less gear reduc- cheaper. That’s the bad news. The good problems easier, as you’ll see, but no one tion, the robot will coast! news, of course, is that electronics and wants a slow robot if they can help it. If Sometimes stopping quickly is impor- processors continue their steady march you’ve used these motors, you might tant (e.g., stairs ahead), or your robot toward higher performance and lower have wanted to trade in some torque may do something hazardous to its cost. As an engineer, you have to figure for some speed. health. This brings up an important out how to use the extra resources and The 9-V Lego motors shown in Photo 1 point: motors with high gear reduc- capabilities most effectively. In some are readily available. They are part of tions stop quickly when power is cases, extra processing allows you to the Mindstorms kit, so you can build removed. But this is a silly reason to use use lower-cost mechanical compo- the rest of your robot base out of these motors. It’s like telling a begin- nents, which can effectively reduce Legos, which is a good thing for the ning driver to drive only in first gear to the system’s overall cost. mechanically challenged like me. avoid getting into an accident. What the With this idea in mind, I’ll tackle the Fortunately, the Lego motor has driver really needs is better driving robot drive train armed with a modest much less gear reduction (14:1) and is skills. Similarly, what you need is a amount of computing power. When I’m well suited for attaching a wheel direct- better motor controller. done, you’ll have two different closed- ly to the output shaft. If you’ve done loop drive train designs that would this, you know that you get a quick CLOSING THE LOOP make R2-D2’s head spin with envy. I’ll robot, but it’s difficult to control. A motor controller that doesn’t receive show you how to accomplish this Particularly, when you shut off power any feedback information from the using inexpensive permanent motor is an open-loop controller. magnet motors and a clever feed- The main advantage of open-loop back scheme that requires no controllers is simplicity. All that’s mechanical overhead. Let’s begin required is a way to control the by addressing the topic of motors. voltage or current going to the motor. The Lego Mindstorms RCX MOTOR OPTIONS controller, for example, uses an Clearly, selecting a motor is one open-loop motor controller that of the most important decisions to allows you to select from several make when designing the drive voltages (by varying the pulse- train. If you’ve played with motors width modulator duty cycle) and gears, you’ve certainly devel- depending on how much speed and oped the following intuition: more torque you want to deliver to your gear reduction results in less speed robot’s wheels. and more torque, and less gear A disadvantage of open-loop con- reduction results in more speed and trol is inaccuracy. Others include less torque. Modified RC servomo- the inability to deal with uncon- tors, which are often used in robot trollable variables such as bumps drive trains, have large gear reduc- in a floor, inclines, and low batter- Photo 1—Lego is an excellent medium for implementing robots such as tions (typically 300:1) resulting in the four-wheeled HUMR. The Gameboy Advance acts as the controller and ies, which can create an undesir- high torques and low speeds. As a provides 32-bit RISC processing performance, a color LCD, and sound. able situation by slowing or stop-
34 Issue 169 August 2004 CIRCUIT CELLAR® www.circuitcellar.com Method Description Advantages Disadvantages Sources Optical-mechanical A rotating slotted disk is placed No drift. Digital output integrates Typically expensive. Requires extra Hewlett Packard, encoders between a light source and detector easily with digital controllers. Long cables. computer mouse to infer position. lifetime. Mechanical Switches are triggered by the motor’s No drift. Typically expensive. Requires extra Vishay, various encoders motion to infer position. cables. Imposes drag. Output needs industrial vendors debouncing. Lower speed. Shorter lifetime. Hall effect sensors When used with a magnet, they can No drift. Uses existing gear in gear Typically expensive. Requires extra Allegro, various sense metallic (ferrous) gear teeth to train. ables, extra magnet, and ferrous gear. industrial vendors infer position. Back EMF The back EMF voltage of a motor is No extra mechanical components or Drift. Requires A/D converter and Acroname, measured to infer velocity. cables. Typically inexpensive. extra computation to obtain position. Charmed Labs
Table 1—There are a few popular feedback methods for sensing motor motion. Back EMF sensing is often overlooked, but its advantages can be attractive to many robotics applications. ping the motor. What’s required is a age with an A/D converter, for example, taken literally. A control loop typically way to sense the motion of the motor you can infer the motor’s velocity. When entails a software loop that repeatedly and compensate by increasing or the voltage is integrated (summed) over executes a control algorithm. Each rep- reducing the power. Closed-loop con- time, the position can be inferred as well. etition of the control algorithm is trollers can do both. The main disadvantage of back EMF called a control cycle. The control algo- Closed-loop controllers are found in sensing is that the inferred position rithm can be described simply with the all sorts of places: thermostats, cruise drifts over time with respect to the following expressions, which are eval- control systems, and elevators just to actual position because of noise in the uated once per control cycle: name a few. Almost without excep- back EMF voltage. In practice, howev- error = V − V tion, commercial robots use closed- er, the error introduced by position DESIRED MEASURED V = f() error loop motor control. Even the Roomba, drift is small when compared to the CONTROL a $199 vacuuming robot, uses a error introduced by wheel slippage Basically, there is a function, f, which closed-loop motor controller. alone. This performance can be determines the value to send to your
Closed-loop control requires a method obtained with a 9-V Lego motor and controller (VCONTROL) as a function of the for sensing the motor’s motion. Table 1 robot controller from Charmed Labs, error. VMEASURED is the measured (sensed) lists some popular methods. The method which uses back EMF sensing. value, and VDESIRED is the desired (com- that will work best for you depends as manded) value. The difference between much on project constraints as your pref- PID CONTROL the values is the control error. Note erences. The majority of closed-loop Closed-loop motor control entails both that one of the simplest control meth- motor controllers use optical-mechanical sensing and controlling the motor’s ods is the bang-bang controller, which encoders for position feedback, but the motion. I have described different you can find in your thermostat. extra cabling and mechanical complexity sensing techniques. The general con- V = Heat if error > 1° are usually worth avoiding. I chose back sensus is that an H-Bridge with pulse- CONTROL Cool if error < −° 1 EMF as a feedback method. The mechan- width-modulation (PWM) is the best Off otherwise ical simplicity (no mechanics) and lack method for controlling the motor. (For of cables make it an attractive option. more information, refer to L. Mays’s My thermostat doesn’t automatical- Back EMF exploits the fact that perma- article, “Muscle for High-Torque ly select between heating and cooling; nent magnet motors are also generators. Robotics,” Circuit Cellar, issue 153, it would be nice if it did. Many robots When a motor spins, a voltage is gener- 2003.) Here, a PWM signal switches an use bang-bang controllers for their ated across its terminals. The voltage, H-Bridge to control the voltage going motors. However, PID control is a referred to as the back EMF voltage, is into the motor and its speed. Note that much more effective technique: directly proportional to the motor’s veloci- I will refer to this type of controller () () ty. Thus, when sensing the back EMF volt- throughout the rest article as a PWM VkCONTROL = VPROPORTIONAL k + () () controller. Its VINTEGRALk + V DERIVATIVEE k input will be V()kk = K × error() Endpoint referred to as the PROPORTIONAL P
k PWM value. Velocity Tragectory PositionDESIRED Error PID VPWM + Motor V()k = K × ∑ error ()j generator controller When combin- INTEGRAL I – j = 0 Acceleration ing sensing and PositionMEASURED () × ()− control in a VDERIVATIVEk = K D error k Figure 1—A PID controller is typically used to control the velocity and position of a closed-loop con- error()k − 1 motor. I’ll focus on implementing a PID position controller, which is shown here with troller, the word () ()− () the trajectory generator. “loop” can be error k = VDESIRED k V MEASURED k
www.circuitcellar.com CIRCUIT CELLAR® Issue 169 August 2004 35 End position (PositionDESIRED) as fast as possible. If also shows the corresponding positions your robot’s only speed is “as fast as associated with the velocity profile. possible,” it may cause harm to you and These positions are quickly fed into the others. It’s often useful to specify the PID position controller, usually once Position speed and acceleration when command- per control cycle. The idea is that the ing the motor controller. This is where trajectory generator provides positions the trajectory generator comes in. It pro- rapidly enough so the motor moves Start position duces a continuous stream of positions, smoothly along the desired trajectory.
Acceleration Constant velocity Deceleration or waypoints, for the PID controller to Generating trapezoid trajectories is use to regulate the motor’s motion. The relatively straightforward. Refer to the Velocity trajectory generator sits outside the Circuit Cellar ftp site for a working PID control loop as shown in Figure 1. implementation.
Time For example, a simple trajectory generator provides constant accelera- WRITING CODE Figure 2—The trapezoid trajectory gets its name from the shape of the velocity profile. It is used to move a tion until the desired velocity is Let’s write some real code! I’m a big motor to a desired end-position in a controlled manner. reached. It holds this velocity until it fan of C++. Its class and class inheri- nears the desired endpoint position. tance concepts mean that I don’t have Next, it provides constant decelera- to write as much code. And writing less The control value at time k is equal to tion until the endpoint is reached. This code is right up there with watching the sum of the proportional, integral, trajectory results in the velocity profile less TV and reducing my cholesterol. and derivative terms (VPROPORTIONAL, VINTE- shown in Figure 2. It’s called a trape- This closed-loop motor controller
GRAL, and VDERIVATIVE) at time k. KP, KI, and zoid trajectory because of its shape. For lends itself nicely to a class hierarchy.
KD are the PID gains, which are easily simplicity, the acceleration and deceler- CAxesOpen is the base class and an determined through experimentation, a ation are equal in magnitude. Figure 2 easy starting point. It implements an process known as tuning. The PID con- troller is popular because of its effective- Listing 1—CAxesOpen and CAxesClosed form the first two classes in the motor controller class hier- ness and relative simplicity. All that’s archy and a majority of the code. The complete implementation can be found on the Circuit Cellar ftp site. required is a set of reasonable gain values. #define AC_MAX_AXES 4 Let’s consider the relevant problem class CAxesOpen of controlling a motor’s position (see {
Figure 1). VCONTROL becomes VPWM, public: which is the PWM value sent to the CAxesOpen(int servoAxes); // Constructor virtual ~CAxesOpen(); // Destructor motor’s PWM controller. VDESIRED becomes PositionDESIRED, which is the virtual int GetPosition(int axis); desired position of the motor, and VMEA- void SetPWM(int axis, int pwm); becomes Position , which //... SURED MEASURED }; is the measured position of the motor. class CAxesClosed : public CAxesOpen To get a feel for how the PID con- { troller works, consider the proportion- public: al term by itself. If the position error CAxesClosed(int servoAxes, int operationalAxes=1); // Constructor is large, so is the proportional term virtual ~CAxesClosed(); // Destructor and hence the resultant PWM value,
VPWM. This causes the motor to move void Periodic(); // Called once per control cycle quickly toward the desired position. bool Done(int axis); // Returns true if trajectory is finished As the motor closes in on the desired void SetPIDGains(int pGain, int iGain, int dGain); position (as the error decreases), the void Stop(int axis); // Stop immediately proportional term decreases, which void Hold(int axis, bool val); // Hold current position slows the motor. Thus, the propor- virtual void Move(int axis, // Perform trajectory move int endPosition, int velocity, int acceleration); tional term does almost all of the work. virtual int GetPosition(int axis); The other integral and derivative terms protected: correct for problems that the propor- // Called by GetPosition() tional term cannot correct by itself. virtual void ForwardKinematics(const int servoVal[], int operVal[]); You will better understand these // Called by Periodic() terms when I cover tuning a PID loop. virtual void InverseKinematics(const int operVal[], int servoVal[]); TRAJECTORY GENERATION private: void TrapezoidTrajectory(); The PID controller is designed to void PIDControl(); (Continued) get to the desired position
36 Issue 169 August 2004 CIRCUIT CELLAR® www.circuitcellar.com open-loop motor controller, which Listing 1—Continued. allows you to set the PWM value and int m_servoAxes, m_operationalAxes; get the position of each motor axis // Trajectory input parameters within a set of axes. As Listing 1 shows, int m_trajectoryEndPosition[AC_MAX_AXES]; CAxesOpen is extremely simple with int m_trajectoryVelocity[AC_MAX_AXES]; int m_trajectoryAcceleration[AC_MAX_AXES]; its two public functions. Note that unsigned int m_trajectory; // True if trajectory is active SetPWM() accepts a signed PWM value. // Trajectory generator output It is intended that the sign of this value int m_generatedTrajectoryVelocity[AC_MAX_AXES]; determine the direction of the motor. int m_generatedTrajectoryPosition[AC_MAX_AXES]; // PID controller variables CAxesOpen is an easy first step, but int m_pGain, m_iGain, m_dGain; it isn’t very useful by itself. You need int m_desiredPosition[AC_MAX_AXES]; to implement another class that closes int m_errorIntegral[AC_MAX_AXES]; the loop. The closed-loop controller int m_errorPrevious[AC_MAX_AXES]; unsigned int m_hold; // For maintaining the current position class is called (not surprisingly) }; CAxesClosed, and inherits from CAxesOpen. As shown in Listing 1, CAxesClosed is a little more com- Listing 2—The code that implements the PID position controller (PIDControl()) is surprisingly simple. plex. The complete source code is It is called from Periodic(), which is called once per control cycle. posted on the Circuit Cellar ftp site. void CAxesClosed::Periodic() CAxesClosed is bigger, but it’s doing { almost all of the work in the closed-loop TrapezoidTrajectory(); control system. It implements the PID InverseKinematics(m_generatedTrajectoryPosition, control algorithm and the trapezoid tra- m_desiredPosition); PIDControl(); jectory generator, and ties it all together. } Using CAxesClosed entails periodical- void CAxesClosed::PIDControl() ly calling Periodic() for each control { cycle from an external source. Looking at int error, pwm; for (int axis=0; axis
38 Issue 169 August 2004 CIRCUIT CELLAR® www.circuitcellar.com at least attempt to return to its origi- in oscillations that cannot be sufficient- example, if the motor comes to rest at nal position. Increasing the propor- ly reduced by increasing the derivative a position that is relatively close to its tional gain causes the motor to return gain. At this point, you’ve pushed desired position, the error is small. to its original position more quickly things too far. Reduce the proportional And, if the error is small, the propor- after being perturbed. Increasing the gain and find a suitable derivative gain tional term may not provide enough gain further will eventually result in the that produces the desired response PWM to move the motor the extra dis- motor overshooting the original position after perturbing the motor. tance to reach its desired position. This and then oscillating back and forth. Sometimes, friction or another dis- is likely to happen if your motor has Continue to increase the proportional turbance prevents the motor from static friction, or if your robot’s wheels gain until the motor starts to oscillate reaching its desired position. That’s have external forces acting on them, in this manner after being perturbed. where the integral term comes in. It such as those from an incline or hill. Oscillation is more pronounced in ensures that the error will always Because the integral term is inte- motors with low friction or drag. reach zero in the steady state. For grating the error over time, a nonzero Motors with large gear reduction tend to have more friction and oscillate less. The Lego motors have little fric- tion, which makes them efficient but more challenging to control. To reduce the oscillation, you need to increase the derivative gain. The derivative gain adds velocity damping: the deriv- ative of the error is equal in magni- tude but opposite in sign to the motor velocity. Thus, the derivative term wants to slow down the motor in direct proportion to the motor velocity. What else slows things down in pro- portion to velocity? Friction. Increasing the derivative gain is like adding friction to your motor, which may sound like something you should avoid. However, increasing the deriva- tive gain does not adversely affect the efficiency of your motor as friction does. Go ahead and continue to increase the derivative gain until the oscillation is under control. If increas- ing the derivative gain does not remove the oscillation, the proportion- al gain may be too high. Try reducing the proportional gain, setting the deriva- tive gain to zero, and starting over. We are shooting for a motor that returns to its original position as quickly as pos- sible and then stops (not oscillates) after being perturbed. It’s acceptable for the motor to overshoot the original position slightly before coming to rest. After the oscillation is under con- trol, you can choose to be satisfied and stop here, or you may want to see how far you can push things. If so, try increasing the proportional gain again, which will bring the oscillations back. Again, you can quell the oscillations by increasing the derivative gain. You can proceed in this manner until you reach a proportional gain that results
www.circuitcellar.com CIRCUIT CELLAR® Issue 169 August 2004 41 larly the proportional gain) for a bet- ter, faster responding system. A con- trol frequency of a few hundred hertz Translation is usually sufficient, however. When Left wheel Right wheel changing the control frequency, be positive direction positive direction sure to retune PID gains to achieve
Left motor Rotation Right motor the best response.
Left wheel Right wheel TESTING A SINGLE AXIS After tuning your PID control loop, you can fire up the trajectory genera- tor and move your motor. This entails calling Move() with your desired trajectory parameters. Move() Photo 2—The differential robot base is popular because of its simplicity. Only two motors are required. It’s usually more intuitive to specify motion in terms of translation and rotation instead of left and right wheel motion. starts the trajectory generator TrapezoidTrajectory(). If every- thing is working, the motor smoothly error causes the integral term to grow integral gain to zero. However, most accelerates to the desired velocity, gradually over time. Eventually, the control systems can benefit from a holds the desired velocity, and then integral term will become large small amount of integral gain. It decelerates and stops exactly at the enough to overcome the external force depends on how much steady-state desired position. Nice! You can play (e.g., friction or an incline). The larger error your motor typically experiences with the trajectory parameters until the integral gain, the faster it over- and how much you are willing to tol- the motor moves to your satisfaction. comes the external force. But bear in erate in your system. Note that it is possible to specify mind that a sufficiently large integral As I mentioned before, a higher con- velocity and acceleration values that gain can introduce oscillations to your trol frequency results in better con- are beyond your motor’s capabilities. system. Because the Lego motors have trol. A higher control frequency allows The source code for performing the test little static friction, I choose to set the you to increase the PID gains (particu- is posted on the Circuit Cellar ftp site.
42 Issue 169 August 2004 CIRCUIT CELLAR® www.circuitcellar.com You may have noticed that I have work is done within CAxesClosed. r ()right_wheel + left_wheel completely ignored units so far, translation = CDiffBase is simple by comparison. which makes specifying reasonable 2 r ()righht_ wheel − left_wheel trajectory values difficult. (What does rotation = WHERE ARE THE UNITS? a velocity of 1,000 actually mean?) I b When commanding your robot base, will remedy this in the next few sec- where r is the wheel radius of both specifying position, velocity, and tions. The lack of recognizable units wheels, and b is the distance between acceleration with recognizable units is in the lower levels of the control sys- them. Note that the right_wheel, important. But what units should you tem saves a significant amount of left_wheel, and rotation variables are choose? In the interest of saving com- computing bandwidth. expressed in radians. The inverse of putation, it’s a good idea to express I’ve described how to implement a these expressions, which map opera- these units as integers instead of float- complete closed-loop PID position tional space to servo space, is called ing point values. Thus, the units need controller with trajectory generator the inverse kinematics: to provide reasonable resolution when for a set of motor axes. This alone expressed as integers. Considering this, b × rotation gives you precise control of your translation − I’ve chosen millimeters for translation 2 robot’s motors. I could stop here, but left_wheel = units and milliradians for rotation units. this is where things start getting r To perform unit conversion, × interesting! The next sections will b rotation CDiffBase overrides Move() and per- + translation 2 integrate the kinematics of the robot right_wheel = forms conversion before passing the base into the control system. r arguments down to CAxesClosed (see Kinematics makes the robot base eas- Note that most robotics professionals Listing 4). Performing unit conversion ier and more intuitive to control. and scholars are accustomed to seeing in this manner is efficient because it the kinematics expressions in matrix occurs once per Move() command DIFFERENTIAL ROBOT BASE form instead of the expanded form instead of once per control cycle. The differential base is the classic shown here. Unit conversion for the differential robot base and probably the most pop- Now you’re ready to implement a base relies on two scaling constants ular. It consists of two motors and differential base controller class called (translationScale and drive wheels, as shown in Photo 2. CDiffBase (see Listing 4). Most rotationScale), which are passed Often, the base is circular and the importantly, note that CDiffBase is into the CDiffBase constructor (see wheels are mounted colinearly with derived from CAxesClosed and over- Listing 4). These scaling constants are the center of the base. This allows the rides InverseKinematics() and specified in units that can be easily robot to rotate around the center of the ForwardKinematics() with its own determined through calibration. For base, which simplifies path planning. versions. These versions contain the example, translationScale is spec- The differential base has two simplified differential base kinemat- ified in ticks per meter. Here, “ticks” motors and two degrees of freedom. ics, which allow CAxesClosed to are the units that are passed into When thinking about its motion, you convert operational space axis posi- CAxesClosed::Move() after unit usually think of it moving forward, tions into servo space axis positions conversion. What are ticks exactly? turning, moving backward, etc. In and vice versa. As a result, For the translation axis, they are units other words, you imagine the robot CDiffBase now accepts motion com- of distance; for the rotation axis, they translating, rotating, or both. In gen- mands (e.g., Move()) in terms of are units of angular displacement. eral, describing the robot’s motion in translation (axis index 0) and rotation Calibrating your robot base will reveal terms of translation and rotation is (axis index 1), which make up its the sizes of these units. more intuitive than describing the operational space. motion of each wheel (e.g., right Let’s examine what is going on. CALIBRATION wheel clockwise, left wheel counter- Assume a working implementation of Calibration accurately determines clockwise, etc.) your base class CAxesOpen such that the values of translationScale and I call the degrees of freedom (or axes) the left wheel is axis index 0 and the rotationScale. Determining these with which I prefer to describe motion right wheel is axis index 1. Next in constants through calibration is usual- (translation and rotation) my operational the hierarchy, CAxesClosed calls ly the method that yields the most space. The motors themselves (left and InverseKinematics() from within accurate robot base. right wheels) make up the servo space, Periodic() (see Listing 2). There are lots of ways to calibrate or alternatively the joint space when it InverseKinematics() takes opera- your base, but the freewheeling applies to robot manipulators.[1] tional space positions from the trajec- method is probably the easiest. Run a The mathematical expressions I use tory generator as inputs and outputs program that continuously prints the when mapping from servo space to the corresponding servo space posi- positions of the operational space axes operational space are called the for- tions. The PID controller uses these (translation and rotation) by calling ward kinematics. The forward kine- positions to control the position of CAxesClosed::GetPosition(). matics for the differential base are: each motor. Note that almost all of the While running this program, record www.circuitcellar.com CIRCUIT CELLAR® Issue 169 August 2004 43 the translation value that is being more importantly, because the base is printed. Roll the robot in a straight underconstrained, it’s possible for the line for 1 m, and record the transla- Wheel 0 wheels to fight each other in a sort of tion value again. Subtracting the two x b tug-of-war. The operational space con- values will yield the number of ticks troller prevents this while providing Wheel 3 per meter of translation, which is Wheel 1 y intuitive control. Rotation translationScale. Use the same method to determine Wheel 2 HUMR CLASS AND KINEMATICS rotationScale, except rotate the Figure 3 shows the four wheels and robot one revolution (360°) to deter- motors of the HUMR base, which are mine the number of rotation ticks per Figure 3—Only three omniwheels are required for numbered zero through three. The holonomic motion, but four omniwheels (shown here) revolution, which is rotationScale. provide more power and accuracy. When equipped with arrows indicate the positive motion You may download an implementa- 9-V Lego motors and 4-cm (diameter) omniwheels, the direction of each motor. The inverse tion of the calibration program from HUMR has a top speed of 50 cm/s and a top accelera- kinematics, which is fairly easy to 2 the Circuit Cellar ftp site. tion of 200 cm/s . derive using vector math, provide the You may be wondering why motion constraints required for our ForwardKinematics() and a simple example of moving to a spe- underconstrained system. Deriving the InverseKinematics() in Listing 4 cific goal location. A differential robot forward kinematics may seem tricky ignore r and b. Note that r and b only must first turn toward the goal before because there are four wheels serve as scaling constants for transla- it moves toward it. A holonomic robot (knowns) and three operational axes tion and rotation, you can move them simply heads straight toward the goal (unknowns), but the HUMR’s simple out of your kinematics calculations (no turning required), which is simpler right-angle geometry makes it possible and into the scaling constants and typically faster. to derive these expressions by inspec- (translationScale and There are many different holonomic tion. Forward kinematics are as fol- rotationScale). Because you deter- robot designs, but probably the sim- lows: mine your scaling constants through plest consists of independently driven r()wheel 1 − wheel 3 calibration, the values of r and b do omnidirectional wheels (see Photo 1). x = 2 not need to be explicitly known. That Omnidirectional wheels, or “omni- r()−wheel 0 + wheel 2 makes life easier! wheels,” have passive rollers evenly y = distributed along the outside periphery 2 AFFORDABLE HUMR of the wheel. The rollers provide the In the previous section, I covered omniwheel with an extra degree of rotation = −r ()wheel 0 + wheel 1 + wheel 2 + wheel 3 implementing an operational space freedom that permits the wheel to 4b differential robot controller. Although move orthogonally (sideways) with a differential base is simple, it only respect to the regular wheel motion. Inverse kinematics are as follows: has two degrees-of-freedom. This limi- I’ll refer to this extra degree of freedom tation, for example, makes it impossi- as the minor axis of the omniwheel. − ()y + b × rotation wheel 0 = ble to translate in a particular direc- The major axis is the degree of free- r tion unless the wheels are oriented in dom regularly associated with wheels. ()x −× b rotation wheel 1 = that direction. In other words, a differen- When a motor is attached to an omni- r tial robot needs to turn or rotate before wheel, the minor axis remains passive it can head in a particular direction. and the major axis becomes powered. ()y −× b rotation wheel 2 = A robot that is confined to a plane It is often the case that a robot will r (e.g., the floor) has three degrees of have as many motors as degrees of free- −()x + b × rotaation wheel 3 = freedom at most. These are commonly dom. When the number of motor axes r represented as two translation degrees exceeds the number of degrees of free- of freedom (x and y) and rotation, as dom, the system is underconstrained. where r is the wheel radius, and b is shown in Figure 3. In the field of The Holonomic Underconstrained the distance from any wheel to the mobile robotics, a robot with three Mobile Robot (HUMR) has four omni- center of the robot. degrees of freedom in a plane is consid- wheels, which is one more than is neces- The implementation of HUMR class ered holonomic. The differential base, sary for holonomic motion (see Photo 1). CHumrBase is similar to CDiffBase. for example, is lacking a degree of free- The four-wheeled HUMR has the CHumrBase, like its sibling, only dom and is considered nonholonomic. advantage of increased power and accu- requires two scaling constants: one for Why is a holonomic robot useful? racy as well as simplified kinematics the translation axes Holonomic robots can accelerate in and Lego construction. (translationScale) and one for the any direction at any time, which As with the differential robot base, rotation axis (rotationScale). As makes them highly maneuverable and the servo space of the HUMR provides before, you can move the r and b surprisingly easy to control. Consider an unintuitive operational space. But, terms out of the kinematics expres-
44 Issue 169 August 2004 CIRCUIT CELLAR® www.circuitcellar.com sions and into the scaling constants to translates along its x-axis while rotat- minimize the computation (see Listing 5). ing, for example, it will go in a circle, Furthermore, I recommend that the not in a straight line. This is because scaling constants be determined through the x- and y-axes are always pointing the freewheeling calibration process. in the same direction with respect to the robot. When the robot rotates, so EXPENSIVE FRISBEE do the axes. In order to move in a One of the more impressive results straight line, you need to rotate the of holonomic motion is the ability to translation axes in the opposite direc- rotate while translating in a straight tion with respect to the robot’s rota- line. I call this Frisbee motion because tion. Rotating the x- and y-axes it resembles the motion of, well, a entails a simple application of sine Frisbee. However, if your robot simply and cosine with respect to the robot’s
Listing 5—The InverseKinematics() routine implements Frisbee mode by rotating the x- and y- axes. By embedding different mathematical expressions in InverseKinematics(), all sorts of differ- ent and interesting modes are possible.
void CHumrBase::ForwardKinematics(const int servoVal[], int operVal[]) { operVal[X_AXIS] = ( servoVal[1] - servoVal[3])>>1; operVal[Y_AXIS] = (-servoVal[0] + servoVal[2])>>1; operVal[ROTATE_AXIS] = (-servoVal[0] - servoVal[1] - servoVal[2] - servoVal[3])>>2; } void CHumrBase::InverseKinematics(const int operVal[], int servoVal[]) { int cosRotation, sinRotation, rotation; int diffRotOperVal[2], diffOperVal[2]; if (m_frisbee) { rotation = GetPosition(ROTATE_AXIS); // Get rotation angle cosRotation = CosLut(rotation); sinRotation = SinLut(rotation); // Calculate velocity (compute difference) diffOperVal[X_AXIS] = operVal[X_AXIS] - m_prevOperVal[X_AXIS]; diffOperVal[Y_AXIS] = operVal[Y_AXIS] - m_prevOperVal[Y_AXIS]; // Rotate x and y axes and scale down by shifting right by 10 diffRotOperVal[X_AXIS] = (diffOperVal[X_AXIS]*cosRotation + diffOperVal[Y_AXIS]*sinRotation)>>10; diffRotOperVal[Y_AXIS] = (-diffOperVal[X_AXIS]*sinRotation + diffOperVal[Y_AXIS]*cosRotation)>>10; // Add to new position m_newOperVal[X_AXIS] += diffRotOperVal[X_AXIS]; m_newOperVal[Y_AXIS] += diffRotOperVal[Y_AXIS]; // Save for next iterration m_prevOperVal[X_AXIS] = operVal[X_AXIS]; m_prevOperVal[Y_AXIS] = operVal[Y_AXIS]; } else { m_newOperVal[X_AXIS] = operVal[0]; m_newOperVal[X_AXIS] = operVal[1]; } servoVal[0] = -m_newOperVal[Y_AXIS] - operVal[ROTATE_AXIS]; // Wheel 0 servoVal[1] = m_newOperVal[X_AXIS] - operVal[ROTATE_AXIS]; // Wheel 1 servoVal[2] = m_newOperVal[Y_AXIS] - operVal[ROTATE_AXIS]; // Wheel 2 servoVal[3] = -m_newOperVal[X_AXIS] - operVal[ROTATE_AXIS]; // Wheel 3 }
www.circuitcellar.com CIRCUIT CELLAR® Issue 169 August 2004 45 rotation angle: modest amount of computing power. For your next robot or motion-control x′ = cos() rotation × x + sin() rotation × y project, keep these ideas and methods y′ = − sin()rotatio nnrotation × x + cos() × y in mind. Your robot will thank you Implementing this functionality can with greatly improved performance! I be easily accomplished by embedding these expressions in the Rich LeGrand has 10 years of experi- InverseKinematics() function (see ence in robotics and multimedia sys- Listing 5). Note that tems. When he’s not tuning PID loops ForwardKinematics doesn't need to by hand, he works at Charmed Labs, be changed for now because it doesn't which provides advanced embedded affect the HUMR’s motion. I’ve intro- technologies for consumer and educa- duced a new variable called m_fris- tional use. He holds a B.S.E.E. and bee into InverseKinematics, B.A.C.S. from Rice University and an which, when set, rotates the x- and y- M.S.E.E. from North Carolina State axes with respect to the rotation angle University. Rich has authored domes- and enables Frisbee motion. It tic and international patents for holo- assumes that you have a reasonably nomic robot control. You can reach efficient means of obtaining the sine him at [email protected]. and cosine of the rotation angle, which is specified in milliradians. The PROJECT FILES CosLut() and SinLut() functions To download the code, go to ftp.circuit employ a look-up table of sine and cellar.com/pub/Circuit_Cellar/2004/169. cosine values scaled (multiplied) by 1,024 to make integer multiplies accu- REFERENCE rate. Scaling the results back down (dividing by 1024) is accomplished by [1] O. Khatib, “A Unified Approach to right-shifting the results by 10 bits. Motion and Force Control of Robot The implementations of SinLut(), Manipulators: The Operational CosLut(), the rest of CHumrBase, and Space Formulation,” IEEE Journal the video clips of Frisbee motion are Robotics and Automation, RA3, posted on the Circuit Cellar ftp site. 1987. Your implementation of Frisbee mode allows you to change the rota- RESOURCE tion angle however you wish without Vector math, www-2.cs.cmu.edu/ affecting the direction of motion. Aside ~pprk/physics.html. from being cool to look at, Frisbee mode can be useful when a sensor on SOURCES top of the robot needs to pan back and Omnidirectional wheels forth, for example. I’m sure you can Acroname, Inc. think of other modes that are useful for (720) 564-0373 whatever you want your robot to accom- www.acroname.com plish. Or, you may want to experiment with a different operational space. Xport 2.0 and Xport Robot Controller Implementing these ideas only requires Charmed Labs modifying the kinematics routines. (201) 444-7327 www.charmedlabs.com IMPROVED PERFORMANCE Mindstorms Robotics Invention I’ve covered quite a bit a ground in System this article: PID control, tuning, trajecto- Lego Co. ry generation, and operational space con- +45 79506070 trol for two different robot bases. I have www.lego.com implemented a class hierarchy that makes constructing closed-loop opera- Gameboy Advance and Gameboy tional space controllers straightforward Advance SP and applicable to practically any robot Nintendo of America, Inc. base. And I have accomplished this (800) 255-3700 with software that consumes only a www.nintendo.com
46 Issue 169 August 2004 CIRCUIT CELLAR® www.circuitcellar.com