Project - Inertial Sensing in Snake Robots

Tuesday, May 11, 2010

Background

Sensors / Test Rig

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

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.

Data - Gyros

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

Conclusion and Future Work

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.

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.

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

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:

swing_load

swing_analyze

swing_kalman

Code