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

编程语言 2023-12-17 06:05:36 阅读次数: 0

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

消除重力加速度的影响

  • 受力分析: IMU静止时，与IMU所在平面会有一个向上的支持力（重力反作用力），IMU就会测量的角速度就是反作用力带来的。
  • 坐标关系:IMU在平面或者斜坡，IMU的Z轴都或多或少与重力有一个夹角，因此重力的反作用力(支持力)会按照夹角分配到IMU各个轴上，我们就是将与重力坐标系转到IMU坐标系，如下图所示黑线坐标系转到红线坐标系，图中只展示了三维坐标系的二维形式。
  • 消除重力：将IMU坐标系得到的值减去重力在每个轴上的分量，就能消除重力对加速度的影响。
    在这里插入图片描述
  • 分析已知变量, IMU能获得的数据有三维的加速度以及三维的欧拉角。其中欧拉角就是重力坐标系与IMU坐标系三轴之间的夹角，因此通过欧拉角，按照z、y、x顺序，就能将重力坐标系旋转到IMU坐标系。
    R I G = R z ⋅ R y ⋅ R z R^{G}_{I} = R_{z} \cdot R_{y} \cdot R_{z} RIG​=Rz​⋅Ry​⋅Rz​
    其中G表示重力坐标系，也有用W表示的世界坐标系，I为IMU坐标系。 R I G R^{G}_{I} RIG​表示从重力坐标系(World)到IMU系的旋转矩阵。
    在这里插入图片描述
    所以将重力加速度转到IMU坐标系下的公式为
    G r a v i t y I ⃗ = R I ⃗ ⋅ G r a v i t y \vec{Gravity_{I}} = \vec{R_{I}} \cdot Gravity GravityI​ ​=RI​ ​⋅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 here
        imu_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 values
        RCLCPP_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 trajectory
        double 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 values
            RCLCPP_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 trajectory
            nav_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 values
            tf2::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 trajectory
            trajectory_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(dependencies
  rclcpp
  sensor_msgs
  std_msgs
  std_srvs
  tf2
  nav_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_node
  DESTINATION lib/${
    
    PROJECT_NAME}
)

install(
  DIRECTORY launch
  DESTINATION 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 files
  set(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 files
  set(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="[email protected]">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>

运行命令为： ros2 run my_imu_processing_pkg imu_processing_node

猜你喜欢

转载自blog.csdn.net/CCCrunner/article/details/132512060
今日推荐
《美国对全球网络空间安全与发展的威胁和破坏》报告发布
火速冲上 GitHub 热榜 —— 开源编程语言、框架哪有这么可爱？
北京人形机器人创新中心发布全球首个纯电驱拟人奔跑的全尺寸人形机器人“天工”
LFOSSA 源来如此公开课 | 掌握云原生未来：CNCF 认证全面攻略与备考秘籍
周排行
循环神经网络（rnn）讲解
Tigao教程四：单独的关节运动
金蝶K3WISE15.0-注册套打教程
如何在Mac上配置Kubernetes
Android应用结束自身进程的方法
SpringMVC学习 十三 拦截器栈
中国驻洛杉矶总领馆举行新春招待会
HttpClient get post 发送
11 - three.js 笔记 - 绘制三维字体模型
Mysql递归获取某个父节点下面的所有子节点和子节点上的所有父节点
每日归档
更多
2024-05-01(4)
2024-04-30(1)
2024-04-29(40)
2024-04-28(0)
2024-04-27(56)
2024-04-26(39)
2024-04-25(22)
2024-04-24(36)
2024-04-23(26)
2024-04-22(39)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022