IMU简介
十轴惯性导航传感器WTGAHRS2传感器集成高精度的陀螺仪、加速度计、地磁场传感器、GPS 模块,采用高性能的微处理器和先进的动力学解算与卡尔曼动态滤波算法,能够快速求解出模块当前的实时运动姿态。
采用先进的数字滤波技术,能有效降低测量噪声,提高测量精度。传感器内部集成了姿态解算器,配合动态卡尔曼滤波算法,能够在动态环境下准确输出模块的当前姿态,姿态测量精度静态 0.05 度,动态 0.1 度,稳定性极高,性能甚至优于某些专业的倾角仪!
内部自带电压稳定电路,工作电压 3.3v~5v,引脚电平兼容 3.3V/5V 的嵌入式系
统,连接方便。支持串口 TTL/232,方便用户选择最佳的连接方式。串口速率 2400bps~921600bps
可调。GPS 信息、姿态传感器信息同步输出。最高 200Hz 数据输出速率。输入内容可以任意选择,输出速率 0.1~200HZ 可调节。
资料下载:https://pan.baidu.com/s/1kUxka08OW2x1Ej-7jvEgWw
验证码:0eug
官网:http://wit-motion.cn/guandaodaohang/26.html
驱动程序
在ROS中的驱动是以驱动文件的形式存在的,需要我们自己编写驱动程序。在这里我们参考https://blog.csdn.net/qqliuzhitong/article/details/114384297文章编写自己的驱动程序。
IMU串口通信协议
IMU使用的是JY901芯片,在官方说明文档里可以找到串口通信协议
程序
关于自定义话题及ros话题发布接受机制请参考:https://blog.csdn.net/qq_43569735/article/details/108065179
串口程序+话题发布
//Created by ljm
#include <ros/ros.h>
#include <serial/serial.h>
#include <iostream>
#include "agv_imu/imu_data.h"
int main(int argc, char** argv)
{
ros::init(argc, argv, "agv_imu");
ros::NodeHandle n;
//创建imu数据话题发布
ros::Publisher pub_imu_data;
pub_imu_data = n.advertise<agv_imu::imu_data>("AGV/imu",50);
//创建一个serial对象
serial::Serial sp;
//创建timeout
serial::Timeout to = serial::Timeout::simpleTimeout(100);
//设置要打开的串口名称
sp.setPort("/dev/ttyUSB0");
//设置串口通信的波特率
sp.setBaudrate(9600);
//串口设置timeout
sp.setTimeout(to);
double ax,ay,az;
double wx,wy,wz;
double angle_roll,angle_pitch,angle_yaw;
agv_imu::imu_data msg;
try
{
//打开串口
sp.open();
}
catch(serial::IOException& e)
{
ROS_ERROR_STREAM("Unable to open port.");
return -1;
}
//判断串口是否打开成功
if(sp.isOpen())
{
ROS_INFO_STREAM("/dev/ttyUSB0 is opened.");
}
else
{
return -1;
}
ros::Rate loop_rate(100);
while(ros::ok())
{
//获取缓冲区内的字节数
size_t n = sp.available();
if(n!=0)
{
uint8_t buffer[1024];
//读出数据
n = sp.read(buffer, n);
for(int i=0; i<n; i++)
{
if(buffer[i]==0X55) //IMU地址
{
if(buffer[i+1]==0x51) //加速度帧
{
signed short int temp;
temp=buffer[i+3];
temp=temp<<8;
temp=(temp|buffer[i+2]);
ax=(temp/32768.0)*16*9.8051;
temp=buffer[i+5]; //加速度八位
temp=temp<<8;
temp=(temp|buffer[i+4]); //加速度高八位
ay=(temp/32768.0)*16*9.8051;
temp=buffer[i+7]; //加速度低八位
temp=temp<<8;
temp=(temp|buffer[i+6]); //加速度高八位
az=(temp/32768.0)*16*9.8051;
std::cout<<"ax:"<<ax<<std::endl;
std::cout<<"ay:"<<ay<<std::endl;
std::cout<<"az:"<<az<<std::endl;
i+=11;
}
if(buffer[i+1]==0x52) //角速度帧
{
signed short int temp;
temp=buffer[i+3];
temp=temp<<8;
temp=(temp|buffer[i+2]);
wx=(temp/32768.0)*2000;
temp=buffer[i+5]; //角速度低八位
temp=temp<<8;
temp=(temp|buffer[i+4]); //角速度高八位
wy=(temp/32768.0)*2000;
temp=buffer[i+7]; //角速度低八位
temp=temp<<8;
temp=(temp|buffer[i+6]); //角速度高八位
wz=(temp/32768.0)*2000;
std::cout<<"wx:"<<wx<<std::endl;
std::cout<<"wy:"<<wy<<std::endl;
std::cout<<"wz:"<<wz<<std::endl;
i+=11;
}
if(buffer[i+1]==0x53) //角度帧
{
unsigned short int temp;
temp=buffer[i+3]; //角度低八位
temp=temp<<8;
temp=(temp|buffer[i+2]); //角度高八位
angle_roll=(temp/32768.0)*180;
temp=buffer[i+5]; //角度低八位
temp=temp<<8;
temp=(temp|buffer[i+4]); //角度高八位
angle_pitch=(temp/32768.0)*180;
temp=buffer[i+7]; //角度低八位
temp=temp<<8;
temp=(temp|buffer[i+6]); //角度高八位
angle_yaw=(temp/32768.0)*180;
std::cout<<"angle_roll:"<<angle_roll<<std::endl;
std::cout<<"angle_pitch:"<<angle_pitch<<std::endl;
std::cout<<"angle_yaw:"<<angle_yaw<<std::endl;
i+=11;
}
msg.angx=angle_roll;
msg.angy=angle_pitch;
msg.angz=angle_yaw;
msg.ax=ax;
msg.ay=ay;
msg.az=az;
msg.wx=wx;
msg.wy=wy;
msg.wz=wz;
pub_imu_data.publish(msg);
}
}
std::cout << std::endl;
//sp.write(buffer, n);
}
loop_rate.sleep();
}
//关闭串口
sp.close();
return 0;
}
自定义消息话题:imu_data.msg
float64 ax
float64 ay
float64 az
float64 wx
float64 wy
float64 wz
float64 angx
float64 angy
float64 angz
效果