Last time, I described my approach for estimating the terrain under the robot based on the inertial measurement unit and proprioceptive foot feedback. Now, I’ll cover how that is used to balance.

“R” Frame

First, let me explain the “R” or “robot” frame and how it is used. The frames I’ve discussed in this series so far are the “B” frame, which is rigidly attached to the center of the robot body, the “M” frame, which is located at the center of mass and level with the ground, and the “T” frame, which is under the robot and level with the current terrain.

Last time I discussed the challenges when operating the mjbots quad A1 on sloped surfaces. While there are a number of possible means of tackling this, the approach I’ve gone with for now is to estimate the slope of the terrain under the robot, and use that to determine how to position the center of mass. Here’ll I’ll cover the estimation part of this solution.

On paper, the quad A1 has plenty of information to estimate the terrain under its feet. Between the IMU with attitude estimator, the proprioceptive feedback from the joints, and the ability to move the feet around, it would be obvious to a human whether the ground under them was sloped or level. The challenge here is to devise an algorithm to do so, despite the noise in the IMU, the fact that the feet are not always on the ground, and that as the robot moves, the terrain under it changes.

Last time I covered the new software library that I wrote to help use all the features of the pi3hat, in an efficient manner. This time, I’ll cover how I measured the performance of the result, and talk about how it can be integrated into a robotic control system.

Test Setup

To check out the timing, I wired up a pi3hat into the quad A1 and used the oscilloscope to probe one of the SPI clocks and CAN bus 1 and 3.

The pi3hat r4.2, now in the mjbots store, has only minor hardware changes from the r4 and r4.1 versions. What has changed in a bigger way is the firmware, and the software that is available to interface with it. The interface software for the previous versions was tightly coupled to the quad A1s overall codebase, that made it basically impossible to use with without significant rework. So, that rework is what I’ve done with the new libpi3hat library:

I’ve now got the last custom board from the quad A1 up in the mjbots store for sale, the mjbots pi3 hat for $129.

This board breaks out 4x 5Mbps CAN-FD ports, 1 low speed CAN port, a 1kHz IMU and a port for a nrf24l01. Despite its name, it works just fine with the Rasbperry Pi 4 in addition to the 3b+ I have tested with mostly to date. I also have a new user-space library for interfacing with it that I will document in some upcoming posts. That library makes it pretty easy to use in a variety of applications.

This post will be short, because it is just re-implementing the functionality I had in my turrets version 1 and 2, but this time using the raspberry pi as the master controller and two moteus controllers on each gimbal axis.

I have the raspberry pi running the primary control loop at 400Hz.  At each time step it reads the IMU from the pi3 hat, and reads the current state of each servo (although it doesn’t actually use the servo state at the moment).  It then runs a simple PID control loop on each axis, aiming to achieve a desired position and rate, which results in a torque command that is sent to each servo.  Here’s the video proof!

Another of the tasks I’ve set for myself with regards to future Mech Warfare competitions is redesigning the turret.  The previous turret I built had some novel technical features, such as active inertial gimbal stabilization and automatic optical target tracking, however it had some problems too.  The biggest one for my purposes now, was that it still used the old RS485 based protocol and not the new CAN-FD based one.  Second, the turret had some dynamic stability and rigidity issues.  The magazine consisted of an aluminum tube sticking out of the top which made the entire thing very top heavy.  The 3d printed fork is the same I one I had made at Shapeways 5 years ago.  It is amazingly flexible in the lateral direction, which results in a lot of undesired oscillation if the base platform isn’t perfectly stable.  I’ve learned a lot about 3d printing and mechanical design in the meantime (but of course still have a seemingly infinite amount more to learn!) and think I can do better.  Finally, cable management between the top and bottom was always challenging.  You want to have a large range of motion, but keeping power and data flowing between the two rotating sections was never easy.

Now that the IMU is functioning, my next step is to use that to produce an attitude estimate.  Here, I dusted off my [unscented Kalman filter](http://unscented Kalman filter) based estimator from long ago, and adapted it slightly to run on an STM32.  As before, I used a UKF instead of the more traditional EKF not because of its superior filtering performance, but because of the flexibility it allows with the process and measurement functions.  Unlike the EKF, the UKF is purely numerical, so no derivation of Jacobians is necessary.  It turns out that even an STM32 has plenty of processing power to do this for things like a 7 state attitude filter.

The next peripheral to get working on the quad’s raspberry pi interface board is the IMU. When operating, the IMU will primarily be used to determine attitude and angular pitch and roll rates.  Secondarily, it will determine yaw rate, although there is no provision within the IMU to determine absolute yaw.

To accomplish this, the board has a BMI088 6 axis accelerometer and gyroscope attached via SPI to the auxiliary STM32G4 along with discrete connections for interrupts.  This chip has 16 bit resolution for both sensors, decent claimed noise characteristics, and supposedly the ability to better reject high frequency vibrations as seen in robotic applications.  I am currently running the gyroscope at 1kHz, and the accelerometer at 800Hz.  The IMU is driven off the gyroscope, with the accelerometer sampled whenever the gyroscope has new data available.