Install IMU-sensor on Raspberry-Based Robot.
Hi, folks! I hope you did not lost in my stories :) So I am continuing to make my Self-Driving Robot “Smarter”, the project begins here.
So last time we installed the sonar and servo as Linux Kernel Modules. Now we are working with a very important sensor for us — it is IMU-sensor.
Recently I have bought ASM330LHHX IMU-sensor, here is a datasheet for it. My goal was to gather the IMU data from it and publish in ROS topic. After that I will use this data to get odometry and transform data.
The transform and odometry data play a key role in navigation stack. It helps us to understand robot’s position on global and local map. Also you can match sensor data with its current localization to build a map. These actions are the important parts of SLAM algorithms which are usually used for the autonomy stack.
In this story we will pass through the following steps:
- Mounting IMU-sensor on board and installing it on robot’s platform.
- I²C communication with IMU sensor.
- Parameters for gyroscope and accelerometer.
- FIFO mode and script example
- Integrating driver to ROS environment and using imu_filter for decreasing error.
Mounting IMU-sensor on board and installing it on robot’s platform.
An IMU-sensor is a small chip which can get measurements of accelerometer and gyroscope. Some models also can provide temperature measurements and magnitude. On Image 1 you can see macro image of IMU-sensor.
Let’s take a look on its pins’ scheme on Image 2.
Here we need the power pin (Vdd), ground pin (GND), and SDA pin — it is pin where I²C interface is used. Here we also have another pins for another purporses which we are not using right now. For example, interruption pins (INT1, 2). You can set the interruption on some event. With usage pins MSDA and MSCL you can add some other devices on bus and form a chain.
So here is the board’s design for mounting the IMU-sensor, again thank’s to my close friend Cluster for assistance in board’s developing.
VCC i/o is for power, SDA is for I²C interface, and also the other i/o for other pins.
The ready board with IMU-sensor are installed on robot’s platform. The board is on the left side from sonar. It is not very good location, it would be better to install it under the sonar, but we do not have a suitable place there, so will sort it out later.
Also you can notice, that we add splitters, as we are lack of pins for power. And the last thing is to connect SDA pin on board with SDA pin on Raspberry. And after that we add the devices on a bus as /dev/i2c-n (1,2,3) and can communicate with usage i/o operations on file.
I²C communication with IMU sensor.
IMU-sensor supports i2c interface. This interface allows us to install several devices on a bus and request them directly using device’s address.
I²C has own physical and channel protocols, but it is covered under Linux kernel modules and i2c lib in Linux provide api for interacting with smbus, so we can just skip this part.
All we should know — the number i²c device (1), the device’s address and addresses of needed registers.
The IMU sensor allows us to interact in different ways. First, we can simply read the current data from sensor. Second, we can set saving data in FIFO data structure on registers of the device. And then we are reading the latest n values of gyroscope and accelerometer. The second way is more preferred. The first reason is — in this case you do not need to request the device frequently and your CPU is not so busy in a ROS while loop. The second reason — even if you request the device not so often, you do not loose the data and notice the robot’s movement.
I have written the app for the IMU interaction. Here we have the init() method:
int ASM330LHHX_driver::init(){
file = open(device.c_str(), O_RDWR);
if (file < 0) {
std::cerr << "error opening device" << std::endl;
return -1;
}
if (ioctl(file, I2C_SLAVE, address) < 0){
std::cerr << "i2c mode open failed" << std::endl;
return -1;
}
__s32 res = i2c_smbus_write_byte_data(file, REG_CTRL1_XL, acc_param);
if (res < 0) {
std::cerr << "i2c write params on accelerometer failed" << std::endl;
return -1;
}
res = i2c_smbus_write_byte_data(file, REG_CTRL2_G, gyr_param);
if (res < 0) {
std::cerr << "i2c write params on gyroscope failed" << std::endl;
return -1;
}
return 0;
}At the beginning we request for file opening for read and write. Then ioctl sets the file in a control device mode. This system manipulates the underlying device parameters of special files. After that we are using the i2c lib which provide the api for interacting with the smbus. Here you can find the detailed documentation.
In this code fragment we are setting parameters for the gyroscope and accelerometer. REG_CTRL2_G and REG_CTRL1_XL are registers’ addresses which set parameters on a device. About choosing parameters we are going to talk later in this story.
And here is how we read the data from IMU sensor. We are creating the structure for IMU data. The read method should return 6 values of int16. First three values are x, y, z gyroscope’s values. And second three are accelerometer’s values. They return changes in angular velocities and linear accelerations.
typedef struct __attribute__ ((packed)) {
int16_t raw_angular_velocity_x;
int16_t raw_angular_velocity_y;
int16_t raw_angular_velocity_z;
int16_t raw_linear_acceleration_x;
int16_t raw_linear_acceleration_y;
int16_t raw_linear_acceleration_z;
} IMURecord;So we have three values from gyroscope measured in degrees per second (further dps), angular velocity changes. And 3 values for accelerometer (linear acceleration) measured in g.
bool ASM330LHHX_driver::read(IMURecord& record) {
__s32 res = i2c_smbus_read_i2c_block_data(file, REG_OUTX_L_G, sizeof(record), (uint8_t*)&record);
if (res < 0) {
std::cerr << "could not read data" << std::endl;
return false;
}
std::cout << "read bytes: " << res << std::endl;
return true;
}Here is we just simply read the data to structure from output registers. As registers follow one by one, for reading you need just the address of first register.
Parameters for gyroscope and accelerometer.
Beside different modes, we also can set different parameters on the device. On page 8 of the datasheet you can find register’s map. Now we need registers for accelerometer’s parameters (CTRL1_XL) and gyroscope’s paramateres (CTRL2_G). You can set the a frequency parameter (from 1.6 to 6667 Hz), a power mode (low and high), scaling (from 4G to 16G for accelerometer and from 250 to 2000 dps for gyroscope). Scaling set the length of values’s range — for example, 2G means values from -2G to 2G.
Your parameters set should be defined as a number where every bit set is responsible for some parameters. For example, let see the screenshot from the datasheet with gyroscope parameters.
First 4 bits are responsible for power and frequency value, then two bits follow which are responsible for dps scale.
Just not to find it every time, I set values for 208 Hz and low power but with a scales selection.
//params full scale selection for accelerometer
//I chosed 208 Hz, other parameteres is changable
#define range_2G 0x50
#define range_4G 0x58
#define range_8G 0x5C
#define range_16G 0x54
//params full scale selection for gyroscope
//I chosed 208 Hz, other paramteres is changable
#define dps_250 0x50
#define dps_500 0x54
#define dps_1000 0x58
#define dps_2000 0x5CFIFO mode and script example.
The FIFO interaction a little more complicated. There are examples from python script below. The SMBus python lib is used in this section. Here the algorithm of an interaction is following:
- During init you should set parameters, reset the memory content, set parameters for FIFO (the queue size is 12);
bus = SMBus(BUS_ID)
bus.write_byte_data(DEVICE_ID, REG_CTRL3_C, 1) # reset
bus.write_byte_data(DEVICE_ID, REG_CTRL1_XL, 0x48) # acc 104Hz +-4G
bus.write_byte_data(DEVICE_ID, REG_CTRL2_G, 0x48) # gyr 104Hz +-1000dps
bus.write_byte_data(DEVICE_ID, REG_FIFO_CTRL3, 0b00010001) # acc 12Hz, gyr 12Hz
bus.write_byte_data(DEVICE_ID, REG_FIFO_CTRL4, 0b00000000) # stop FIFO
bus.write_byte_data(DEVICE_ID, REG_FIFO_CTRL4, 0b00000001) # stops collecting data when FIFO is full- During reading you should first request FIFO status, get the unread number then read the data from FIFO in a while loop;
while True:
try:
fifo_status = bytes(bus.read_i2c_block_data(DEVICE_ID, REG_FIFO_STATUS1, 2))
s, = unpack("<h", fifo_status)
unread = s & 0b1111111111
if unread == 0: continue
#print(f"unread={unread}")
# continue
except OSError:
continue
while unread > 0:
data = bytes(bus.read_i2c_block_data(DEVICE_ID, REG_FIFO_DATA_OUT_TAG, 7))
v = unpack("<chhh", data)
tag = v[0][0]
cnt = (tag >> 1) & 3
sensor = tag >> 3
print(f"CNT={cnt} SENSOR={sensor:02X}")
if sensor == 1:
gx, gy, gz = v[1] * GYR_RES / 32767, v[2] * GYR_RES / 32767, v[3] * GYR_RES / 32767
print(f" GX={gx:+07.2f} ", end='')
print(f" GY={gy:+07.2f} ", end='')
print(f" GZ={gz:+07.2f} ")
if sensor == 2:
ax, ay, az = v[1] * ACC_RES / 32767, v[2] * ACC_RES / 32767, v[3] * ACC_RES / 32767
print(f" AX={ax:+05.2f} ", end='')
print(f" AY={ay:+05.2f} ", end='')
print(f" AZ={az:+05.2f} ")
unread = unread - 1
continueIntegrating the IMU driver to the ROS environment and using imu_filter for decreasing error.
As we are going to build the autonomy system based on ROS, we should integrate the driver to ROS environment.
We are going to write a simple ROS publisher. If you are not aware what the publisher and the subscriber are, please visit this tutorial for details.
There is the special message type for the IMU-sensor (sensor_msgs/Imu.msg):
# This is a message to hold data from an IMU (Inertial Measurement Unit)
#
# Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec
#
# If the covariance of the measurement is known, it should be filled in (if all you know is the
# variance of each measurement, e.g. from the datasheet, just put those along the diagonal)
# A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the
# data a covariance will have to be assumed or gotten from some other source
#
# If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation
# estimate), please set element 0 of the associated covariance matrix to -1
# If you are interpreting this message, please check for a value of -1 in the first element of each
# covariance matrix, and disregard the associated estimate.
Header header
geometry_msgs/Quaternion orientation
float64[9] orientation_covariance # Row major about x, y, z axes
geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance # Row major about x, y, z axes
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance # Row major x, y zSo all we need to do — translate values from scale values and then transform it from g to m/s² and from dps to rps (radians per second). Also we need to get the double value from int16. We know, that this is a scaling value. So we need to multiply it by the scaling value, than divide into the max value. Do not forget about radians degrees ratio— pi/180 and m/s² g ratio — 9.80665.
void ROS_Node::run() {
IMURecord record{0, 0, 0, 0, 0, 0};
double gyr_, acc_;
driver.get_currents_params(gyr_, acc_);
while(ros::ok()) {
ros::Time measurement_time = ros::Time::now();
sensor_msgs::Imu imu;
imu.header.stamp = measurement_time;
imu.header.frame_id = "raw_imu_data";
if (driver.read(record)) {
//std::cout << "send data to ROS" << std::endl;
//convert from g to m/s^2 1 g-unit = 9.80665
imu.linear_acceleration.x = (double)(record.raw_linear_acceleration_x) * (acc_ / MAX_VAL) * 9.80665;
imu.linear_acceleration.y = (double)(record.raw_linear_acceleration_y) * (acc_ / MAX_VAL) * 9.80665;
imu.linear_acceleration.z = (double)(record.raw_linear_acceleration_z) * (acc_ / MAX_VAL) * 9.80665;
//convert to radians
imu.angular_velocity.x = (double)(record.raw_angular_velocity_x) * (gyr_ / MAX_VAL) * (M_PI/180.0);
imu.angular_velocity.y = (double)(record.raw_angular_velocity_y) * (gyr_ / MAX_VAL) * (M_PI/180.0);
imu.angular_velocity.z = (double)(record.raw_angular_velocity_z) * (gyr_ / MAX_VAL) * (M_PI/180.0);
imu_pub.publish(imu);
}
else {
ROS_ERROR("Could not read data from sensor");
}
}
}Then we should create the ROS-node and also create a publisher for IMU messages. In the while loop we are receiving the current values and then transform them like showed above and publish to /imu/raw_data.
Also I added optional parameters in launch file for ROS. Here you can add path, address and params.
<launch>
<node name="imu_driver" pkg="imu_driver" type="imu_driver" output="screen">
<param name="dev_path" value="/dev/i2c-1" />
<param name="address" value="107"/>
<param name="gyr_param" value="88"/>
<param name="gyr_param" value="88"/>
</node>
</launch>Here is the output of rostopic echo:
I also decided to use ready ROS package for IMU data filtering from IMU tools. This package filters IMU data considering accelerometer gain, angular gain and you can also define different biases. Also you can add adaptive gain mechanism. At the end you are receiving renewed linear acceleration and quaternions values with considering gains and bias.
That’s all for today. In a next chapter we are going to set one more IMU sensor from the other side and get the odometry and transforms from two IMU sensors. One IMU can be noisy, so we cannot be sure about accuracy.
And after that we can build the first primitive localization by the IMU and the sonar.
See you next chapter!:)