Mobile Robot Teleoperation and Go-To-Goal with Python
By: Johan Schwind
Tags: Jetson Nano, Jupyter Notebook, Python, Robotics

Models Matter

Something that became very clear to me when building this system is that mathematical models are required to build a robust behavior. The ones I use here are based on Dr. Magnus Egerstedt’s work at Georgia Tech’s School of Electrical and Computer Engineering. They will help us to achieve two things:

· Calculating Cerus’ pose

· Calculating the error between the Cerus’ pose and the goal location

Calculating the Pose

We know that our robot is initialized at x, y, phi = 0 (we can also initialize it with different values, but the initial world position has to be known). Once moving, we can calculate the distance the robot has moved using the encoder ticks of our left and right wheels using the following differential drive model:

Dl = 2 * 𝜋 * R * deltaTicksLeft / N

Dr = 2 * 𝜋 * R * deltaTicksRight / N

Where Dl and Dr are the distance driven by the left and right wheels, R is the radius of the wheels and N is the number of encoder ticks per wheel revolution. We can then calculate the centerline distance driven Dc (this is how much the center of our robot has moved):

Dc = (Dl + Dr) / 2

Using this centerline distance, we can calculate the updated pose x’, y’, phi’:

x’ = x + Dc * cos(phi)

y’ = y + Dc * sin(phi)

phi’ = phi + (Dr — Dl) / L

Where L is the track width of our robot. Implementing this in Python looks like this:

Calculating the Error to Goal

Next, we want to calculate the error between the robot’s pose and the goal location.

phidesired = arctan(ygoal-y)/(xgoal -x)

Where phidesired is our desired heading to get to the goal location defined by xgoal and ygoal.

From this, we can calculate the heading error:

errorheading = arctan2(sin(phidesired-phi), cos(phidesired-phi))

Using arctangens2, we ensure that the heading error remains within -𝜋 and 𝜋 and prevents our robot from spinning out of control.

Similarly, we can calculate the X and Y error:

errorx = xgoal-x

errory = ygoal-y

In Python code:

Putting it All Together

With a way to calculate and update the pose of our robot while moving and a way to calculate the error between the pose and the goal location, we can now implement our behavior to minimize the error and reach the goal location. We use a Cerus class to keep track of important parameters:

We then define a moveRobot() function, that reads the encoder values from the Arduino via serial, calculates the delta ticks and calls calculatePose() with these. Finally, it calls calculateError() and assesses if the error on X and Y is greater than 5 cm. If it is, it issues a move command by making a call to twist(). The 5 cm tolerance are important — giving no tolerance here can make the robot spin like crazy when it reaches the goal location.

For simplicity’s sake, we drive to the goal location at a fixed velocity of constVel = 0.2 and only vary our angular velocity — akin to sitting in a car on cruise control where steering left and right is the only control.

It is also worth noting that we assess if serial data from the Arduino is in the buffer by evaluating ser.inWaiting(). If you remember from article two, we send serial data every 50 ms, effectively causing the Jetson to recalculate the pose and error every 50 ms. Purely waiting for serial data blocks the Jetson from doing other things and recalculating the pose too often caused all sorts of inaccuracies when I was testing. This implementation seemed to work best. If for some reason the Python main loop of this program is slower than 50 ms, the robot won’t drive properly either.

Lastly, we simply loop while not at the goal location and print a message when we’ve reached the goal:

while not atGoal:

moveRobot() print(“Robot at goal position.”)

This code can easily be expanded to accept a list of goal locations (or ‘waypoints’), that the robot will travel to in a sequence, allow it to move along a complex path.

You can find the complete code here, on GitHub.

Oh, Drift!

When I simulated this code, it worked perfectly. The robot would rotate smoothly and accurately drive to its goal location every time. Testing this in real life, however, the robot would often miss its goal by quite a bit, especially when it had to turn a lot to reach it. The reason for that is that the Mecanum wheels I chose for Cerus are quite slippery and the wheelbase quite long, causing significant drift. There are ways to counter this, including using wheels with more grip, shortening the wheelbase, or using other sensors to determine location, including an IMU. They are part of a long list of things things that I wasn’t able to cover in this series — but will hopefully be able to write about in the future.

Conclusion

With this series, we’ve built hardware and low-level controls for a robot and have implemented convenient teleoperation and a first simple behavior. This was a big learning opportunity for myself and I hope you found it useful, too.