Project - Inertial Sensing in Snake Robots
The goal of this project was to use the internal feedback of the snake robots in Howie Choset’s Biorobotics Lab to start addressing the problem of estimating the robot’s motion and environment.  In order to simplify the problem and have a reliable ground truth for the data from the snake, a test rig was created where the snake was allowed to swing freely about a single axis with an encoder for measuring position.  Using an extended kalman filter I was able to combine redundant gyro and accelerometer readings to successfully estimate the snake robots’s pose, angular velocity, and center of rotation.
Tuesday, May 11, 2010
The snake robots referred to in this project are what the lab call ModSnakes. Their design consists of 16 identical modules each with a single degree of actuation. The snake’s design was originally built around hobby servos, but has now progressed to fully custom modules with sealed housings (above). These modules are chained together such that they have redundant motion in a 3-D environment. Each module (below left) contains a 3-axis accelerometer and a 2-axis gyro to sense motion and an encoder to sense the relative angle between adjacent modules. There are also a number of other internal state sensors including motor current draw and internal temperature.
The first step to using the accelerometer and gyro data is to determine a consistent coordinate frame for each module.  Up until now accelerometer measurements had just been reported in a frame that was convenient at the time.   With gyros being added, it made sense to report the feedback in a standard right-hand coordinate frame (above right).  The x-axis points down the length of the snake, the y-axis points to the left, and the z-axis points up.  The gyros sense rotation about the x- and y-axes.  Since the modules are rotated 90º at each joint, I chose to define the snake’s swing as rotating about the z-axis and transform all the feedback into that coordinate frame.
The inertial sensors in the snake modules are low cost MEMs devices.  The accelerometer is a LIS3LV02DL from STMicroelectronics.  It measures 3-axis accelerations to +/- 2g and the snake reads it on as a 10-bit analog to digital channel.  The gyro is the IDG500 from Invensense, Inc.  It measures angular velocities to +/-500º / sec and is also read on a 10-bit analog to digital channel.  During the tests, the gyros never reached their measurement limit, but the accelerometers aligned with the x-axis of the snake exceeded 2g towards the head due to the centripetal force of swinging.
Sensors / Test Rig
The test rig was constructed of wood, held in place by cork pads and 70 lbs of weight.  The snake was clamped in the shaft collar on the left of the image above, and the swinging motion was supported by a pair of bearings and a .50” steel shaft.  At the end of the shaft a quadrature encoder measured the swing position with a resolution of 2000 points per revolution.  The encoder signals were read by an Arduino Duemillanove and sent to the same computer that was logging data from the snake.  This allowed the encoder from the test rig to be timestamped with the same (or close enough to the same) clock time as the snake log files for easy comparisons later on.
During the tests the snake was lifted to approximately level, and then released into a free swing.  Wind resistance and friction in the bearings caused the oscillation of the swing to damp out relatively quickly, but I was still able to log data for approximately 60 seconds of swinging on three separate trials.
The snake was powered on and actively commanded to be in a straight shape during the swings.  This means that each module was actively trying to maintain a straight angle relative to the the adjacent module and would apply torques to the joints according to the PD control on each module.  Without this, the modules are back-drivable to the point where the snake would bend during the swing and the motion would become somewhat chaotic.  Due to slight miscalibrations of the module center positions and backlash in each module’s gear train, the snake still underwent some bending at the peak of the right-hand swing and there was some oscillation due the controller in the outermost modules.
Data - Test Rig
The encoder data was recorded and low-passed with a forward-backwards rolling window of 5 samples to make the differentiation to get velocity less noisy with no lag due to filtering.  The noise is mostly due to the slightly irregular time steps at which angles were recorded over the serial communication to the computer.
Plots of the snake’s angular position during the swing.  Clicking the pictures (and all otheris in this report) brings up a full-size postscript version of the plot.
Due to the way the 2-axis gyros are placed in the snake, and the fact that the test rig was aligned with the principle axes of the snake, only every other of the 16 modules had a gyro that measured angular velocity of the swinging.
Once the gyros were corrected for zero bias, they were compared to each other to get a rough estimate of how repeatable they are and also see of the slight bending of the snake affected their readings.  Intuitively, the slight bending of the snake during the swing would cause the outmost modules to over-rotate during the swing so they should measure a higher peak velocity than modules in the tail that are closer to the center of rotation.  This is confirmed by gyro data as shown in the plots below.
Plots of the snake’s angular position during the swing.  Clicking the pictures (and all others in this report) brings up a full-size postscript version of the plot.
Data - Gyros
The accelerometers measure the acceleration along the 3 principle axes of each module, including the constant acceleration due to gravity.  In the case of acceleration tangential to the rotation of the swing, gravity will be dominant force that is measured in the modules throughout the snake.  In the case of acceleration normal to the swing rotation centripetal acceleration will dominate the measurements at the head, and decrease towards the center of rotation (as seen below).
Data - Accelerometers
To combine the accelerometer and gyro data and form an accurate estimate of pose, I used an extended Kalman filter (EKF) built primarily on the MATLAB from the previous homework assignment.
The state vector consisted angular position, velocity, and acceleration.  The measurement vector included the 8 gyros that were aligned with the rotation of the swing, along with all 16 accelerometers in the x- and y-axes of the snake.  Initially I set up the filter assuming the snake was perfectly straight throughout its rotation, ignoring feedback from the module encoders.  This incorporated the angular velocity overshoot into the prediction model for the measurement vector.  However, using the joint angle data (below) from the snake to rotate the swinging accelerations into the appropriate frame for each module, and differentiating this data to account for the over-rotation of the head modules improved the accuracy.  Differentiating yet again to account for the angular acceleration of the joints proved difficult due to the noisy signal, and these effects are still being neglected.  Part of the difficulty is that the change in joint angles are so small that the resolution limit of the encoder is apparent ( approx. 0.35º / tic ).
Results - Kalman Filtering
State estimation using only the inertial sensors from the snake works well for the constrained 2D dynamics that are well-described by the motion of the pendulum.  The challenges that remain are implementing a similar framework for 3D dynamics and testing the snake in this 2D environment during movements that are dominated by its own actuated motion.
Conclusion and Future Work
In the plots above the redder lines are gyro data towards the tail of the snake and greener lines are data closer to the head.  This shows the greater angular velocities measured by the head modules due to the bending of the snake.  The step-like artifacts are from delays/errors in the serial communications to certain modules.
Plotting the average measurements of the gyros winds up incorporating this overshoot, but overall the data tracks well with the measured velocity from the test rig encoder.
Integrating the gyro data to get position reasonably accurate, but the integration error starts to accumulate past about 30 seconds.  By 60 seconds of swinging the integration has accumulated 20º of error.
The top plot of x-acceleration matches the intuition of the effect of centripetal acceleration due to the swing.  Magnitudes increase at the head, clipping at the sensor’s max value of 2g.  The frequency of oscillation is twice the frequency of the swing since the acceleration follows the same trend in both directions of the swing.
The y-acceleration needs a little more explanation, since the oscillation actually flips at the head modules compared to the modules further back.  This was not intuitive to me at first and I thought might have an artifact of the snake bending during the swings.  However, since the acceleration tangential to the snake is:
    y_accel = ( gravity ) * sin( Θ ) + ( radius of rotation ) * dω / dt
The effect of the angular acceleration and radius of rotation again is such that at the tail of the snake has the accelerations dominated by gravity.  However, as you increase distance from the center of rotation the angular acceleration counters that of gravity, up until you reach the radius of gyration.  This is the point where the y-acceleration due to gravity and angular acceleration cancel each other and there is no net measured acceleration through the entire pendulum swing.  Past this point the magnitudes of the y-accelerations are reversed, since the head modules are being ‘tugged’ down by the rest of the snake.
These trends are illustrated in the magnified graphs below.  Also of note is that the readings are noisier than the gyros, especially on the y-accelerations.  This is possibly due in part to jitter caused by the motor controller trying to hold the snake straight during the swings.
Screening for maxed out accelerometer readings and accounting for the slight change in angular position and velocity down the length of the snake improved the model.  The plots below show the state vector for the EKF compared to the measured state according to the test rig.  Note that the angular acceleration from the test rig is noisy due to having to differentiate the position twice.
One more problem in implementing the filter was the need to account for the accelerometers that reached their max measurement value during the swings.  Using the data as is caused initial problems for state estimation, so initially I only used data from the accelerometers towards the tail of the snake.
Later, a fairly simple workaround was to check the value of each accelerometer just before the measurement update step of the EKF.  If the magnitude of the acceleration was more than 1.9 g, then I manually set the measurement process noise for that particular time step to 1000 times its normal value.  This effectively caused the filter to trust the predicted measurement more than the actual measurement in these cases, and can be seen working in the plots below.  They show the acceleration for the head module, which experienced the strongest centripetal accelerations during swinging.
Finally, the position for the center of rotation along the snake was added to the state vector to see if the EKF could converge to an accurate estimate.  The filter was initialized with an arbitrary guess of being at the middle of the snake.  The estimate becomes accurate as soon as the swing begins, and begins to drift as the magnitudes of the swing decrease.
The plots above are for hand-picked parameters for the Q and R noise parameters of the EKF.  The values were selected to be a diagonal matrix with unique values for the 4 state variables, and a unique gain for the 8 gyros, 16 x-accceleration, and 16 y-acceleration measurements.
    % noise parameters (hand-selected)
    params = [ 0.0001       % position
               0.001        % velocity    
               0.001        % acceleration
               0.01         % center of rotation
               0.001        % z-gyro
               0.1          % x-accel
               0.1 ];       % y-accel
While the filter proved to be stable and reasonably accurate, there was a noticeable lag on the predicted position, possibly due to the lag present in the measured accelerometer values.  As a first attempt at optimizing the filter, MATLAB’s Nelder-Mead optimization was used with the hand-picked parameters as a seed.
The optimization criteria was a weighted combination of the position, velocity, and center of rotation error of the EKF predicted state against measured state from the test rig.  The results below show the predicted state for the optimized noise parameters:
        % noise parameters (fminsearch)
    params = [ 0.00000034       % position
               0.00026322       % velocity
               0.00027174       % acceleration
               0.00015465       % center of rotation
               0.00009249       % y-gyro
               0.27828062       % x-accel
               0.53863423 ];    % y-accel  
A direct comparison of the estimated angular position with the previous parameters to the optimized parameters shows the improvement.  However, this came at the expense of a noisier prediction of center of rotation.
The link below contains a zip file of everything needed to load a log, plot a bunch of stuff, and run the EKF.  To use it, unzip the file, add the folder and all subfolders to the path in MATLAB and run the following scripts in order: