机器人自动导航ROS开发之-IMU(惯性测量单元)驱动----激光雷达-3D视觉-IMU-GPS融合机器人SLAM导航--老毛

并可进一步解算出姿态角如俯仰角、横滚角、航向角的传感器组合装置。惯性测量单元IMU就像是电子设备的“内耳”和“平衡感”它不依赖外部信号如GPS仅凭自身就能感知物体在三维空间中的运动状态。简单来说IMU 是一个集成了多种传感器的模块通过测量加速度和角速度来推算物体的姿态、速度和位置。⚙️ 核心结构三大传感器IMU 的核心通常由以下三种传感器组成它们各司其职协同工作加速度计:功能测量物体在三个正交轴X, Y, Z上的线加速度。原理可以想象成在一个小盒子里用弹簧悬挂着一个重物。当盒子加速时重物会因为惯性而挤压弹簧通过测量弹簧的形变就能知道加速度的大小。作用感知倾斜在静止时加速度计能感知到重力加速度1g的方向从而计算出物体相对于地面的俯仰角和横滚角。感知运动在运动时它能测量物体加速或减速的快慢。陀螺仪功能测量物体绕三个正交轴X, Y, Z旋转的角速度或称角速率。原理现代IMU多采用微机电系统MEMS陀螺仪其原理基于科里奥利力。可以想象一个高速振动的音叉当整个装置旋转时音叉会受到一个垂直于振动方向和旋转方向的力通过检测这个力就能算出旋转速度。作用感知旋转精确测量物体转动的快慢和方向。 短期姿态稳定在短时间内陀螺仪的数据非常精准能快速响应物体的姿态变化。磁力计可选但很常见:功能测量物体所在位置的磁场强度和方向。 原理相当于一个高精度的电子罗盘。作用确定航向通过感知地球磁场的方向来确定物体的航向角或称偏航角即物体朝向哪个地理方向。修正漂移为陀螺仪提供绝对的航向参考以修正其累积误差。 核心功能与数据融合IMU 的真正强大之处在于它不是简单地将三个传感器的数据罗列出来而是通过复杂的算法如卡尔曼滤波将它们融合在一起取长补短最终解算出物体的完整姿态和运动信息。姿态解算这是 IMU 最核心的功能。通过融合加速度计、陀螺仪和磁力计的数据可以实时计算出物体在三维空间中的三个姿态角俯仰角物体绕横轴Y轴的旋转角度好比飞机抬头或低头。横滚角物体绕纵轴X轴的旋转角度好比飞机向左或向右压坡度。航向角物体绕垂直轴Z轴的旋转角度好比指南针指示的方向。为什么需要数据融合因为每个传感器都有优缺点加速度计静态测倾角准但一运动就会混入自身加速度导致数据“噪音”很大。陀螺仪动态响应快短期精度高但存在零偏漂移长时间积分会导致误差无限累积角度越算越偏。磁力计能提供绝对的航向参考但容易受到周围铁磁物质如电机、钢筋的干扰。数据融合算法就像一个聪明的“裁判”它会动态地信任更可靠的传感器数据。例如在快速转动时它更相信陀螺仪在平稳飞行时它用加速度计和磁力计的数据去“拉回”陀螺仪的漂移。导航推算在已知初始位置和速度的前提下通过对加速度进行一次积分可以得到速度进行二次积分可以得到位置。这种方式被称为惯性导航。虽然由于误差累积它不适合长时间独立定位但在GPS信号丢失的短时间内如隧道中它是维持定位的关键。 主要应用领域由于其自主性强、数据更新率高、短期精度好的特点IMU 被广泛应用于消费电子智能手机自动旋转屏幕、计步、游戏手柄、VR/AR 设备头部追踪。无人机与机器人实现飞行姿态的稳定控制、自主平衡、导航和避障。自动驾驶与 GPS、激光雷达等传感器融合提供高频率、高可靠的车辆姿态和位置信息尤其在隧道、城市峡谷等 GPS 信号不佳的场景下至关重要。航空航天飞机、导弹、卫星的姿态控制和导航系统。工业与测绘平台稳定、设备姿态监测、移动测绘系统等。下面为构成了一个基于 ROS (Robot Operating System) 的 IMU (惯性测量单元) 数据采集、解析和校验系统。它通过串口与 IMU 传感器通信将读取到的原始数据打包成 ROS 消息发布出去并包含一个用于数据校验的 CRC 模块和一个用于监听数据的节点。1.crc.h (头文件) //这个文件是 CRC (循环冗余校验) 模块的头文件声明了用于数据校验的类和方法。ifndef CRC_H // 防止头文件被重复包含的宏定义开始define CRC_H // 防止头文件被重复包含的宏定义include //Cinclude std_msgs/UInt8.h //include stdio.h //include stdint.h // 包含标准整型库用于定义 uint8_t, uint16_t 等固定宽度类型namespace crc { // 定义一个名为 crc 的命名空间防止命名冲突class CRC { // 声明一个名为 CRC 的类public:CRC(void); // 声明 CRC 类的构造函数~CRC(void); // 声明 CRC 类的析构函数uint8_t sum(uint8_tbuf, uint8_t i, uint8_t number); // 声明一个求和校验函数对缓冲区 buf 从索引 i 开始的 number 个字节进行求和并取反unsigned int CRC16(unsigned charpchMsg, unsigned int wDataLen); // 声明一个 CRC16 校验函数计算数据的 16 位 CRC 值uint8_t cc11; // 声明一个公共的 uint8_t 类型成员变量可能用于存储校验结果};}endif // 防止头文件被重复包含的宏定义结束2.crc.cpp //这个文件实现了 crc.h 中声明的 CRC 校验逻辑包含一个求和校验和一个标准的 CRC16 校验算法。includeinclude std_msgs/UInt8.hinclude ros/ros.hinclude stdio.hinclude crc.husing crc::CRC;static const unsigned char chCRCHTalbe[] // CRC 高位字节值表{0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,0x00, 0xC1, 0x81, 0x40};static const unsigned char chCRCLTalbe[] // CRC 低位字节值表{0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7,0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E,0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9,0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC,0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32,0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D,0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38,0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF,0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1,0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4,0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB,0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA,0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0,0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97,0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E,0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89,0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83,0x41, 0x81, 0x80, 0x40};CRC::CRC(void) //构造函数,不能带返回值{}CRC::~CRC(void) //构造函数,不能带返回值{}uint8_t CRC::sum(uint8_t *buf,uint8_t i,uint8_t number){uint8_t sum1;uint8_t p;//sum1buf[i];for(p0; pnumber; p){sum1buf[ip];}sum1 ~sum1;return sum1;}unsigned int CRC::CRC16(unsigned char* pchMsg, unsigned int wDataLen){unsigned int chCRCHi 0xFF; // 高CRC字节初始化unsigned int chCRCLo 0xFF; // 低CRC字节初始化unsigned long int wIndex; // CRC循环中的索引while (wDataLen--){// 计算CRCwIndex chCRCLo ^ *pchMsg ;chCRCLo chCRCHi ^ chCRCHTalbe[wIndex];chCRCHi chCRCLTalbe[wIndex] ;}return ((chCRCHi 8) | chCRCLo) ;}imu_nod.cpp //这是 IMU 节点的主程序负责与 IMU 传感器进行串口通信解析数据并将其发布为 ROS 消息include ros/ros.h // 包含 ROS 核心库include serial/serial.h // 包含串口通信库include std_msgs/String.h // 包含 ROS 的 String 消息类型include std_msgs/Empty.h // 包含 ROS 的 Empty 消息类型include std_msgs/UInt8.h // 包含 ROS 的 UInt8 消息类型include std_msgs/UInt8MultiArray.h // 包含 ROS 的 UInt8MultiArray 消息类型用于发布字节数组include crc.h // 包含 CRC 校验模块的头文件// 宏定义将四个字符组合成一个 32 位整数用于构建命令字define iCMD(a,b,c,d) ((int32_t)(a24)(int32_t)(b16)(int32_t)(c8)(int32_t)d)// 枚举定义定义不同的命令字enum iCCMD{iRoboticCmd (int)iCMD(R,B,T,_), // 机器人命令iImuRpy (int)iCMD(I,M,U,B), // IMU 姿态角命令iImuAll (int)iCMD(I,M,U,F) // IMU 全部数据命令};// 定义一个用于发送查询指令的数据结构typedef struct{uint16_t header; // 帧头应为 0xAAEAuint32_t Cmd; // 命令字uint16_t length; // 数据长度uint8_t p; // 参数uint8_t crc; // 校验位} imu_query;int main (int argc, char** argv) {serial::Serial serialPort; // 创建一个串口对象ros::init(argc, argv, imu_node); // 初始化 ROS 节点命名为 imu_noderos::NodeHandle nh; // 创建节点句柄// --- 串口初始化 ---try {serialPort.setPort(/dev/ttyUSB0); // 设置串口端口serialPort.setBaudrate(115200); // 设置波特率为 115200serial::Timeout timeOut serial::Timeout::simpleTimeout(10); // 设置超时时间为 10msserialPort.setTimeout(timeOut); // 应用超时设置serialPort.open(); // 打开串口} catch (serial::IOException e) { // 捕获打开串口失败的异常ROS_ERROR_STREAM(Unable to open imu port!); // 打印错误日志return -1; // 退出程序}if(serialPort.isOpen()) // 检查串口是否成功打开ROS_INFO_STREAM(Imu port opened); // 打印成功信息elsereturn -1; // 打开失败则退出// --- ROS 发布者初始化 ---// 创建一个发布者发布 std_msgs::UInt8MultiArray 类型的消息到 sensor_imu 主题ros::Publisher sensor_pub nh.advertisestd_msgs::UInt8MultiArray(sensor_imu, 10);ros::Rate loop_rate(50); // 设置循环频率为 50Hz// --- 主循环 ---while(ros::ok()) {ros::spinOnce(); // 处理 ROS 回调函数// --- 1. 发送查询指令 ---imu_query iq; // 创建一个查询指令结构体iq.header 0xAAEA; // 设置帧头iq.Cmd iImuAll; // 设置命令字为获取全部 IMU 数据iq.length 0x0001; // 设置数据长度iq.crc 0x01; // 设置校验位 (此处为占位符)// --- 1.2 序列化 ---uint8_t sendBuffer[10]; // 创建一个 10 字节的发送缓冲区memset(sendBuffer,0,sizeof(sendBuffer)); // 将缓冲区清零// 将 iq 结构体的各个字段按字节拆分并存入 sendBuffersendBuffer[0] (uint8_t)0xAA;sendBuffer[1] (uint8_t)0xEA;sendBuffer[2] (uint8_t)((iq.Cmd24)0xff);sendBuffer[3] (uint8_t)((iq.Cmd16)0xff);sendBuffer[4] (uint8_t)((iq.Cmd8)0xff);sendBuffer[5] (uint8_t)((iq.Cmd0)0xff);sendBuffer[6] 0x00;sendBuffer[7] 0x01;sendBuffer[8] 0x01;sendBuffer[9] 0x01;// --- 1.3 发送数据 ---ROS_INFO_STREAM(Writing to serial port ); // 打印发送日志serialPort.write( sendBuffer,sizeof(sendBuffer) ); // 通过串口发送数据// 打印发送的字节内容ROS_INFO(: 0x%x, 0x%x; 0X%x, 0X%x, 0X%x, 0X%x; 0X%x, 0X%x; 0X%x, 0X%x, sendBuffer[0],sendBuffer[1], sendBuffer[2],sendBuffer[3],sendBuffer[4],sendBuffer[5], sendBuffer[6],sendBuffer[7], sendBuffer[8], sendBuffer[9]);// --- 2. 接收并解析数据 ---if(serialPort.available()) { // 检查串口缓冲区是否有数据ROS_INFO_STREAM(Reading from serial port ); // 打印接收日志int len serialPort.available(); // 获取可读数据的长度ROS_INFO(serial receive len:%d , len); // 打印数据长度uint8_t redeiveData[128]{0}; // 创建接收缓冲区serialPort.read (redeiveData, len) ; // 从串口读取数据// 打印接收到的原始数据std::cout len: std::endl;for(int i0; ilen; i) std::cout (int)redeiveData[i] ,;std::cout std::endl;// --- 2.3 发布 ROS 消息 ---std_msgs::UInt8MultiArray msgBuffer; // 创建一个 ROS 消息msgBuffer.data.resize(len1); // 调整消息数据数组的大小msgBuffer.data[0] len; // 第一个字节存放数据长度for(int i0; ilen; i) msgBuffer.data[i1]redeiveData[i]; // 填充数据sensor_pub.publish( msgBuffer); // 发布消息// --- 2.4 解析数据 ---// 如果接收到的数据长度为 0x40 (64字节)则进行解析if(len0x40) {ROS_INFO_STREAM(read 40);// 解析姿态角 (Roll, Pitch, Yaw)uint8_t sRPY 78;int32_t roll,pitch,yaw;roll iCMD(redeiveData[sRPY3],redeiveData[sRPY2],redeiveData[sRPY1],redeiveData[sRPY0]);pitchiCMD(redeiveData[sRPY7],redeiveData[sRPY6],redeiveData[sRPY5],redeiveData[sRPY4]);yaw iCMD(redeiveData[sRPY11],redeiveData[sRPY10],redeiveData[sRPY9],redeiveData[sRPY8]);// 解析陀螺仪数据 (Gyroscope)uint8_t sGyro sRPY12;int32_t Gx,Gy,Gz;Gx iCMD(redeiveData[sGyro3],redeiveData[sGyro2],redeiveData[sGyro1],redeiveData[sGyro0])/4096/100;Gy iCMD(redeiveData[sGyro7],redeiveData[sGyro6],redeiveData[sGyro5],redeiveData[sGyro4])/4096/100;Gz iCMD(redeiveData[sGyro11],redeiveData[sGyro10],redeiveData[sGyro9],redeiveData[sGyro8])/4096/100;// 解析加速度计数据 (Accelerometer)uint8_t sAcc sGyro12;int32_t Ax,Ay,Az;Ax iCMD(redeiveData[sAcc3],redeiveData[sAcc2],redeiveData[sAcc1],redeiveData[sAcc0])/4096/100;Ay iCMD(redeiveData[sAcc7],redeiveData[sAcc6],redeiveData[sAcc5],redeiveData[sAcc4])/4096/100;Az iCMD(redeiveData[sAcc11],redeiveData[sAcc10],redeiveData[sAcc9],redeiveData[sAcc8])/4096/100;// 解析磁力计数据 (Magnetometer)uint8_t sMag sAcc12;int32_t Mx,My,Mz;Mx iCMD(redeiveData[sMag3],redeiveData[sMag2],redeiveData[sMag1],redeiveData[sMag0]);My iCMD(redeiveData[sMag7],redeiveData[sMag6],redeiveData[sMag5],redeiveData[sMag4]);Mz iCMD(redeiveData[sMag11],redeiveData[sMag10],redeiveData[sMag9],redeiveData[sMag8]);// 打印解析后的数据std::cout RPY: roll \t pitch \t yaw std::endl;std::cout Gyro: Gx \t Gy \t Gz std::endl;std::cout Acc : Ax \t Ay \t Az std::endl;std::cout Mag : Mx \t My \t Mz std::endl;}}loop_rate.sleep(); // 按照设定的频率休眠}return 0;}listen.cpp //这是一个简单的 ROS 监听器节点用于订阅 sensor 主题并检查接收到的数据。include ros/ros.h // 包含 ROS 核心库include std_msgs/String.h // 包含 ROS 的 String 消息类型include std_msgs/UInt8MultiArray.h // 包含 ROS 的 UInt8MultiArray 消息类型include std_msgs/MultiArrayLayout.h // 包含 MultiArrayLayout 消息类型include std_msgs/MultiArrayDimension.h // 包含 MultiArrayDimension 消息类型include std_msgs/Int32MultiArray.h // 包含 Int32MultiArray 消息类型include crc.h // 包含 CRC 校验模块的头文件crc::CRC cc_crc; // 创建一个 CRC 类的全局实例class Listener { // 定义一个 Listener 类public:// 声明一个回调函数用于处理接收到的 UInt8MultiArray 消息void callback(const std_msgs::UInt8MultiArray::ConstPtr r_buffer);};// Listener 类的回调函数实现void Listener::callback(const std_msgs::UInt8MultiArray::ConstPtr r_buffer) {// 检查接收到的数据的第二个字节是否为 0x7eif(r_buffer-data[1]0x7e) {// 如果是则打印第一个字节的值和 CRC 实例中的 cc11 变量ROS_INFO(listener heard: %x %d ,r_buffer-data[0],cc_crc.cc11);} else {// 如果不是则打印 wrongROS_INFO(wrong);}}int main(int argc, char **argv) {ros::init(argc, argv, listener); // 初始化 ROS 节点命名为 listenerros::NodeHandle n; // 创建节点句柄