第 14.6.2 節
慣性計IMU
0瀏覽次數0訪問次數--跳出率--平均停留
簡介
概念
IMU是慣性測量單元（Inertial Measurement Unit）的縮寫，它是一種集成了多個慣性傳感器如加速度計、陀螺儀、磁力計等的導航傳感器。IMU可以實時地測量和記錄其所在物體的加速度、角速度和方向等信息，並通過信號處理和數學模型來計算出物體的姿態、位置和運動狀態等。
IMU（慣性測量單元）主要由以下幾種類型組成：
- 加速度計（Accelerometer）：用於測量物體的加速度，通常以三個軸向（X、Y、Z）提供加速度數據。加速度計可以幫助判斷物體的線性運動和動作姿態變化。
- 陀螺儀（Gyroscope）：用於測量物體的角速度或角度變化率。陀螺儀可以提供關於物體旋轉的信息，判斷物體的角度、方向和轉動動作。
- 磁力計（Magnetometer）：用於測量物體周圍的磁場強度和方向（即測量場強）。磁力計可以幫助判斷物體的方向和導航，尤其在地磁定位和航向控制中起重要作用。
這些傳感器通常集成在一起形成IMU，並通過數據融合算法將它們的輸出結合起來，提供綜合的姿態、位置和運動信息。
作用
IMU是一種重要的傳感器組件，它在姿態感知、行駛狀態監測、非GPS定位中發揮着重要的作用。
特點
慣性測量單元（IMU）作爲一種常見的傳感器，在機器人領域中也有許多應用，下面是IMU在機器人領域的一些優點和缺點：
（1）優點：
- 實時性高： IMU能夠以很高的採樣頻率獲取姿態和加速度數據，提供實時的運動信息。
- 精度較高： IMU可以測量物體的加速度和角速度，經過合成和濾波處理後，可提供較高精度的姿態估計和運動跟蹤。
- 不受光照條件限制： IMU不依賴於光照條件，適用於各種環境，如室內、室外和低光條件。
- 尺寸小巧： IMU通常具有小型、輕量級和緊湊的設計，適合嵌入式系統和對空間要求較高的機器人平臺。
- 低功耗： IMU的功耗相對較低，適合資源受限或需要長時間運行的應用。
（2）缺點：
- 累積誤差： 由於積分計算的誤差會隨時間累積，IMU的姿態估計隨着時間推移可能會出現漂移，需要與其他傳感器進行融合或採用校準方法進行補償。
- 受外部干擾影響： IMU的測量結果容易受到振動、震動和溫度變化等外界干擾的影響，可能導致姿態估計不穩定或不準確。
- 不適用於絕對定位： 單獨使用IMU無法提供物體的絕對位置信息，需要結合其他傳感器（如GNSS）來實現絕對定位。
- 無法感知環境信息： IMU只能提供自身的加速度和角速度數據，無法直接感知環境或檢測周圍物體。
綜上所述，IMU在機器人領域具有實時性高、精度較高、不受光照條件限制、尺寸小巧和低功耗等優點。然而，它也存在累積誤差、受外部干擾影響、不適用於絕對定位和無法感知環境信息等缺點。因此，在設計機器人系統時需綜合考慮應用需求，並與其他傳感器（如相機、激光雷達等）進行融合，以提升機器人的感知、定位和控制能力。
消息接口
在ROS2中imu消息被封裝爲了sensor_msgs/msg/Imu接口，可通過指令ros2 interface show sensor_msgs/msg/Imu查看該接口的具體數據結構，結果如下：
# 这是一个从惯性测量单元（Inertial Measurement Unit，简称IMU）获取数据的消息。
#
# 加速度应该以米/秒^2为单位（不是以g为单位），旋转速度应该以弧度/秒为单位。
#
# 如果测量的协方差已知，则应该填写相应值。全零的协方差矩阵将被解释为“协方差未知”，必须从其他源获取协方差才能使用协方差数据。
#
# 如果没有其中一个数据元素的估计值（例如，你的IMU不会产生方向估计），请将相关协方差矩阵的第 0 个元素设置为 -1。
# 如果你正在解析此消息，请检查每个协方差矩阵的第一个元素中是否有 -1 的值，并忽略相关的估计值。
std_msgs/Header header # 标头
builtin_interfaces/Time stamp # 时间戳
int32 sec
uint32 nanosec
string frame_id # imu坐标系
geometry_msgs/Quaternion orientation # 四元数姿态
float64 x 0
float64 y 0
float64 z 0
float64 w 1
float64[9] orientation_covariance # 姿态协方差矩阵
geometry_msgs/Vector3 angular_velocity # 三轴角速度
float64 x
float64 y
float64 z
float64[9] angular_velocity_covariance # 角速度协方差矩阵
geometry_msgs/Vector3 linear_acceleration # 三轴线性加速度
float64 x
float64 y
float64 z
float64[9] linear_acceleration_covariance # 线性加速度协方差矩阵
功能包創建
首先先創建功能包
ros2 pkg create imu_plumb --build-type ament_cmake --dependencies rclcpp sensor_msgs --node-name imu_node
先編譯一下
colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON
節點主要邏輯
#include "rclcpp/rclcpp.hpp"
#include "serial_driver/serial_driver.hpp"
#include "cyber_imu/WitStandardProtocol.h"
#include "sensor_msgs/msg/imu.hpp"
#include <cstdint>
#include <rclcpp/logging.hpp>
#include <sensor_msgs/msg/detail/imu__struct.hpp>
#include <tf2/LinearMath/Quaternion.h>
class ImuNode: public rclcpp::Node
{
public:
ImuNode():Node("ImuNode_node_cpp")
{
RCLCPP_INFO(this->get_logger(),"惯性计imu节点启动!");
//创建odom话题通信发布方
imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("/imu",10);
// 声明参数（带默认值）
// 串口默认设备名（根据实际设备调整）
this->declare_parameter("device_name", "/dev/ttyUSB0");
//串口默认波特率
this->declare_parameter("baud_rate", 115200);
// 获取参数值
const std::string device_name = this->get_parameter("device_name").as_string();
const uint32_t baud_rate = this->get_parameter("baud_rate").as_int();
// 创建串口配置对象
// 波特率默认115200；不开启流控制；无奇偶效验；停止位1。
drivers::serial_driver::SerialPortConfig config(
baud_rate,
drivers::serial_driver::FlowControl::NONE,
drivers::serial_driver::Parity::NONE,
drivers::serial_driver::StopBits::ONE);
// 初始化串口
try
{
io_context_ = std::make_shared<drivers::common::IoContext>(1);
// 初始化 serial_driver_
serial_driver_ = std::make_shared<drivers::serial_driver::SerialDriver>(*io_context_);
serial_driver_->init_port(device_name, config);
serial_driver_->port()->open();
RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
RCLCPP_INFO(this->get_logger(), "Using device: %s", serial_driver_->port().get()->device_name().c_str());
RCLCPP_INFO(this->get_logger(), "Baud_rate: %d", config.get_baud_rate());
}
catch (const std::exception &ex)
{
RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what());
return;
}
async_receive_message(); //进入异步接收
}
private:
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
std::shared_ptr<drivers::serial_driver::SerialDriver> serial_driver_;
std::shared_ptr<drivers::common::IoContext> io_context_;
void async_receive_message() //创建一个函数更方便重新调用
{
auto port = serial_driver_->port();
// 设置接收回调函数
port->async_receive([this](const std::vector<uint8_t> &data,const size_t &size)
{
if (size > 0)
{
RCLCPP_DEBUG(this->get_logger(),"\n");
RCLCPP_DEBUG(this->get_logger(),"以下是接收到的IMU惯性计数据:");
//创建imu消息类型
auto imu_msg = sensor_msgs::msg::Imu();
bool valid_frame = false; // 添加有效帧标志
bool valid_data = false; // 添加有效帧标志
for(int16_t i = 0;i < (int16_t)size;++i)
{
uint8_t data_buffer = data[i];
// RCLCPP_INFO(this->get_logger(),"接收到的字节：%x",data_buffer);
// 处理接收到的数据
if(wit_imu_.rx.Data_Analysis(&data_buffer) == true)
{
valid_frame = true; // 标记有有效帧需要处理
}
if(valid_frame == true)
{
if(wit_imu_.rx.data.cmd == 0x51)
{
// 存储加速度计数据
wit_imu_.rx.data.imu.Accel.X = wit_imu_.rx.data.int16_buffer[0];
wit_imu_.rx.data.imu.Accel.Y = wit_imu_.rx.data.int16_buffer[1];
wit_imu_.rx.data.imu.Accel.Z = wit_imu_.rx.data.int16_buffer[2];
wit_imu_.rx.data.imu.Temp = wit_imu_.rx.data.int16_buffer[3];
//加速度单位m/s2，温度单位℃摄氏度
wit_imu_.rx.data.imu.Accel.X = wit_imu_.rx.data.imu.Accel.X / 32768.0f * 16.0f * 9.8f;
wit_imu_.rx.data.imu.Accel.Y = wit_imu_.rx.data.imu.Accel.Y / 32768.0f * 16.0f * 9.8f;
wit_imu_.rx.data.imu.Accel.Z = wit_imu_.rx.data.imu.Accel.Z / 32768.0f * 16.0f * 9.8f;
wit_imu_.rx.data.imu.Temp = wit_imu_.rx.data.imu.Temp / 100.0f;
// 打印加速度计数据
RCLCPP_DEBUG(this->get_logger(), "加速度(XYZ): %.6f, %.6f, %.6f",
wit_imu_.rx.data.imu.Accel.X, wit_imu_.rx.data.imu.Accel.Y, wit_imu_.rx.data.imu.Accel.Z);
//减少时间戳带来的延迟
imu_msg.header.stamp = this->get_clock()->now();
}
else if(wit_imu_.rx.data.cmd == 0x52)
{
// 存储陀螺仪数据
wit_imu_.rx.data.imu.Gyro.X = wit_imu_.rx.data.int16_buffer[0];
wit_imu_.rx.data.imu.Gyro.Y = wit_imu_.rx.data.int16_buffer[1];
wit_imu_.rx.data.imu.Gyro.Z = wit_imu_.rx.data.int16_buffer[2];
//角速度单位rad/s
wit_imu_.rx.data.imu.Gyro.X = wit_imu_.rx.data.imu.Gyro.X / 32768.0f * 2000.0f * (M_PI / 180.0f);
wit_imu_.rx.data.imu.Gyro.Y = wit_imu_.rx.data.imu.Gyro.Y / 32768.0f * 2000.0f * (M_PI / 180.0f);
wit_imu_.rx.data.imu.Gyro.Z = wit_imu_.rx.data.imu.Gyro.Z / 32768.0f * 2000.0f * (M_PI / 180.0f);
// 打印陀螺仪数据
RCLCPP_DEBUG(this->get_logger(), "角速度(XYZ): %.6f, %.6f, %.6f",
wit_imu_.rx.data.imu.Gyro.X, wit_imu_.rx.data.imu.Gyro.Y, wit_imu_.rx.data.imu.Gyro.Z);
}
else if(wit_imu_.rx.data.cmd == 0x54)
{
// 存储磁力计数据(单位lsb)
wit_imu_.rx.data.imu.Magnet.X = wit_imu_.rx.data.int16_buffer[0];
wit_imu_.rx.data.imu.Magnet.Y = wit_imu_.rx.data.int16_buffer[1];
wit_imu_.rx.data.imu.Magnet.Z = wit_imu_.rx.data.int16_buffer[2];
//磁场单位uT
// wit_imu_.rx.data.imu.Magnet.X = wit_imu_.rx.data.imu.Magnet.X / 1.0f;
// wit_imu_.rx.data.imu.Magnet.Y = wit_imu_.rx.data.imu.Magnet.Y / 1.0f;
// wit_imu_.rx.data.imu.Magnet.Z = wit_imu_.rx.data.imu.Magnet.Z / 1.0f;
// 打印磁力计数据
RCLCPP_DEBUG(this->get_logger(), "磁场(XYZ): %.6f, %.6f, %.6f",
wit_imu_.rx.data.imu.Magnet.X, wit_imu_.rx.data.imu.Magnet.Y, wit_imu_.rx.data.imu.Magnet.Z);
}
else if(wit_imu_.rx.data.cmd == 0x53)
{
// 存储欧拉角数据
wit_imu_.rx.data.imu.Euler.roll = wit_imu_.rx.data.int16_buffer[0];
wit_imu_.rx.data.imu.Euler.pitch = wit_imu_.rx.data.int16_buffer[1];
wit_imu_.rx.data.imu.Euler.yaw = wit_imu_.rx.data.int16_buffer[2];
//欧拉角单位dec
wit_imu_.rx.data.imu.Euler.roll = wit_imu_.rx.data.imu.Euler.roll / 32768.0f * 180.0f;
wit_imu_.rx.data.imu.Euler.pitch = wit_imu_.rx.data.imu.Euler.pitch / 32768.0f * 180.0f;
wit_imu_.rx.data.imu.Euler.yaw = wit_imu_.rx.data.imu.Euler.yaw / 32768.0f * 180.0f;
// 打印欧拉角数据
RCLCPP_DEBUG(this->get_logger(), "欧拉角(XYZ_RPY): %.6f, %.6f, %.6f",
wit_imu_.rx.data.imu.Euler.roll, wit_imu_.rx.data.imu.Euler.pitch, wit_imu_.rx.data.imu.Euler.yaw);
//欧拉角单位rad
wit_imu_.rx.data.imu.Euler.roll = wit_imu_.rx.data.imu.Euler.roll * (M_PI / 180.0f);
wit_imu_.rx.data.imu.Euler.pitch = wit_imu_.rx.data.imu.Euler.pitch * (M_PI / 180.0f);
wit_imu_.rx.data.imu.Euler.yaw = wit_imu_.rx.data.imu.Euler.yaw * (M_PI / 180.0f);
}
else if(wit_imu_.rx.data.cmd == 0x59)
{
for(int16_t i = 0 ; i < 4 ; i++)
{
// 存储四元数数据
wit_imu_.rx.data.imu.Quat.q[i] = wit_imu_.rx.data.int16_buffer[i];
//四元数是归一化的四元数
wit_imu_.rx.data.imu.Quat.q[i] = wit_imu_.rx.data.imu.Quat.q[i] / 32768.0f;
}
// 打印四元数数据
//注意，0对应y，1对应x，-2才对应z。
RCLCPP_DEBUG(this->get_logger(), "四元数(xyzw): %.6f, %.6f, %.6f, %.6f",
wit_imu_.rx.data.imu.Quat.q[1],wit_imu_.rx.data.imu.Quat.q[0],-wit_imu_.rx.data.imu.Quat.q[2],wit_imu_.rx.data.imu.Quat.q[3]);
valid_data = true;
}
wit_imu_.rx.data.cmd = 0x00; //跑过一次就进行清0
valid_frame = false;
}
}
if(valid_data == true)
{
//时间戳
// imu_msg.header.stamp = this->get_clock()->now();
//位姿信息所参考的坐标系
imu_msg.header.frame_id = "imu";
// tf2::Quaternion q;
// //DOF欧拉角单位rad
// q.setRPY(roll_, pitch_, yaw_);
// //DOF欧拉角（四元数）
// imu_msg.orientation.x = q.x();
// imu_msg.orientation.y = q.y();
// imu_msg.orientation.z = q.z();
// imu_msg.orientation.w = q.w();
//DOF欧拉角（四元数）
//注意对应关系
// 传感器坐标系 -> ROS坐标系：
// X -> Y
// Y -> X
// Z -> -Z
imu_msg.orientation.x = wit_imu_.rx.data.imu.Quat.q[1];
imu_msg.orientation.y = wit_imu_.rx.data.imu.Quat.q[0];
imu_msg.orientation.z = - wit_imu_.rx.data.imu.Quat.q[2];
imu_msg.orientation.w = wit_imu_.rx.data.imu.Quat.q[3];
//加速度
imu_msg.linear_acceleration.x = wit_imu_.rx.data.imu.Accel.X;
imu_msg.linear_acceleration.y = wit_imu_.rx.data.imu.Accel.Y;
imu_msg.linear_acceleration.z = wit_imu_.rx.data.imu.Accel.Z;
//角速度
imu_msg.angular_velocity.x = wit_imu_.rx.data.imu.Gyro.X;
imu_msg.angular_velocity.y = wit_imu_.rx.data.imu.Gyro.Y;
imu_msg.angular_velocity.z = wit_imu_.rx.data.imu.Gyro.Z;
imu_pub_->publish(imu_msg);
valid_data = false;
}
}
// 继续监听新的数据
async_receive_message();
}
);
}
};
int main(int argc, char ** argv)
{
rclcpp::init(argc,argv);
auto node = std::make_shared<ImuNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
這種框架對於一個250Hz（每4ms輸出一次）的陀螺儀來說,最後/imu的發佈頻率是100Hz（每10ms發佈一次）,效率也是還可以接受的.
