Swerve drive - Kinematics simulation

Friday, February 10, 2023 | Posted in

As mentioned I am designing and building four wheel steering mobile robot for use in outdoor environments and rough terrain. In that post I mused that both the mechanical design and the software would be the most complicated parts of the drive system and thus the parts I should focus on first. At the moment I don't have good access to a workshop where I can experiment with the mechanical design so for now I'm focussing on the creation of the control software. Once I have the controller software working I can then use Gazebo to test the robot virtually to ensure that the code will work in the actual robot.

Unlike a differential drive a four wheel swerve drive has more degrees of freedom than needed, eight degrees of freedom (steering angle and wheel velocity for each drive module) for the control versus three spatial degrees of freedom (forward, sideways and rotate). This means that a swerve drive is an over-determined system, requiring the control system to carefully control the wheel velocities and angles so that they agree with each other, otherwise the wheels slip or drag.

Swerve drive degrees of freedom
Degrees of freedom for a swerve drive system.

While doing some research I found many different scientific papers describing algorithms for determining the forward and inverse kinematics of a four wheel steering system. There are however very few software libraries which implement the different control algorithms.

Most papers and libraries focus on the simpler case of an indoor robot that moves along a flat horizontal surface. In this case relatively simple kinematics approaches can be used. Unfortunately these algorithms fail when used in 3d uneven terrain. A different approach is required for that case. At the moment I too will be focussed on algorithms for driving on flat surfaces. At least until I have correctly working control code.

After reading a number of papers and looking at some of the available code I have come to the conclusion that I don't understand what is required for a successful swerve algorithm. There are many variables that influence the behaviour making it hard to visualize what goes on as the robot is moving around. So I wrote some code to simulate what is going on (roughly) in a swerve drive while it is moving and steering. I'm going to use this code to answer a number of questions I have about the different control algorithms.

For instance algorithms often calculate the desired end state and then set the wheel position and velocity to those required for that desired state. The question is does this algorithm automatically ensure that the intermediate states are synchronised, and if not does that matter? An example of this would be a robot moving linearly at 45 degrees and transitioning to an in-place rotation, as pictured below. During the transition all drive modules should be synchronised so that the center of rotation for the robot matches with the state of the drive modules.

Transition from 45 degree linear motion to in-place rotation.
Transition from 45 degree linear motion to in-place rotation.

Another example is that many of the code libraries add an optimization that reverses the wheel direction in favour of reducing the steering angle. For instance instead of changing the steering angle from 0 degrees to 225 degrees with a wheel velocity of 1.0 rad/s, change the angle to 45 degrees and reverse the wheel velocity to -1.0 rad/s. Again there are a number of questions about this optimization. For instance making a smaller steering angle change reduces the energy used, however stopping and reversing the wheel motion also takes energy, so what are the benefits of this optimization, if any? And how do the algorithms keep the wheel motions synchronised when performing the reversing optimization?

The following graphs show an example of the simulation output. The motion being simulated is that of a robot transiting from moving at a 45 degree straight path to an in-place rotation. The graphs display the status of the robot body, position and velocity and the status of the different drive modules, the angular orientation and the wheel velocity. The last graph depicts the location of the Instantaneous Centre of Rotation (ICR) for different combinations of drive modules. The ICR is the point in space around which the robot turns. If the control algorithm is correct then the ICR points for different drive module combinations all fall in the same location though out the entire movement pattern.

45 degree linear track to in-place rotation
Simulation data for a 45 degree linear track that transitions to an in-place rotation.

Looking at these graphs a couple of observations can be made. The first observation is that the ICR paths for the different module combinations don't match each other. This means that the drive modules are not synchronised and one or more wheels will be experiencing wheel slip. The second observation is that the linear control algorithm causes sharp changes in velocities leading to extremely high acceleration demands. It seems unlikely that the motors and the structure would be able to cope with these demands.

From this relatively simple simulation we can see that there are a number of behaviours that the control system needs to cope with:

  • The state of the drive modules actively needs to be kept in sync at all times, including during transitions from one movement state to another. This indicates that dynamic control is required and thus poses questions about the update frequency for drive module sensors and control commands.
  • The capabilities and behaviour of the motors needs to be taken into account in order to prevent impossible movement commands and also to ensure that the drive modules remain synchronised during movement commands that require fast state changes from the motors, e.g. to deal with motor deadband or fast accelerations.
  • The structural and kinematic limitations need to be considered when giving and processing movement commands.
  • Linear control behaviour is not ideal as it causes large acceleration demands so a better approach is necessary.

Now that I have some working simulation code what are the next steps? The first stage is to simulate some more simple movement trajectories to validate the simulation code. Once I have confidence that the code actually simulates real world behaviour I can implement different control algorithms. These algorithms can then be compared to see which algorithm behaves the best. Currently I'm thinking to implement

  • A control algorithm that uses a known movement profile for the robot base. It can then calculate the desired drive module state across time to match the robot base movement. The movement profile could be linear, polynomial or any other sensible profile.
  • A controller that optimizes module turn time by having it turn the shortest amount and reversing the wheel velocity if required.
  • A low jerk controller that ensures smooth movement of the robot body and drive modules.

In future posts I will provide more details about the different controller and model algorithms.

Swerve drive - Moving a robot in all directions, mostly

Sunday, December 4, 2022 | Posted in

Over the last year I have been using my SCUTTLE robot as a way of learning about robotics and all the related fields like mechanics and electronics. A major part of this journey is the desire to design and build an autonomous mobile robot from the ground up.

My goal is to build an off-road capable robot that can navigate autonomously between different locations to execute tasks either by itself or in cooperation with other robots. This is quite an inspirational goal that involves quite a few robot different parts, a lot of code and many hours of building and testing to achieve.

The chassis of the robot will have four drive modules. Each module has one wheel attached that will both be independently driven, and independently steerable. This configuration is called four wheel independent steering or swerve drive. These kind of steering systems are used in heavy transport, agriculture machines, mars rovers and robot competitions. The advantages of a swerve drive system are that:

  • It provides a high degree of mobility. In a swerve drive direction of movement and orientation are independent so the robot can face forwards while driving sideways. Additionally in a swerve drive the Instantaneous Center of Rotation (ICR) is not fixed to a specific line as it is with Ackermann steering or differential drive. This flexibility allows the swerve drive to combine rotational movements with linear movements in ways that other drive systems cannot.
  • It has normal size wheels which provide a high carry capacity. While omni-wheels have the similar degree of freedom as a swerve drive does, omni-wheels can often not carry the same load due to the lower carrying capacity of the rollers that allow the omni-wheels to move sideways.
  • It doesn't rely on wheel slip, as multi-wheel differential drive does. This means that it has lower power demands, so more of the motor torque can be used to move the robot forward.
  • It has the ability to traverse rough and dirty terrain due to the fact that all wheels are driven as well as using normal wheels on each drive module. Omni-wheels and mecanum wheels face more issues in these environments due to dust and dirt clogging up the wheels as well as having greater difficulty tackling obstacles.
  • It is able to keep ground disturbance to a minimum as it is able to steer the robot while minimizing sliding movement. Other drive systems, e.g. tracks or multi-axle differential drives, have a bigger impact due to the sliding movement required for these systems to turn the robot.

Of course the swerve drive system isn't a magical system that only has advantages. There are also plenty of disadvantages. For instance swerve drive systems:

  • Are mechanically complicated. They require multiple motors per unit and multiple units per robot. On top of that there are usually a number of mechanical components, gears and bearings, involved in getting a working swerve drive.
  • Need a complicated control system. Swerve systems are generally over-determined, i.e. they have more degrees of freedom in the drive system, 2 per drive module, than there are degrees of freedom in the robot, 2 translation directions and a rotation. This means that all modules have to be synchronised at all times in order to prevent wheels from being dragged along. The available degrees of freedom combined with the synchronisation demand means some complicated math is required to make a swerve drive control work.
  • Similar to the control side of the drive determining the position and velocity of the robot using wheel odometry requires more complicated math. This is due to the fact that the different drive modules don't necessarily agree with each other.
  • Have more failure modes than other drive systems due to the fact that there are more moving parts.

So with all these complications why would I try to build a swerve drive as my second robot and not a differential drive robot or something similar. As pointed out previously there are good reasons to use a swerve drive in an outdoor environment, i.e. high agility, good load capacity, traction from all wheels, low ground impact. However the main reason I want to design and build a swerve drive is because it is a challenge. Swerve drives are complicated and designing and building one involves solving interesting problems in mechanical engineering, electrical engineering and software engineering.

There is currently no complete design for this robot yet, however there is a short list of design decisions that have been made so far.

  • It will be a four wheel swerve drive robot. Swerve drives have been built with anything from three wheels up, e.g. the Curiosity mars rover has 6 drive modules. The reason to use four modules is that it will be symmetrical and still minimize the number of parts necessary.
  • The software for the robot will be using ROS2 Humble. Using ROS should provide me with a base framework and a lot of standard capabilities, like the navigation stack, that I won't have to write myself. Additionally ROS has a decent simulation environment that will allow me to test my code before putting it on a real robot.
  • The hardware will be controlled using ROS2 controllers. This will allow me to abstract the hardware so that I can better test the controller.
  • The initial design will be an indoor model and about the same size as my SCUTTLE robot is. This will simplify the initial design and allow me to compare with SCUTTLE.

The parts of the robot that I expect to be complicated and quite possibly show stoppers are the software and the mechanical design. For the software the drive controller software, which translates the requested velocity commands to motor commands for both the drive and steering motors, will be complicated as it needs to make sure that all drive modules are the correct state. This piece of software also needs to handle all the error conditions that occur.

On the mechanical side I need to design the drive module such that it can drive the wheel forwards and backwards while allowing ‘infinite’ steering rotation. This will require a co-axial setup and a bit of gearing. The second complicated part of the mechanical design is the inclusion of a suspension system. Ideally the motors should be attached to the sprung side of the suspension system so that they don't get exposed to excessive vibration. This however will complicate the mechanical design.

My plan is to work on the control software first. I can test that software using simulation and so figure out if I can even make it work. I have created a simple URDF model that uses ROS2 controllers to simulate a four wheel steering platform. This allowed me to learn more about ROS2, ROS2 controls and how the interaction of those two with Gazebo works.

At the moment I'm implementing a prototype for the controller in python so that I can use the model to test if my algorithm works before I turn it into a proper ROS2 controller, which will need to be written in C++.

Once I have some kind of controlling software I am aiming to build a single drive module with a drive motor, a steering motor and the mechanical assembly that allows a single wheel to be steered and driven. I will use this module to work out both the details on the mechanical and software sides of the project.

Once I have the controller and the drive module working properly I will build a simple robot, similar in size to my SCUTTLE robot to further work on swerve drives. It will take me a little while to get to that state though. In the mean time I will keep working on my design and documenting my journey.

Starting robotics - Fixing the wheel encoders

Tuesday, November 15, 2022 | Posted in

While testing the bumper for SCUTTLE I noticed that when reversing SCUTTLE would start a turn instead of driving straight backwards due to one motor turning faster than the other motor. There are a number of reasons this could be happening, for instance the driver code isn't properly commanding the motors, or the encoders are returning incorrect data, etc..

Encoder output for failing encode
Encoder output for failing encoder

The first thing I did for my investigation was to make sure my encoders provide sensible data. The non-ROS SCUTTLE code contains a useful Python script to measure the current value of the wheel encoders in a loop. I ran this code for a few minutes while the wheels were stationary. In theory the results of this measurement should be two consistent values, one for the left encoder and one for the right encoder. As you can see in the graph it turns out that the angle measurement provided by the left encoder was very noisy.

While a noisy encoder shouldn't by itself cause the incorrect reversing pattern, it will make it more difficult to find the actual cause of the problem. So before I address the bumper reversing behaviour I needed to fix the encoder noise. The two main reasons for noisy encoder data that I could think of were:

  • The encoder is broken in some way. SCUTTLE uses the AS5048A position sensor which is relatively robust, but can be broken if you send the I2c commands on the wrong pins. Which could happen if you say ... put the connector on the wrong way when assembling your SCUTTLE ...
  • The distance between the encoder and the magnet on the motor shaft isn't correct. The specifications for the encoder state that the distance between the chip and the magnet on the motor shaft should be between 0.5mm and 2.5mm, assuming a magnet of the recommended size and strength is used.

I tried measuring the distance between the encoder and the magnet, but that isn't easy. There is no way to get a measuring tool near that space so I had to resort to indirect measurements. I measured the left and right brackets. They were about 0.5mm difference in size. That doesn't seem much but it would be enough to push the encoder out too far from the magnet.

One solution would be to slightly sand the taller bracket so that both brackets are the size height. In doing that I would have to be very careful to ensure that I sand the bracket flat, i.e. with no change in angle in the face. A second solution is to remove the encoders from their brackets and swap the brackets over.

Encoder output for functioning encoders
Encoder output for functioning encoders

As the latter is relatively easy to do that is the first solution I tried. I took the encoders of their brackets and attached them to the other bracket. These brackets were put back on SCUTTLE and the test script was executed once again. And this time both encoders gave me consistent results. I was expecting the other encoder to go bad due to distance, but apparently both encoders are with in the distance specification with this new configuration.

With the encoder issue fixed I can get back to diagnosing the issue with the bumper reverse action. My current suspicion is that the issue is caused by the fact that the the current SCUTTLE driver code is written as an open loop. This means that there is no feedback to the motor control software that indicates how fast the wheels are actually turning in response to a given motor input. And because even motors of the same type are all slightly different, they all react differently to the same motor input. In my case at low speeds one of my motors responds earlier than the other motor. In the end this means that at low speeds the bumper code thinks scuttle is driving backwards in a straight line while it is actually driving around in circles.

Starting robotics - Building a bumper for scuttle. The electronics

Friday, October 21, 2022 | Posted in

The final part of building a bumper for SCUTTLE is to assemble the electronics component which translates the movement of the bumper into signals which can be processed by the bumper software. In order to do this I designed a simple circuit using KiCad and advice from one of the many robotics forums.

SCUTTLE bumper electronics schematic
SCUTTLE bumper electronics schematic made in KiCad

The circuit contains a microswitch as the trigger. The switch is connected to one of the 3.3V output pins on the raspberry PI 4 board on one side and to one of the GPIO pins on the other side. When the switch is depressed the circuit closes and the GPIO pin is driven to 3.3V, which is considered a high signal. In order to ensure that the voltage on the GPIO pin is 0V when the circuit is not closed I added a pull down resistor. On the raspberry PI it is possible to programmatically add a pull down resistor, however because I'm using this as a learning exercise I thought it would be more suitable to include a physical pull down resistor in the circuit.

The next thing I wanted from the circuit was the ability to see if the circuit was closed or not, so that when I'm debugging it is obvious if there is a problem with the power, electronics or software. For this purpose I added a yellow LED to the circuit, which lights up when the circuit is closed. Adding the LED then adds the requirement to protect it from over current in case that the input pin was programmed to be an output pin by mistake. For this purpose I added a resistor next to the input pin.

SCUTTLE bumper electronics boards
SCUTTLE bumper electronics boards

After testing the circuit on the breadboard the next step was to solder two switch boards and a distribution board. Each switch board would have a micro-switch, the LED, the resistors and a JST-XH three pin connector for power, ground and signal wires. The distribution board would have four JST-XH three pin connectors and one JST-XH six pin connector. The four three pin connectors would allow me to have a front bumper and a rear bumper, each having a switch board on the left and the right. I used JST-XH connectors instead of the Dupont connectors because the JST connectors are directional, thereby removing any potential issues with plugging the connector in the wrong way.

Part of this journey involved learning how to solder electronics components. I bought a Weller WE 1010 soldering station for this and future jobs. I managed to do a reasonable job soldering the parts but it is obvious that I still have a lot to learn when it comes to soldering.

SCUTTLE bumper electronics assembled
SCUTTLE bumper electronics assembled

The last task was to crimp the connectors and connect the boards. Because the raspberry Pi has a Dupont header I needed to crimp both Dupont connectors and JST-XH connectors. For the Dupont connectors I got an Iwiss SN-025 crimper. It works pretty well for those connectors. However the crimper dies are too wide for the JST-XH connectors. So to crimp the JST-XH connectors I had to get a different crimper with narrower dies. So for this I got an Engineer PAD-11 crimper. While this is not a ratcheting crimper it works really well, at least for the JST-XH connectors.

So now that the mechanical setup is done, the software is, mostly, done and the electronics have been soldered and connected my bumper works. Sort of. It turns out that there is a problems with my SCUTTLE that make the bump response only work partially. It seems that there is an issue with the wheel encoders, which causes the software to not know how much one of the wheels has rotated. I will discuss fixing the encoders in another post.

Starting robotics - Building a bumper for scuttle. The software

Friday, September 9, 2022 | Posted in

In my previous post I talked about creating a bump sensor for my SCUTTLE robot. After creating the mechanical design I started working on the software. There are two parts to the software, translating the state of the micro switches and turning the state changes into movement commands for the robot, i.e. if the robot runs into an obstacle it should stop and reverse its last movement. I decided to create a ROS node for each of these actions, i.e. one node for the movement generation and one to translate the switch states. The reason for creating two nodes instead of putting all the code into one node is that this allows me to run the movement generation code both in a simulation and on the physical robot. So I gain the ability to test more of my code, but I lose a bit of performance because the two nodes communicate using messages which is slower than just using method calls.

To test the bump sensor code I use Gazebo to simulate how the bump sensor would work. Gazebo has the ability to calculate collisions and create ContactsState messages based on these collision calculations. So I created a third ROS node to translate these Gazebo messages to my own bumper event messages. With that I can test my obstacle response code in Gazebo which provides a more controlled environment than the physical robot does.

The first thing to do is to update the robot definition to include the bumper. This is done by adding the links and joints that make up my bumper to an URDF file. This URDF file is linked to the main model description for SCUTTLE. After that I added the information for the Gazebo bumper sensor. In the URDF file this looks as follows:

    <gazebo reference="front_bumper_plate_left_link">
        <sensor name="front_bumper_left" type="contact">
            <plugin name="gazebo_ros_bumper_controller_front_left" filename="libgazebo_ros_bumper.so">

I found that defining a contact sensor for Gazebo requires getting the link ID correct. In order to do so you need to follow these steps:

  • The bumper contact information needs to be defined inside a gazebo element. This gazebo element should have a reference attribute that points to the link that is to be used as the bumper surface.
  • The name for the collision element needs to be found after translation to the SDF format. Normally this is something Gazebo does internally. In this case you'll need to do this manually. If your geometry is defined in an xacro file then you first need to translate this to URDF using the rosrun xacro xacro --inorder -o model.urdf model.urdf.xacro command. After that you can translate the URDF to SDF with the gz sdf -p scuttle.urdf > scuttle.sdf command. Once you have the SDF file you can search for the correct element IDs.

In the above example the collision name, base_link_fixed_joint_lump__front_bumper_plate_left_cl_collision_3, is the one you need to extract from the SDF file. Note that it can change if you make changes to your xacro / URDF file, so in that case you will need to extract it again.

Once you have defined the URDF Gazebo will generate contact messages when the bumper hits an object. These contact messages contain information describing where the contact occurred on the geometry mesh, so in theory I could have used this information to determine if the contact would trigger the left limit switch or the right limit switch or both. However that requires a decent amount of calculations which is both work and introduces the potential for errors. So I opted to split the bumper into two parts, one for the left side and one for the right side. If anything contacts anywhere on the left side I consider that a trigger for the left part of the bumper and similar for the right side.

SCUTTLE with bumper in RViz
SCUTTLE with bumper in RViz

With the URDF work done I started writing the code for the different ROS nodes. First the gazebo translator node. This node subscribes to the ContactsState messages that Gazebo sends when the bumper geometry collides with something. These messages are then translated to a BumperEvent message for further processing.

One interesting observation about the Gazebo contact messages are that they exhibit something similar to switch bounce, in other words it seems that the contact is intermittent even if the bumper plate is in solid contact with the object. I'm guessing that this is caused by the fact that calculating collisions between moving surfaces is difficult. In the end it doesn't matter what causes this behaviour though because we need to deal with it in some sensible way.

Once the bumper event messages have been generated they are processed by the second node. This node subscribes to both odometry events and bumper events. Internally it keeps track of the motion state of SCUTTLE. On reception of a bumper event the node sends a velocity command to stop the robot. Once the robot has stopped it sends commands for it to reverse course far enough that it is no longer contact with the obstacle.

One thing to note is that while the bumper code is working to reverse course after hitting an obstacle, the other parts of ROS, e.g. the navigation stack, are probably still trying to steer the robot in the original direction because those parts don't know about the obstacle. After all it is not on the navigation map. This results in many different velocity commands being send, which could be very confusing for the robot. One design decision was to make the bumper code unaware of other nodes. This was done because allowing the bumper code to supress velocity commands from other nodes would imply that the bumper code is always the most important publisher when it comes to velocity commands. There are cases where this isn't true, e.g. when using the bumpers to park SCUTTLE against a specific object like in the case of a docking station.

To ensure that a consistent set of velocity commands are sent to the motors I used the twist multiplexer node which takes in velocity commands from many nodes and forwards them using a priority-based scheme. In the current implementation the priorities are, from low to high

  • Navigation
  • Keyboard
  • Bumper
  • Joypad / Joystick

By using the twist multiplexer mode it is easy for users to change the priority by changing a configuration file instead of having to change the bumper code.

With that the bumper code is able to make SCUTTLE respond to objects it bumps into. The next step in the process of adding bump sensors to SCUTTLE is to assemble the electronic components so that the movement of the bumper can be registered and reported to the software components.

Finally a addition to the software that needs to be made is the ability to add the obstacles in the navigation map so that SCUTTLE can navigate around the newly discovered obstacles.