IMU的基本原理
(如有错误,望批评指正)
惯性测量单元(IMU)通常包含加速度计和陀螺仪组成的组合单元,陀螺仪就是内部有一个陀螺,它的轴由于陀螺效应始终与初始方向平行,这样就可以通过与初始方向的偏差计算出旋转方向和角度,这些都是角度变化值。而加速度计能敏感地感受到三个方向上加速度变化,如果在静止状态上,应该有一个垂直与xy轴向下,大小为9.8的加速度(通常都不是这个值),我们可以在静止下通过在每个方向上的加速度值得出初始pitch和roll角。
上图中,红色为x轴,绿色是y轴,蓝色是z轴; 一般相机正视方向为z轴。相当于左上角为原点的像素矩阵。
-
相机在围绕x轴旋转产生pitch俯仰角 θ \theta θ
-
再旋转z轴产生roll翻滚角 α \alpha α
根据几何关系:
θ
=
a
t
a
n
2
(
z
x
2
+
y
2
)
\theta= atan2 (\frac{z}{\sqrt{x^2+y^2}})
θ=atan2(x2+y2z)
α
=
a
t
a
n
2
(
x
y
)
\alpha=atan2(\frac{x}{y})
α=atan2(yx)
简单的位姿解算代码
#include <iostream>
#include <sensor_msgs/Imu.h>
#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <boost/thread/thread.hpp>
#include <math.h>
using namespace message_filters;
using namespace std;
typedef struct imu6d{ //定义一个储存IMU数据的结构体
double accel_x;
double accel_y;
double accel_z;
double gyro_x;
double gyro_y;
double gyro_z;
struct imu6d operator +(const struct imu6d &p) //重载+运算符
{
struct imu6d temp;
temp.accel_x = p.accel_x + accel_x;
temp.accel_y = p.accel_y + accel_y;
temp.accel_z = p.accel_z + accel_z;
temp.gyro_x = p.gyro_x + gyro_x;
temp.gyro_y = p.gyro_y + gyro_y;
temp.gyro_z = p.gyro_z + gyro_z;
return temp;
}
friend ostream & operator << (ostream & os, const imu6d &p) //重载<<运算符方便查看数据
{
os << "accel_x: "<< p.accel_x << "\t accel_y: "<< p.accel_y << "\t accel_z: "<< p.accel_z<<"\n"
<< "gyro_x: "<< p.gyro_x<< "\t gyro_y: "<< p.gyro_y<< "\t gyro_z: "<< p.gyro_z;
return os;
}
} imu6d;
typedef struct imu3d{
double x;
double y;
double z;
struct imu3d operator +(const struct imu3d &p)
{
struct imu3d temp;
temp.x = p.x + x;
temp.y = p.y + y;
temp.z = p.z + z;
return temp;
}
struct imu3d operator *(const double p)
{
struct imu3d temp;
temp.x = x * p;
temp.y = y * p;
temp.z = z * p;
return temp;
}
friend ostream & operator << (ostream & os, const imu3d &p) //重载<<运算符方便查看数据
{
os << "x: "<< p.x << "\t y: "<< p.y << "\t z: "<< p.z;
return os;
}
} imu3d;
class imuTester{
public:
ros::NodeHandle nh_;
typedef sync_policies::ApproximateTime<sensor_msgs::Imu, sensor_msgs::Imu> syncPolicy; //ExactTime使用时间戳精确同步;ApproximateTime近似同步
imuTester() //构造函数
{
time_s=0;
flag = 1;
total_angular = {0,0,0};
Angle = {0,0,0};
total_yaw = 0;
//订阅加速度计、角速度计信息。下面的new是写类时需要的用法,不用类可参考官方写法
accel_sub = new message_filters::Subscriber<sensor_msgs::Imu> (nh_,"/camera/accel/sample",2000,ros::TransportHints().tcpNoDelay());
gyro_sub = new message_filters::Subscriber<sensor_msgs::Imu> (nh_,"/camera/gyro/sample",2000,ros::TransportHints().tcpNoDelay());
//同步消息并进入回调,这里因为有boost::bind的多线程,导致不能delete sync
sync = new Synchronizer<syncPolicy> (syncPolicy(10), *accel_sub, *gyro_sub);
sync->registerCallback(boost::bind(&imuTester::imu_callback,this, _1, _2));
IMU_pub = nh_.advertise<sensor_msgs::Imu>("/imu_data_pub", 1);
}
~imuTester()//析构函数
{
cout<<"delete memory..."<<endl;
delete accel_sub;
delete gyro_sub;
cout<<"delete memory finished!"<<endl;
}
private:
double time_s;
double total_yaw; //记录偏航角变化
bool flag; // 判断是否第一次进入回调
message_filters::Subscriber<sensor_msgs::Imu>* accel_sub;
message_filters::Subscriber<sensor_msgs::Imu>* gyro_sub;
Synchronizer<syncPolicy>* sync;
ros::Publisher IMU_pub;
imu3d total_angular;
imu3d Angle;
imu3d calculate_accel(const imu6d data) //计算加速度计算出的翻滚角roll和俯仰角pitch,由于重力方向垂直向下,所以初始的偏航角无法计算也不重要
{
imu3d angular;
angular.x = atan2(data.accel_x, data.accel_y);//翻滚角
angular.y = atan2(data.accel_z, sqrt(data.accel_x*data.accel_x + data.accel_y*data.accel_y));//俯仰角
angular.z = total_yaw; //偏航角不能靠重力加速度求得
cout<< "Accel Roll: " <<angular.x * (180/3.14159)<<" \t Accel Pitch: "<<angular.y * (180/3.14159)<<"\t Accel Yaw: "<<angular.z * (180/3.14159)<<endl;
cout<< "Accel X: " <<data.accel_x <<"\t Accel Y: "<<data.accel_y<<"\t Accel Z: "<<data.accel_z <<endl;
return angular;
}
imu3d calculate_gyro(const imu6d data, const double duration_time)//初始化角度为0,在整个运动过程中计算累计角度作为运动角度
{
imu3d delta_angular;
cout<<duration_time<<endl;
//相机坐标系中,相机的朝前是z轴,朝左是x轴,朝下是x轴
delta_angular.x = data.gyro_x * duration_time;
delta_angular.y = data.gyro_y * duration_time;
delta_angular.z = data.gyro_z * duration_time;
total_angular.x += delta_angular.x; // 绕x轴转动,俯仰角pitch
total_angular.y += delta_angular.y; // 绕y轴转动,偏航角yaw
total_angular.z += delta_angular.z; // 绕z轴转动,翻转角roll
cout<< "Gyro Roll: " <<total_angular.z * (180/3.14159)<<" \t Gyro Pitch: "<<total_angular.x * (180/3.14159)<<"\t Gyro Yaw: "<<total_angular.y * (180/3.14159)<<endl;
imu3d delta_angle; //把陀螺仪对roll、pitch、yaw的影响转换到加速度计roll、pitch、yaw所对应的相应的位置
delta_angle.x = -delta_angular.z;
delta_angle.y = delta_angular.x;
delta_angle.z = delta_angular.y;
return delta_angle;
}
imu3d complementaryFilter(const imu6d data, const double duration_time) //补偿滤波计算位姿
{
Angle = (Angle + calculate_gyro(data, duration_time)) *0.95 + calculate_accel(data)*0.05;
cout<< "Filter Roll: " <<Angle.x * (180/3.14159)<<" \t Filter Pitch: "<<Angle.y * (180/3.14159)<<"\t Filter Yaw: "<<Angle.z * (180/3.14159)<<endl;
total_yaw = Angle.z;
return Angle;
}
void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg_accel, const sensor_msgs::ImuConstPtr &imu_msg_gyro)
{
cout<<(imu_msg_accel->header.stamp.toSec() == imu_msg_gyro->header.stamp.toSec())<<endl;
double t = imu_msg_accel->header.stamp.toSec();
imu6d imu_data;
imu_data.accel_x = imu_msg_accel->linear_acceleration.x;
imu_data.accel_y = -imu_msg_accel->linear_acceleration.y;
imu_data.accel_z = imu_msg_accel->linear_acceleration.z;
imu_data.gyro_x = imu_msg_gyro->angular_velocity.x;
imu_data.gyro_y = imu_msg_gyro->angular_velocity.y;
imu_data.gyro_z = imu_msg_gyro->angular_velocity.z;
if (flag == 1){
time_s = t;
flag = 0;
Angle = calculate_accel(imu_data);
cout <<"first angle: "<<Angle<<endl;
return;
}
double duration_time = t-time_s;
time_s = t;
complementaryFilter(imu_data, duration_time);
//把加速度计信息和里程计信息的消息弄一起发布
sensor_msgs::Imu imu_data_pub;
imu_data_pub.header.stamp = imu_msg_accel->header.stamp;
imu_data_pub.header.frame_id = imu_msg_accel->header.frame_id;
//线加速度
imu_data_pub.linear_acceleration.x = imu_msg_accel->linear_acceleration.x;
imu_data_pub.linear_acceleration.y = imu_msg_accel->linear_acceleration.y;
imu_data_pub.linear_acceleration.z = imu_msg_accel->linear_acceleration.z;
//角速度
imu_data_pub.angular_velocity.x = imu_msg_gyro->angular_velocity.x;
imu_data_pub.angular_velocity.y = imu_msg_gyro->angular_velocity.y;
imu_data_pub.angular_velocity.z = imu_msg_gyro->angular_velocity.z;
IMU_pub.publish(imu_data_pub);
}
};
int main(int argc, char **argv)
{
ros::init(argc, argv, "imuTester");
imuTester imut;
ros::spin();
return 0;
}