宁波网站推广运营,免费咨询律师不收费,广州番禺地图全图,温州哪里有网站建设IMU的全称是惯性测量单元#xff0c;包括一个三轴的加速度计以及一个三轴的陀螺仪#xff0c;分别测量出物体的加速度和角速度信息#xff0c;不受周围环境结构、光照等外界因素影响。通常IMU的输出频率在100-1000hz之间#xff0c;远高于相机或者激光雷达的输出频率
消除…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} RIGRz⋅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_subscriptionsensor_msgs::msg::Imu(/imu/data_raw, 10, std::bind(ImuProcessingNode::imuCallback, this, std::placeholders::_1));// imu_subscriber_ this-create_subscriptionsensor_msgs::msg::Imu(// /imu/data_raw, 10, std::bind(ImuProcessingNode::calculateCorrectedLinearAcceleration, this, std::placeholders::_1));trajectory_publisher_ this-create_publishernav_msgs::msg::Path( // Use Path message typeintegrated_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_guardstd::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计算次数::countgravity_::gravity_ gradient::gradient current_squared_sum::current_squared_sumstd::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::coutaccX::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::couttimeDiff::timeDiff imu_pointer_last_:imu_pointer_last_ imuPointerBack:imuPointerBackstd::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::coutx:integrated_pose.pose.position.x y:integrated_pose.pose.position.y z:integrated_pose.pose.position.zstd::endl;integrated_trajectory.poses.push_back(integrated_pose); // Add the pose to the trajectorytrajectory_publisher_-publish(integrated_trajectory);}}rclcpp::Subscriptionsensor_msgs::msg::Imu::SharedPtr imu_subscriber_;rclcpp::Publishernav_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::vectordouble imu_time_;std::vectordouble imu_roll_;std::vectordouble imu_pitch_;std::vectordouble imu_yaw_;std::vectorfloat imu_acc_x_;std::vectorfloat imu_acc_y_;std::vectorfloat imu_acc_z_;std::vectorfloat imu_angular_velo_x_;std::vectorfloat imu_angular_velo_y_;std::vectorfloat imu_angular_velo_z_;std::vectordouble imu_shift_x_;std::vectordouble imu_shift_y_;std::vectordouble imu_shift_z_;std::vectorfloat imu_velo_x_;std::vectorfloat imu_velo_y_;std::vectorfloat imu_velo_z_;std::vectorfloat imu_angular_rotation_x_;std::vectorfloat imu_angular_rotation_y_;std::vectorfloat 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_sharedImuProcessingNode();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_nodesrc/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 version1.0?
?xml-model hrefhttp://download.ros.org/schema/package_format3.xsd schematypenshttp://www.w3.org/2001/XMLSchema?
package format3namemy_imu_processing_pkg/nameversion0.0.0/versiondescriptionTODO: Package description/descriptionmaintainer email2267507789qq.comphiltell/maintainerlicenseTODO: License declaration/licensebuildtool_dependament_cmake/buildtool_dependdependrclcpp/dependdependsensor_msgs/dependdependstd_msgs/dependdependstd_srvs/dependdependtf2/dependdependnav_msgs/dependtest_dependament_lint_auto/test_dependtest_dependament_lint_common/test_dependexportbuild_typeament_cmake/build_type/export
/package
运行命令为 ros2 run my_imu_processing_pkg imu_processing_node
