Robot velocities based on IMU data

Question

I'm currently stuck on obtaining a description of the robot base velocities (xdot, ydot, thetadot) based on data obtained from 4 IMU sensors located, with an offset, on 4 pivot points. See picture below:

And a closeup of 1 of the pivot wheels:

The IMU is located at the red dot and since this is a 2d problem, I'll only consider acceleration in the X,Y direction of the IMU frame and rotation around the Z axis of the IMU frame.

Now, since I'm looking to obtain a description for the velocities xdot, ydot en thetadot, this all would result in something like the following:

[dx dy dtheta]' = [3x12 matrix] * [ax_1 ay_1 gz_1 .. .. .. etc.]'

I think that if a description of how the accelerations of the individual IMU's influence the robots odometry is known, I can average the individual components to come to the 'true' odometry. So what's first (I think) is obtaining a description of either dx or dy or dtheta as a function of the angle delta and the velocities of the IMU.

If we want to know the location of the IMU in the robot's frame, we would need translation matrices which look like the following:

H_imu_p = [1 0 -d_s; 0 1 0; 0 0 1];
H_p_r = [cos(d) -sin(d) -d_px; sin(d) cos(d) -d_py; 0 0 1];

Where d_s is the distance in X between the IMU and the pivot point and d_px and d_py are the distance between Robot Origin and pivot point expressed in the Robot's frame.

Multiplying these matrices would result in a matrix that is able to give the location of the IMU in the Robot's frame. Since the IMU is located wrt the IMU frame at:

p_imu = [0 0 1]'

We can obtain the location of the IMU wrt the global frame by taking the last column of the multiplied translation matrices.

Differentiating this information twice obtains how the accelerations of the IMU are expressed in the Robot's frame.

Here is where the question part begins:

Assuming all this information above is correct, how do I go from an expression that expresses xddot_IMU yddot_IMU and thetaddot_IMU in the robot's frame to that of determining the velocities of the robot's frame?

Would integrating the measurement results (so obtain velocities of the IMU) and then having the expression of location of the IMU wrt the robot's frame differentiated once? deltadot could then be obtained from the gyroscopic data around the Z-axis. I'm having doubts about this method due to the complex kinematic relationships between the IMU and the robot. Essentially all the four pivot points could rotate without the robot ever moving (while still measuring values at [ax_1 ay_1 gz_1 .. .. .. etc.]

I'm not asking for a worked out solution but I'm quite stuck here and couldn't find the required literature to find a way out. If someone could point me towards the solution (or towards some examples in literature), that would be very much appreciated.

Answer 1

Unfortunately, the arrangement you show won't work very well. There are a whole raft of reasons, but they boil down to two classes - gimbal lock and singularities.

I'll try to illustrate the problem with an example of an entire class of motion that this setup can't handle.

Based on your sketch, I'm assuming the IMU output is acceleration, and that you are looking to define instantaneous chassis acceleration based on the instantaneous accelerations of four IMUs. In general, you can't.

Suppose all IMU x accelerations are zero and y accelerations are all equal. The obvious solution is Thetadot = 0 and the robot doing orbits at constant speed with the casters all having equal angles and rates of rotation about the chasis. However, there are other solutions. Suppose the chasis is spinning about its center. Now X,Y are zero and Thetadot is nonzero. This can have exactly the same IMU outputs. Furthermore, you can combine these motions, where a retrograde spin and orbit combine to yield the same IMU outputs.

Other problems occur when the IMU velocity is low relative to other factors such as caster angle rates. For instance, you can keep all four IMUs stationary and still make orbits of radius = caster offset without triggering any output. EDIT - this would produce a yaw rate, which I didn't know was available when I wrote the previous sentence.

As formulated, even given a complete initial condition and IMU history, you still can't generate a trajectory in general, and an error budget analysis would also show serious problems in ordinary, smooth, well behaved trajectories.

We use angular resolvers for a reason.