# How to control a 4 wheel mechanum wheel robot with PID?

I have a robot with 4 mechanum wheels. I implemented a pid controller on each wheel and as a result it doesn't drive straight. When I send a command to simply move in the x direction forward, it ends up doing an arc.

Obviously my wheels/motors aren't identical and I can't have the same PID tuning parameters for all of them. I tried to tune all 4 controllers manually and gave up, because I wasn't able to get a consistent result.

So now I am trying a different approach where I want to tune the linear and angular velocities individually. I also split up the linear velocity into x and y. The parts in green are the ones that I'm currently working on. Does this approach make sense? One thing that doesn't seem right to me is that in the inverse kinematic equation I convert the set point to a PWM value that I then write to the robot. Should there be an additional PID controller that is only working on setting a predetermined RPM to the motor?

Also is there a way to avoid including a low pass filter? I had to implement one because the speed is calculated from quadrature encoders and it was too noisy.

If anyone has any thoughts on this mechanum wheel pid control, any help would be greatly appreciated. Thank you.

Let's consider first the task you are wanting to accomplish: following some sort of Cartesian space trajectory. So, you are wanting to make sure your robot is only moving in the x-direction. However, you have no measurement of where you are along the x-direction. To remedy this, you've chosen to use the motor encodings - I am assuming to do basic distance calculations determining how far you have moved. Now, I would like to stress this very, very much: motor encodings of wheeled robots are not suitable for forward kinematics. In the same vein: motor velocities of wheeled robots are not suitable for forward kinematics. The biggest problems with using these values is that the wheels can, and are designed to, slip. This will cause compounding errors over time resulting in unusable information in the long run (really even after a few seconds of motion). So, instead of using the motor encodings to determine your location in the world, use a sensor designed to do just that - like an IMU or a camera.

Now, let's say you have an accurate estimation of your position in the world frame. You might then want to determine the error between your current trajectory and your desired trajectory. Let $$x_{t-1}$$ be your previously computed x location and $$x_{t}$$ be your current world location (same for the y location and angle $$\theta$$). To find the velocity error (without using a desired position), we get an error vector that looks like:

$$\mathbf{e} = \left[ \begin{array}{c} v_{x} - \frac{x_{t}-x_{t-1}}{\Delta t} \\ v_{y} - \frac{y_{t}-y_{t-1}}{\Delta t} \\ v_{\theta} -\frac{\theta_{t}-\theta_{t-1}}{\Delta t} \end{array} \right]$$

where $$v_{x}$$ is your desired x velocity, and likewise for $$v_{y}$$ and $$v_{\theta}$$. Since you are using the inverse kinematics, I am assuming you have a Jacobian matrix $$\mathbf{J}$$ that maps wheel velocities to current frame velocities (that is how I've seen the analysis done at least). Take the pseudo-inverse of this, giving you $$\mathbf{J}^{+}$$, which maps current frame velocities to wheel velocities. Solve for the wheel velocities that will correct your world frame velocity error:

$$\mathbf{\omega} = \mathbf{J}^{+} \left[ \begin{array}{ccc} \cos(\theta) & -\sin(\theta) & 0 \\ \sin(\theta) & \cos(\theta) & 0 \\ 0 & 0 & 1 \end{array} \right] \mathbf{e}$$

where $$\omega$$ is the computed velocity to correct this error, and the listed rotation matrix moves the error from the world frame to the current frame. You could of course try to come up with a PID controller for the motors themselves to achieve $$\omega$$, however that still doesn't solve the actual velocity error issue, as there is almost no chance your robot will match the modeled world frame velocity just by using well-controlled motors. So, my suggestion would be to use a PID controller to determine wheel velocity commands based on the velocity of the mobile robot in the world frame.

First, I would use a variable $$\Omega$$ as the sum of the computed $$\omega$$ values at each time step, i.e.

$$\Omega_{T} = \sum^{T}_{t=1} \omega_{t}$$

where $$T$$ is the current time and $$\omega_{t}$$ is the computed $$\omega$$ at each time $$t$$. Because you are looking to reach a constant velocity (this could be extended to non-constant velocities as well), your derivative error term would look something like:

$$\Delta \omega_{t} = \boldsymbol{0} - \frac{\omega_{t} - \omega_{t-1}}{\Delta t} = - \frac{\omega_{t} - \omega_{t-1}}{\Delta t}$$

where $$\omega_{t}$$ is your currently computed wheel velocities and $$\omega_{t-1}$$ is your previously computed wheel velocities. Combining these terms together, your control law could be something like:

$$\mathbf{u}_{t} = k_{p} \omega_{t} + k_{i} \Omega_{t} + k_{d} \Delta \omega_{t}$$

Noting that $$\Omega$$, $$\omega$$, and $$\Delta \omega$$ are vectors, and $$k_{p}$$, $$k_{i}$$, and $$k_{d}$$ are the proportional, integral, and derivative gains of the PID controller. Because PID controllers are designed to work with unknown dynamics (such as how PWM inputs correspond to motor velocities), this should allow you to use PWM inputs assuming the gains are properly tuned.

• I don't integrate the velocity because I can just get the ticks from the encoders to get position. The problem is that I'm trying to control speed. I have a raspberry pi and an Arduino and the green parts are on the Arduino. I'm trying to take advantage of the arduinos speed for real time pid control. Position is easy to do, but I want to control the velocity. Mar 1 at 13:44
• I see, I'll adjust my answer accordingly! However, this doesn't really fix the fundamental issues with your implementation, but I'll elaborate more in my answer. Mar 1 at 15:57

You can examine Linorobot2 and see what he uses, as he provides code for 4x4 Mechanum drive with PID controller.