Mind vortex

Swerve drive - Motion profiles

In the last few posts I have described the simulations I did of a robot with a swerve drive. In other words a robot with four wheels each of which is independently driven and steered. I did simulations for the case where we specified movement commands directly for the drive modules and one for the case where we specified movement commands for the robot body which were then translated in the appropriate movements for the drive modules. One of the things you can see in both simulations is that the motions is quite ‘jerky’, i.e. with sudden changes of velocity or acceleration. In real life this kind of change would be noticed by humans as shocks which are uncomfortable and can potentially cause injury. For the equipment, i.e. the robot parts, a jerky motion adds load which can cause failures. So for both humans and equipment it is sensible to keep the 'jerkiness' as low as possible.

In order to achieve this we first need to understand what jerk actually is. Once we understand it we can figure out ways to control it. Jerk is defined as the change of acceleration with time, i.e the first time derivative of acceleration. So jerk vector as a function of time, \vec{{j}}(t) can be defined as:

\vec{j}(t) = \frac{d\vec{a}(t)}{dt} = \frac{d^{2}\vec{v}(t)}{dt^{2}} = \frac{d^{3}\vec{r}(t))}{dt^{3}}

Where

• t is time.
• \frac{{d}}{{dt}}, \frac{{d}}{{dt^{2}}}, \frac{{d}}{{dt^{3}}} are the first, second and third time derivatives.
• \vec{{a}} is the acceleration vector.
• \vec{{v}} is the velocity vector.
• \vec{{r}} is the position vector.

From these equations we can for instance deduce that a linearly increasing acceleration is caused by a constant jerk value. And a constant jerk value leads to a quadratic behaviour in the velocity. A more interesting deduction is that an acceleration that changes from a linear increase to a constant value means that there must be a discontinuous change in jerk. After all a linear increasing acceleration is caused by a constant positive jerk, and a constant acceleration is achieved by a zero jerk. Where these two acceleration profiles meet there must be a jump in jerk.

With that you can probably imagine what happens if the velocity has a change from a linearly increasing value to a constant value. The acceleration drops from a positive constant value to zero. And the jerk value displays spikes when the acceleration changes. For the linear motion profile this behaviour is amplified as shown in the plot below. The change in position requires a constant value for velocity which requires significant acceleration and jerk spikes at the start and end of the motion profile.

So in order to move a robot, or robot part, from one location to another in a way that the jerk values stay manageable we need to control the velocity and acceleration across time. This is normally done using a motion profile which describes how the velocity and acceleration change over time in order to arrive at the new state at the desired point in time. Two of the most well known motion profiles are the trapezoidal profile and the s-curve profile.

The trapezoidal motion profile

The trapezoidal motion profile consist of three distinct phases. During the first phase a constant positive acceleration is applied. This leads to a linearly increasing velocity until the maximum velocity is achieved. During the second phase no acceleration is applied, keeping the velocity constant. Finally in the third phase a constant negative acceleration is applied, leading to a decreasing velocity until the velocity becomes zero.

The equations for the different phases are as follows

The acceleration, velocity and position in each phase of the trapezoidal motion profile can be described with the standard equations of motion.

v_n(t) = v_{n-1} + a_{n-1} t

r_n(t) = r_{n-1} + v_{n-1} t + \frac{1}{2} a_{n-1} t^2

Where

• t is the amount of time spend in the current phase.
• n is the current phase

For my calculations I assumed that the acceleration phase and the deceleration phase take the same amount of time, thus they have the same acceleration magnitude but different signs. With this the differences for each phase are:

1. a(t) = a_{{max}}
2. a(t) = 0
3. a(t) = -a_{{max}}

My implementation

To simplify my implementation of the trapezoidal motion profile I assumed that the different stages of the profile all take the same amount of time, i.e. one third of the total time. In real life this may not be true because the amounts of time spend in the different stages depend on the maximum reachable acceleration and velocity as well as the minimum and maximum time in which the profile needs to be achieved. Making this assumption simplifies the initial implementation. At a later stage I will come back and implement more realistic profile behaviour.

Additionally my motion profile code assumes that the motion profile is stored for a relative timespan of 1 unit. If I want a different timespan I can just multiply by the desired timespan to get the final result. For this case we can now determine what the maximum velocity is that we need in order to travel the desired distance.

r = 0.5 * v * t_{accelerate} + v * t_{constant} + 0.5 * v * t_{decelerate}

Where

• t_{{accelerate}} is the total time during which there is a positive acceleration, which is one third of the total time.
• t_{{constant}} is the time during which there is a constant velocity, which is also one third of the total time.
• t_{{decelerate}} is the time during which there is a negative acceleration, which again is one third of the total time.

Simplifying leads to

r = \frac{2}{3} v t \Longrightarrow v = \frac{3}{2} \frac{s}{t}

Using this maximum velocity and the equations for the different phases I implemented a trapezoidal motion profile. The results from running this motion profile are displayed in the plots above. These plots show that the trapezoidal has no large spikes in the acceleration profile when compared to the linear motion profile. additionally the jerk spikes for the trapezoidal motion profile are significantly smaller than the ones generated by the linear motion profile. So we can conclude that the trapezoidal motion profile is a better motion profile than the linear profile. However there are still spikes in the jerk values that would be detrimental for both humans and machinery. So it would be worth finding a better motion profile. That motion profile is the s-curve motion profile discussed in the next section.

The s-curve motion profile

Where the trapezoidal motion profile consisted of three different phases, the s-curve motion profile has seven distinct phases:

1. The ramp up phase where a constant positive jerk is applied. Which leads to a linearly increasing acceleration and a velocity ramping up from zero following a second order curve.
2. The constant acceleration phase where the jerk is zero. In this phase the velocity is increasing linearly.
3. The first ramp down phase where a constant negative jerk is applied. This leads to a linearly decreasing acceleration until zero acceleration is achieved. The velocity is still increasing, following a slowing second order curve to a constant velocity value.
4. The constant velocity phase where the jerk and acceleration are both zero.
5. The first part of the deceleration phase where a constant negative jerk is applied. Again this leads to a linearly decreasing acceleration, and a velocity decreasing following a second order curve.
6. In this phase the acceleration is kept constant and the velocity decreases linearly.
7. The final phase where a constant positive jerk is applied with the goal to linearly increase the acceleration until zero acceleration is reached. The velocity will keep decreasing following a second order curve, until it too reaches zero.

As with the trapezoidal motion profile, the acceleration, velocity and position in each phase of the s-curve motion profile can be described with the standard equations of motion. The difference is that the acceleration is a linear function, which introduces a jerk value to the equations.

a_n(t) = a_{n-1} + j_n t

v_n(t) = v_{n-1} + a_{n-1} t + \frac{1}{2} j_n t^2

r_n(t) = r_{n-1} + v_{n-1} t + \frac{1}{2} a_{n-1} t^2 + \frac{1}{6} j_n t^3

Where

• t is the amount of time spend in the current phase.
• n is the current phase

As with the trapezoidal motion profile I assumed that the acceleration and deceleration phases span the same amount of time. Again this means the acceleration and deceleration have magnitude but different signs. The differences for each phase are:

1. j(t) = j_{{max}}
2. j(t) = 0
3. j(t) = -j_{{max}}
4. j(t) = 0, a(t) = 0
5. j(t) = -j_{{max}}
6. j(t) = 0
7. j(t) = j_{{max}}

My implementation

To simplify my implementation of the s-curve motion profile I assumed that all stages, except stage 4, all take the same amount of time, i.e. one eight of the total time. I assumed that stage 4 would take one quarter of the time. Like with the trapezoidal profile making this assumption simplifies the initial implementation. At a later stage I will come back and implement more realistic profile behaviour.

Additionally my motion profile code assumes that the motion profile is stored for a relative timespan of 1 unit. If I want a different timespan I can just multiply by the desired timespan to get the final result. For this case we can now determine what the maximum velocity is that we need in order to travel the desired distance.

Using the equations above I implemented a s-curve motion profile. The results from running this motion profile are displayed in the plots above. These plots show that the s-curve removes the spikes in the jerk profile when compared to the trapezoidal motion profile. This indicates that the s-curve motion profile achieves the goal we previously set, to minimize the jerky motion.

For some applications it is important to provide even smoother motions. In this case the motion profiles may need to take into account the values of the fourth, fifth or even sixth order time derivatives of position, snap, crackle and pop. At the moment I have not implemented these higher order motion profiles.

Use in the swerve simulation

Having these motion profiles is great, however by themselves they are of little use. So I added them to my swerve drive simulation to see what the differences were between the different motion profiles.

Before we look at the new results it is worth looking at the simulations using the linear motion profile. I made one for module control and one for body control. In these simulations you can see that with the linear motion profile the jerk spikes are quite large, especially in the case of the module control simulation. The body control approach performs a little better with respect to the maximum levels of jerk, however the values are still far too high.

The simulations for module control and body control using the trapezoidal profile show a significant decrease in the maximum value of the acceleration and jerk values, in some cases by a factor 15. As expected from the previous discussion there are still some spikes, especially for the steering angles. The changes for the module control case are more drastic than for the body control case, probably due to the fact that the values were very high for the combination of the module control with a linear motion profile. Interestingly the acceleration and jerk maximum values are lower for the module control approach than they are for the body control approach. This is most likely due to the fact that in order to keep the drive modules synchronised relatively high steering velocities are required. For instance, the module control approach using the trapezoidal motion profile sees a maximum steering velocity of about 1.8 radians per second. Compare this to the the body control approach with the trapezoidal motion profile which sees a maximum steering velocity of about 3.8 radians per second.

When we look at the simulations using the s-curve motion profile we can see that the maximum acceleration and jerk values actually increase when compared to the trapezoidal motion profile, except for the values of the steering angle jerk when using the module control approach. It seems likely that these increases are due to the fact that the motion is executed over the same time span, but some of the time is used for a more smooth acceleration and deceleration. This means that there is less time available to travel the desired ‘distance’ which then requires higher maximum velocities and maximum accelerations. The s-curve motion profile does smooth out the movement profiles which would lead to a much smoother ride.

What is next

So now that we have a swerve drive simulation that can use both module based control and body based control as well as have different motion profiles, what is next? There are a few improvements that can be made to the simulation code to further made to the simulation and a path of progression.

The first improvement lies in the fact that none of the motion profiles, linear, trapezoidal and s-curve, are aware of motor limits. This means that they will happily command velocity, acceleration and jerk values that a real life motor would not be able to deliver. In order to make the simulation better I would need to add some kind of limits on the maximum reachable values. This would be especially interesting when using body oriented control. Because the high velocities and accelerations are needed to keep the drive modules synchronised. If one of the drive modules is not able to reach the desired velocity or acceleration then the other modules will have to slow down before they reach their limits. To control the drive modules in such a way that all of the modules stay within their motor limits while also keeping them synchronised requires some fancy math. At the moment I'm going through a number of published papers to see what different algorithms are out there.

The second improvement that is on my mind is to implement some form of path tracking, i.e. the ability to follow a given path. This would give the simulation the ability to better show the behaviour of a real life robot. In most cases when a robot is navigating an area the path planning code will constantly be sending movement instructions to ensure that the robot follows the originally planned path. This means that motion profiles need to be updated constantly, which will be a challenge for the simulation code. Additionally having path tracking in the simulation would allow me to experiment with different algorithms for path tracking and trajectory tracking, i.e. the ability to follow a path and prescribe the velocity at every point on the path. And theoretically with a swerve drive it should even be possible for the robot to follow a trajectory while controlling the orientation of the robot body.

Finally the path of progression is to take the controller code that I have written for this simulation and use it with my ROS2 based swerve robot.

Swerve drive - Movement animations

In my last two posts I talked about different control methods for a swerve drive robot. One method controls the movement of drive modules directly. The other method controls the movement of the body and derives the desired state for the drive modules from that.

To see the difference between these control methods my simulation code created all kinds of interesting plots like the ones above. However I was still having trouble visualizing what was actually happening, especially in the case of the movement of the Instantaneous Centre of Rotation (ICR), i.e. the rotation point for the robot at a given point in time. The lower left graph shows the paths the ICR for different combinations of drive modules. While it looks pretty it does not make a lot of sense to me.

So to address that issue I updated the simulation code to produce some animations that display the position of the robot and the wheels as well as a number of plots for the state of the robot body and the drive modules. To create the animations I used the FuncAnimation class that is available in matplotlib. The animations can then either be turned into an HTML page with animation controls using the HTMLWriter, or into MP4 video files using the FFMpegWriter. In order to get reasonable performance when using the animation functions in matplotlib it is important to update the plots instead of drawing new ones. This can be done using the set_data function, for instance for updating the position of the robot body. It is good to keep in mind that even with this performance improvement the creation of the animations isn't very fast for our robot simulation because a large number of image frames need to be made. For the 6 second movement in the animation anywhere between 150 and 600 frames need to be created.

The animation above shows how the robot behaves when using the direct module controller. As you can see in the video different pairs of wheels have different rotation points, signified by the red dots. As the movement progresses these rotation points have quite a large range of motion. This indicates that the wheels are not synchronized and most likely some of the wheels are slipping.

I created another animation for the same situation but with the body oriented controller. In this case the rotation points are all in a single location leading me to conclude that all the wheels are synchronized and little to no wheel slip is occurring.

One other interesting thing you can see in the video is that the acceleration and jerk values change very abruptly. In real life this would lead to significant loads on the robot and its drive system. In the simulation this behaviour is due to the fact that linear profile that is being used to transition from one state to another. As mentioned before the next improvement will be to replace this linear interpolation with a control approach that will provide smooth transitions for velocity and accelerations.

Edits

• July 6th 2023: Added a section discussing the use of the matplotlib animation functions.

Swerve drive - Better movement by controlling the body motions

In my first attempt to model a swerve drive I controlled the movement of the robot by directly providing commands to the individual drive modules, i.e. by controlling the steering angle and the wheel velocity of each of the four drive modules. Additionally I assumed that the transition from one state (steering angle, wheel velocity) to another state would follow a linear profile. An example of this can be seen in the graphs below. The bottom left graph shows the linear change in steering angle for the four drive modules. It should be noted that a linear profile isn't very realistic as this requires the motors to be capable of instant changes in velocity and acceleration. However making this assumption keeps the calculations simple. At a later stage I will be encoding different movement profiles amongst which a low jerk profile for smooth movements.

The benefit of controlling the robot by directly controlling the drive modules is that the control algorithm is very simple and all movement patterns are possible. For instance it is possible to make all the wheels change steering angle without moving the robot body, something which is not possible with indirect control methods.

However there are also a number of drawbacks. The main one is that for a swerve drive to be efficient the motions of the different drive modules need to be synchronized. That means that all the drive modules needs to be controlled so that none of the wheels slip along the surface. For simple linear or rotational movements of the robot body this is easy to achieve but for complicated movements this is much more difficult. An example is shown in image above which describes the behaviour of the robot as it moves linearly at a 45 degree angle and then transitions to an in-place rotation around the robot centre axis. The bottom right graph shows the instantaneous centre of rotation calculated for different combinations of two wheels. In a synchronised motion all wheels should point to the same instantaneous centre of rotation at any given moment. If the ICR's for the different wheel pairs are in different locations then one or more of the wheels is out of sync and likely slipping along the ground. The graph clearly shows that in this case the drive modules are not synchronized thus causing wheel slip.

One way to improve the synchronization between the drive modules is apply the movement control at an indirect level, for instance on the robot body, and then to work out from there what the drive modules should be doing at any that given point in time along the transition profile.

As with the previous approach I will use a linear profile to determine the transitions between different body states. Then the transition for the body velocities (x-velocity, y-velocity, and rotational velocity) can be determined from the start state to the end state. Once it is known what the body motion looks like I can calculate the states for the drive modules from the body state for all given states in the transition profile.

One issue with this approach is that every body state can be achieved by two different module states, one with the wheel moving ‘forward’ and one with the wheel moving 'backward'. Initially the simulation was hard-coded to always select the ‘forward’ moving state. However this leads to situations where the steering angle is flipped 180 degrees very rapidly. This is shown in the figure where the yellow line switches from 45 degrees (or 0.78 radians) to 225 degrees (or 3.9 radians) in two time steps, or 0.02 seconds. In real life this change in steering angle would likely not be possible because the steering angle motor would not be capable of delivering the enormous velocities and accelerations necessary to achieve this change.

To resolve this issue the simulation calculated the difference in rotation and velocity between the current point in time and the previous point in time along the transition profile, for both the forward and backward motions. Then general rule is to pick the smallest change in both rotation and velocity of the drive module. If that isn't possible then we pick the one with the smallest rotation difference for no real reason other than we have to pick something. In future work a more thorough algorithm will be implemented.

With all that code in place the simulation delivers a much smoother result for the transition from a 45 degree linear motion into a in-place rotation. The ICR path for all the wheel pairs is following a single path indicating that all the drive modules are all synchronised during the movement transition.

The body oriented control approach ensures synchronisation of the drive modules while allowing nearly all the different motions a swerve drive can make. The only motions not possible are those that involve drive module steering angle changes only.

The next improvement will be to replace the linear interpolation between the start state and the desired end state with a control approach that will provide smooth transitions for velocity and accelerations. This will make the motions of the robot less jerky and thus less likely to damage parts while also providing a smoother ride for the payload.

Edits

• July 4th 2023: Changed the term control trajectory to control profile because the term trajectory is generally reserved for path planning situations.
• July 4th 2023: Changed the term ICR trajectory to ICR path because the the ICR does not follow a trajectory, it follows a path.

Swerve drive - Verification of the kinematics solution

As I explained in an earlier post I have written some code to simulate the movement of a four wheel swerve drive.

Currently I have only implemented a simple kinematics based approach. This approach is based on the diagram displaying degrees of freedom for the swerve drive as well as the coordinate systems for the different parts. The simple kinematics approach makes a number of assumptions which greatly simplify the problem space.

• The steering axis of a drive module is vertical and passes through the centre of the wheel, i.e. no positional changes occur when the wheel steering angle changes.
• The robot is moving on a flat, horizontal surface, i.e. the contact point between the wheel and the ground is always inline with the steering axis of a drive module.
• The robot has no suspension, i.e. the body doesn't move vertically relative to the contact point between the wheel and the ground.
• There is no wheel slip.
• There is no wheel lift-off, i.e. the wheels of the robot are always in contact with the ground.
• The motors are infinitely powerful and fast, i.e. there are no limits on the motor performance.

With diagram and the given assumptions we can derive the equations for the wheel velocity and the steering angle of each drive module.

v_i = v + W x r_i

alpha_i = acos (v_i_x / |v_i|)
      = asin (v_i_y / |v_i|)

Where

• v_i - the wheel velocity of the i-th drive module, i.e. the velocity at which the drive module would move forward if there is no wheel slip. The x and y components of this vector are named as v_i_x and v_i_y.
• v - the linear velocity of the robot.
• W - the angular velocity of the robot.
• r_i - the position vector of the i-th drive module.
• alpha_i - the steering angle of the i-th drive module relative to the robot coordinate system.

Based on these equations we can determine the forward kinematics, which translates the movement of the drive modules to the movement of the robot body, and the inverse kinematics, which translates the movement of the robot body to the movement of the drive modules. When doing the calculations for a four wheel swerve drive it is important to note that the forward kinematics calculations are overdetermined, i.e. there are more control variables than there are outputs. This means that there are additional control variables that we can play with. One obvious one for a swerve drive is that we can control the orientation of the robot body independent[*] from the direction of movement of the robot. This also means that the forward kinematics calculations are based on the pseudoinverse approach which computes a best fit, a.k.a. least squares, using the drive module wheel velocities and steering angles. In order words the wheel velocities and steering angles are approximations, not exact values.

The transition between states, i.e. from one combination of x-velocity, y-velocity and rotation velocity to another combination, is done by assuming that there is linear control for the drive module variables, i.e. wheel velocity and steering angle. While linear control profiles are not the best control method it does allow later on only changing the profile code to use a more suitable one, for instance a jerk limited profile.

The code I wrote gives me a graphs like the ones presented in my previous post. However before I use this code to test new control algorithms I want to make sure my code is actually producing the correct results. The verification is done by running a bunch of simple simulations for which I am able to predict the behaviour using some simple maths.

To verify that my code is correct I a ran a number of sets of simulations. The first set is used to ensure that both the positive and the negative direction behaviour for the main axis directions. Any differences in behaviour between the positive and the negative direction point to issues in the simulation code. So the simulations that were done for this verification set are:

• Drive the robot in x-direction while facing in the x-direction, one simulation going forward from the origin and one simulation going backwards from the origin.
• Drive the robot in the y-direction while facing in the x-direction, one simulation going left from the origin and one simulation going right from the origin.
• Drive the robot in a rotation only movement, one simulation going clockwise and one simulation going counter-clockwise.

The second simulation set is designed to verify the coordinate calculations related to rotations. The simulations that were done for this verification set are:

• Rotate the robot by 90 degrees and then drive it in the robot x-direction, driving forwards and backwards.
• Rotate the robot by 90 degrees and then drive it in the robot y-direction, driving left and right.

The third set of simulations is designed to verify the behaviour during combined movements. The simulations that were done for this verification set are:

• Drive the robot on the 30 degree diagonals (30 degrees, 60 degrees, 120 degrees, 150 degrees), while facing in the x-direction, one simulation going forwards from the origin and one going backwards from the origin.
• Drive the robot in a circle around a centre point outside the robot body.

While running the verification sets a number of bugs were found and fixed. At the end of the process all the validations passed indicating that the simulation code is usable.

With all of this done it is now time to do some more complicated simulations both for robot behaviour that is specific to swerve drive systems and for different control algorithms. More on that in the next post.

–

[*] Mostly independent. In reality the motors used in the drive modules will have limits on how fast they can be driven, how much torque they can produce and how fast they can change from one state to the next. This means that there are limitations on movements of the drive modules and thus the robot body. For instance if a drive module has a maximum linear velocity of 1.0 m/s it is not possible to drive the robot diagonally any faster than this velocity, even though we can drive the robot in x-direction at 1.0 m/s and we can drive the robot in y-direction at 1.0 m/s. We just can't do both at the same time.

Edits

• July 4th 2023: Changed the term control trajectory to control profile because the term trajectory is generally reserved for path planning situations.

Swerve drive - Kinematics simulation

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.

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.

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.

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.