消除重力加速度的影响
- 受力分析: 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="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>
运行命令为: ros2 run my_imu_processing_pkg imu_processing_node