The Unicycle filters inertial measurements before using them in controlling the motors. The Earth pulls the robot towards the ground, and so we should expect a pull of one g in the vertical direction as the static acceleration vector. But, we fuse two different estimates of the acceleration vector for accuracy. Compute one of the estimates by normalizing the three-dimensional vector of the accelerometer. Compute another estimate by taking the estimate from the previous time step and rotating it according to the angular velocity about the principal axes of the chassis (x, y and z). In this particular estimate of the gravity acceleration (static + dynamic) we assume that the position of the robot is constant and only a reorientation about the center of mass is required to update. Multiplying the period of one run of the controller (0.007696 second) by the angular velocities that are obtained from the gyroscope component gives us the rotation angles. Then, the rotation matrix (three rows by three columns) of the reorientation is instantiated, given the angles and the gravity acceleration estimate of the previous time step. The acceleration vector of the previous time step is updated with a current estimate by a matrix-vector multiplication.
Once we have the two estimates (the one straight from the accelerometer and the one induced by the gyroscopic velocity on the old estimate,) the fusion algorithm takes a weighted sum of the two for reducing low-frequency drift and high-frequency noise. For this prototype, we mounted three Inertial Measurement Units (IMU, MPU6050) and took the average of three fusion results. Finally, the result is a 3-vector with a minimal component in the x-y plane whenever the robot is upright and in balance. The acceleration vector points out from the center of mass to the ground, in case most of the acceleration is caused by gravity, which is static. However, the magnitude and direction of the acceleration vectors (three vectors in this prototype) change as soon as the Unicycle moves, because of the dynamic component. For the task of balancing, the acceleration is assumed to be almost all static.
This change is due to the following commit: “Filtered IMU data by calculating the gravitational acceleration.” github.com/iam...
Get the user application of the source code: github.com/iam...
8 сен 2024
