PDA

View Full Version : HR-OS1 angular velocity and linear acceleration estimate



kira
02-21-2016, 09:18 PM
Hi, I want to get HR-OS1's angular velocity and linear acceleration estimate.
So , I write estimate code refer to the linked project,
https://github.com/Interbotix/ros_hros5/blob/master/hros5_robot/hros5_ros_control/src/hros5_hardware_interface.cpp.


imu_angular_velocity[0]=(-gyroRL-512)*1600*M_PI/(512*180)-imu_gyro_zero[0];
imu_angular_velocity[1]=(-gyroFB-512)*1600*M_PI/(512*180)-imu_gyro_zero[1];
imu_linear_acceleration[0]=lowPassFilter(0.25,(acclX-512)*G_ACC*4.0/512,imu_linear_acceleration[0]);
imu_linear_acceleration[1]=lowPassFilter(0.25, (acclY-512)*G_ACC*4.0/512,imu_linear_acceleration[1]);
imu_linear_acceleration[2]=lowPassFilter(0.25,(acclZ-640)*G_ACC*4.0/640,imu_linear_acceleration[2]);

The output:

RL_GYRO: 510
FB_GYRO: 512
X_ACCEL: 510
Y_ACCEL: 529
Z_ACCEL: 647
X angular velocity value: 0.000000
Y angular velocity value: 0.000000
X linear acceleration value: -0.126817
Y linear acceleration value: 1.192849
Z linear acceleration value: 0.299427
roll value: -1.304659
picth value: -0.102752
RL_GYRO: 511
FB_GYRO: 512
X_ACCEL: 510
Y_ACCEL: 528
Z_ACCEL: 642
X angular velocity value: -0.054542
Y angular velocity value: 0.000000
X linear acceleration value: -0.133396
Y linear acceleration value: 1.200907
Z linear acceleration value: 0.255198
roll value: -1.335455
picth value: -0.108229
RL_GYRO: 510
FB_GYRO: 512
X_ACCEL: 507
Y_ACCEL: 528
Z_ACCEL: 650
X angular velocity value: 0.000000
Y angular velocity value: 0.000000
X linear acceleration value: -0.195757
Y linear acceleration value: 1.206950
Z linear acceleration value: 0.344533
roll value: -1.253567
picth value: -0.154715


The robot remains stationary, the linear acceleration values should be zero.
I do not know what is going wrong.
Can anyone help me ?

r3n33
02-22-2016, 09:31 PM
Hi kira

Gravitational pull may by present on the accelerometer X and Y even when the robot is stationary unless the sensor is perfectly level (example (http://physics.rutgers.edu/~aatish/teach/srr/workshop3.pdf)). Even then different sensors will have varying amounts of noise in the signal. Your code example and output do not seem to correlate so it is hard to know exactly what is going on. Something you might try in addition to ensuring a level sensor is adjusting the low pass filter on the accelerometer to something like 0.1 to reduce the noise even further.