基于ROS2的消除重力加速度对IMU加速度影响，动态获取当前重力加速度。

IMU的全称是惯性测量单元，包括一个三轴的加速度计以及一个三轴的陀螺仪，分别测量出物体的加速度和角速度信息，不受周围环境结构、光照等外界因素影响。通常IMU的输出频率在100-1000hz之间，远高于相机或者激光雷达的输出频率

消除重力加速度的影响

受力分析: IMU静止时，与IMU所在平面会有一个向上的支持力（重力反作用力），IMU就会测量的角速度就是反作用力带来的。
坐标关系:IMU在平面或者斜坡，IMU的Z轴都或多或少与重力有一个夹角，因此重力的反作用力(支持力)会按照夹角分配到IMU各个轴上，我们就是将与重力坐标系转到IMU坐标系，如下图所示黑线坐标系转到红线坐标系，图中只展示了三维坐标系的二维形式。
消除重力：将IMU坐标系得到的值减去重力在每个轴上的分量，就能消除重力对加速度的影响。
分析已知变量, IMU能获得的数据有三维的加速度以及三维的欧拉角。其中欧拉角就是重力坐标系与IMU坐标系三轴之间的夹角，因此通过欧拉角，按照z、y、x顺序，就能将重力坐标系旋转到IMU坐标系。
$R^{G}_{I} = R_{z} \cdot R_{y} \cdot R_{z}$
其中G表示重力坐标系，也有用W表示的世界坐标系，I为IMU坐标系。 $R^{G}_{I}$ 表示从重力坐标系(World)到IMU系的旋转矩阵。

所以将重力加速度转到IMU坐标系下的公式为
$\vec{Gravity_{I}} = \vec{R_{I}} \cdot Gravity$ ，其中Gravity是常量，我们设Gravity为9.8左右(后面会用梯度下降算法来求得误差最小的重力加速度)。
然后用获取的IMU三轴的加速度减去转换到IMU坐标系的加速度得到绝对的加速度（相对于地面静止）。代码如下:

   float accX = msg->linear_acceleration.y - std::sin(roll) * std::cos(pitch) * gravity_;float accY = msg->linear_acceleration.z - std::cos(roll) * std::cos(pitch) * gravity_;float accZ = msg->linear_acceleration.x + std::sin(pitch) * gravity_;

梯度下降算法求得当地的重力加速度

首先设置一个初始重力加速度，可以设置的大一些，然后设置一个学习率，通过当前误差与理论误差的差为梯度，让初始值沿着梯度下降的方向求得误差，如果误差小于一定的值，那么就保留当前的值。

如下图所示

IMU与水平面有一定的夹角。因此下面的加速度分别是 4.874595 、 -0.502782 、 8.432.378
如果IMU与水平面平行，理论上的值应该是(0,0,9)
因为当前IMU在斜坡上，理论加速度应该为(0,0,0) ，但实际上为（4.874595 ， -0.502782 ， 8.432.378）。我们要做的就是消除重力的影响。
在这里插入图片描述

在这里插入图片描述
具体代码如下:
imu_processing_node.cpp

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/convert.h>
#include <nav_msgs/msg/path.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <iostream>
#include <vector>
#include <mutex>// 在类中添加私有互斥锁成员
std::mutex mutex_;class ImuProcessingNode : public rclcpp::Node
{
public:ImuProcessingNode() : Node("imu_processing_node"){std::cout << "ImuProcessingNode" << std::endl;imu_subscriber_ = this->create_subscription<sensor_msgs::msg::Imu>("/imu/data_raw", 10, std::bind(&ImuProcessingNode::imuCallback, this, std::placeholders::_1));// imu_subscriber_ = this->create_subscription<sensor_msgs::msg::Imu>(//     "/imu/data_raw", 10, std::bind(&ImuProcessingNode::calculateCorrectedLinearAcceleration, this, std::placeholders::_1));trajectory_publisher_ = this->create_publisher<nav_msgs::msg::Path>( // Use Path message type"integrated_trajectory", 10);// Initialize necessary variables hereimu_time_.reserve(imu_que_length_);imu_roll_.reserve(imu_que_length_);imu_pitch_.reserve(imu_que_length_);imu_yaw_.reserve(imu_que_length_);imu_acc_x_.reserve(imu_que_length_);imu_acc_y_.reserve(imu_que_length_);imu_acc_z_.reserve(imu_que_length_);imu_angular_velo_x_.reserve(imu_que_length_);imu_angular_velo_y_.reserve(imu_que_length_);imu_angular_velo_z_.reserve(imu_que_length_);imu_shift_x_.reserve(imu_que_length_);imu_shift_y_.reserve(imu_que_length_);imu_shift_z_.reserve(imu_que_length_);imu_velo_x_.reserve(imu_que_length_);imu_velo_y_.reserve(imu_que_length_);imu_velo_z_.reserve(imu_que_length_);imu_angular_rotation_x_.reserve(imu_que_length_);imu_angular_rotation_y_.reserve(imu_que_length_);imu_angular_rotation_z_.reserve(imu_que_length_);}private:double sumXYZ(double x, double y, double z){// 计算平方和double squared_sum = x * x + y * y + z * z;return squared_sum;}// 在 ImuProcessingNode 类中添加一个新的私有函数来计算矫正后的线性加速度的平方和void calculateCorrectedLinearAcceleration(const sensor_msgs::msg::Imu::SharedPtr msg){// 使用互斥锁保护代码块{std::lock_guard<std::mutex> lock(mutex_); // 加锁gravity_ = 9.9;double roll, pitch, yaw;tf2::Quaternion orientation;tf2::fromMsg(msg->orientation, orientation);tf2::Matrix3x3(orientation).getRPY(roll, pitch, yaw);float accX = msg->linear_acceleration.y - std::sin(roll) * std::cos(pitch) * gravity_;float accY = msg->linear_acceleration.z - std::cos(roll) * std::cos(pitch) * gravity_;float accZ = msg->linear_acceleration.x + std::sin(pitch) * gravity_;// 优化步骤double target_squared_sum = 0.002; // 目标平方和double current_squared_sum = sumXYZ(accX, accY, accZ);std::cout << "current_squared_sum::" << current_squared_sum << std::endl;int count = 0;double zhi = gravity_;double sum = 100.0;while (current_squared_sum > target_squared_sum) // 当平方和大于目标值时继续优化{// 使用梯度下降更新 gravity_double learning_rate = 0.01;                                // 学习率double gradient = current_squared_sum - target_squared_sum; // 梯度gravity_ -= learning_rate * gradient;                       // 更新 gravity_// std::cout<<"计算次数::"<<count<<"gravity_::"<<gravity_<<" gradient::"<<gradient <<" current_squared_sum::"<<current_squared_sum<<std::endl;accX = msg->linear_acceleration.y - std::sin(roll) * std::cos(pitch) * gravity_;accY = msg->linear_acceleration.z - std::cos(roll) * std::cos(pitch) * gravity_;accZ = msg->linear_acceleration.x + std::sin(pitch) * gravity_;count = count + 1;if (sum > current_squared_sum){sum = current_squared_sum;zhi = gravity_;}if (count > 55000){break;}// // 重新计算矫正后的线性加速度平方和current_squared_sum = sumXYZ(accX, accY, accZ);}if (sum < 0.0021){imu_handler = true;std::cout << "重力优化"<< "error::" << sum << "gravity_::" << zhi << std::endl;gravity_ = zhi;}else{std::cout << "error::" << sum << "gravity_::" << zhi << std::endl;}} // 自动解锁}void AccumulateIMUShiftAndRotation(){float roll = imu_roll_[imu_pointer_last_];float pitch = imu_pitch_[imu_pointer_last_];float yaw = imu_yaw_[imu_pointer_last_];float accX = imu_acc_x_[imu_pointer_last_];float accY = imu_acc_y_[imu_pointer_last_];float accZ = imu_acc_z_[imu_pointer_last_];float x1 = std::cos(roll) * accX - std::sin(roll) * accY;float y1 = std::sin(roll) * accX + std::cos(roll) * accY;float z1 = accZ;float x2 = x1;float y2 = std::cos(pitch) * y1 - std::sin(pitch) * z1;float z2 = std::sin(pitch) * y1 + std::cos(pitch) * z1;accX = std::cos(yaw) * x2 + std::sin(yaw) * z2;accY = y2;accZ = -std::sin(yaw) * x2 + std::cos(yaw) * z2;std::cout<<"accX::"<<accX<<" accY::"<<accY<<" accZ::"<<accZ <<std::endl;int imuPointerBack = (imu_pointer_last_ + imu_que_length_ - 1) % imu_que_length_;double timeDiff = imu_time_[imu_pointer_last_] - imu_time_[imuPointerBack];// std::cout<<"timeDiff::"<<timeDiff <<" imu_pointer_last_:"<<imu_pointer_last_<<" imuPointerBack:"<<imuPointerBack<<std::endl;if (timeDiff < scanPeriod){imu_shift_x_[imu_pointer_last_] = imu_shift_x_[imuPointerBack] + imu_velo_x_[imuPointerBack] * timeDiff + accX * timeDiff * timeDiff / 2;imu_shift_y_[imu_pointer_last_] = imu_shift_y_[imuPointerBack] + imu_velo_y_[imuPointerBack] * timeDiff + accY * timeDiff * timeDiff / 2;imu_shift_z_[imu_pointer_last_] = imu_shift_z_[imuPointerBack] + imu_velo_z_[imuPointerBack] * timeDiff + accZ * timeDiff * timeDiff / 2;imu_velo_x_[imu_pointer_last_] = imu_velo_x_[imuPointerBack] + accX * timeDiff;imu_velo_y_[imu_pointer_last_] = imu_velo_y_[imuPointerBack] + accY * timeDiff;imu_velo_z_[imu_pointer_last_] = imu_velo_z_[imuPointerBack] + accZ * timeDiff;imu_angular_rotation_x_[imu_pointer_last_] = imu_angular_rotation_x_[imuPointerBack] + imu_angular_velo_x_[imuPointerBack] * timeDiff;imu_angular_rotation_y_[imu_pointer_last_] = imu_angular_rotation_y_[imuPointerBack] + imu_angular_velo_y_[imuPointerBack] * timeDiff;imu_angular_rotation_z_[imu_pointer_last_] = imu_angular_rotation_z_[imuPointerBack] + imu_angular_velo_z_[imuPointerBack] * timeDiff;}}void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg){// Print original IMU valuesRCLCPP_INFO(this->get_logger(), "Original IMU Orientation: (%f, %f, %f, %f)",msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w);RCLCPP_INFO(this->get_logger(), "Original IMU Linear Acceleration: (%f, %f, %f)",msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z);RCLCPP_INFO(this->get_logger(), "Original IMU Angular Velocity: (%f, %f, %f)",msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z);// Process IMU data, remove gravity influence, integrate, and publish trajectorydouble roll, pitch, yaw;tf2::Quaternion orientation;tf2::fromMsg(msg->orientation, orientation);tf2::Matrix3x3(orientation).getRPY(roll, pitch, yaw);if (!imu_handler){calculateCorrectedLinearAcceleration(msg);}else{float accX = msg->linear_acceleration.y - std::sin(roll) * std::cos(pitch) * gravity_;float accY = msg->linear_acceleration.z - std::cos(roll) * std::cos(pitch) * gravity_;float accZ = msg->linear_acceleration.x + std::sin(pitch) * gravity_;imu_pointer_last_ = (imu_pointer_last_ + 1) % imu_que_length_;imu_time_[imu_pointer_last_] = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9;imu_roll_[imu_pointer_last_] = roll;imu_pitch_[imu_pointer_last_] = pitch;imu_yaw_[imu_pointer_last_] = yaw;imu_acc_x_[imu_pointer_last_] = accX;imu_acc_y_[imu_pointer_last_] = accY;imu_acc_z_[imu_pointer_last_] = accZ;imu_angular_velo_x_[imu_pointer_last_] = msg->angular_velocity.x;imu_angular_velo_y_[imu_pointer_last_] = msg->angular_velocity.y;imu_angular_velo_z_[imu_pointer_last_] = msg->angular_velocity.z;// Print transformed IMU valuesRCLCPP_INFO(this->get_logger(), "Transformed IMU RPY: (%f, %f, %f)", roll, pitch, yaw);RCLCPP_INFO(this->get_logger(), "Transformed IMU Linear Acceleration: (%f, %f, %f)", accX, accY, accZ);AccumulateIMUShiftAndRotation();// Update variables and publish trajectory using trajectory_publisher_// Publish integrated trajectorynav_msgs::msg::Path integrated_trajectory;integrated_trajectory.header = msg->header;integrated_trajectory.header.frame_id = "base_link";geometry_msgs::msg::PoseStamped integrated_pose;integrated_pose.header = msg->header;integrated_pose.pose.position.x = imu_shift_x_[imu_pointer_last_];integrated_pose.pose.position.y = imu_shift_y_[imu_pointer_last_];integrated_pose.pose.position.z = imu_shift_z_[imu_pointer_last_];// Assuming imu_angular_rotation_x_, imu_angular_rotation_y_, imu_angular_rotation_z_// contain the integrated orientation valuestf2::Quaternion orientation1;orientation1.setRPY(imu_angular_rotation_x_[imu_pointer_last_],imu_angular_rotation_y_[imu_pointer_last_],imu_angular_rotation_z_[imu_pointer_last_]);integrated_pose.pose.orientation = tf2::toMsg(orientation1);std::cout<<"x:"<<integrated_pose.pose.position.x<<" y:"<<integrated_pose.pose.position.y<<" z:"<<integrated_pose.pose.position.z<<std::endl;integrated_trajectory.poses.push_back(integrated_pose); // Add the pose to the trajectorytrajectory_publisher_->publish(integrated_trajectory);}}rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscriber_;rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr trajectory_publisher_;bool imu_handler = false;int imu_pointer_last_ = 0;float gravity_ = 9.8;const int imu_que_length_ = 10;double scanPeriod = 0.1;std::vector<double> imu_time_;std::vector<double> imu_roll_;std::vector<double> imu_pitch_;std::vector<double> imu_yaw_;std::vector<float> imu_acc_x_;std::vector<float> imu_acc_y_;std::vector<float> imu_acc_z_;std::vector<float> imu_angular_velo_x_;std::vector<float> imu_angular_velo_y_;std::vector<float> imu_angular_velo_z_;std::vector<double> imu_shift_x_;std::vector<double> imu_shift_y_;std::vector<double> imu_shift_z_;std::vector<float> imu_velo_x_;std::vector<float> imu_velo_y_;std::vector<float> imu_velo_z_;std::vector<float> imu_angular_rotation_x_;std::vector<float> imu_angular_rotation_y_;std::vector<float> imu_angular_rotation_z_;
};int main(int argc, char **argv)
{// RCLCPP_INFO("begin", "Start IMU");std::cout << "Start IMU" << std::endl;rclcpp::init(argc, argv);auto node = std::make_shared<ImuProcessingNode>();rclcpp::spin(node);std::cout << "End IMU" << std::endl;rclcpp::shutdown();return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(my_imu_processing_pkg)if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")add_compile_options(-Wall -Wextra -Wpedantic)
endif()# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(std_srvs REQUIRED)
find_package(tf2 REQUIRED)
find_package(nav_msgs REQUIRED)set(dependenciesrclcppsensor_msgsstd_msgsstd_srvstf2nav_msgs
)include_directories(include)add_executable(imu_processing_node"src/imu_processing_node.cpp"
)ament_target_dependencies(imu_processing_node ${dependencies})install(TARGETS imu_processing_nodeDESTINATION lib/${PROJECT_NAME}
)install(DIRECTORY launchDESTINATION share/${PROJECT_NAME}
)if(BUILD_TESTING)find_package(ament_lint_auto REQUIRED)# the following line skips the linter which checks for copyrights# comment the line when a copyright and license is added to all source filesset(ament_cmake_copyright_FOUND TRUE)# the following line skips cpplint (only works in a git repo)# comment the line when this package is in a git repo and when# a copyright and license is added to all source filesset(ament_cmake_cpplint_FOUND TRUE)ament_lint_auto_find_test_dependencies()
endif()ament_package()

package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3"><name>my_imu_processing_pkg</name><version>0.0.0</version><description>TODO: Package description</description><maintainer email="2267507789@qq.com">philtell</maintainer><license>TODO: License declaration</license><buildtool_depend>ament_cmake</buildtool_depend><depend>rclcpp</depend><depend>sensor_msgs</depend><depend>std_msgs</depend><depend>std_srvs</depend><depend>tf2</depend><depend>nav_msgs</depend><test_depend>ament_lint_auto</test_depend><test_depend>ament_lint_common</test_depend><export><build_type>ament_cmake</build_type></export>
</package>