企业官网建设流程全解析

网站做广告投放 要求做效果评估,百度导航和百度地图,做网站怎么弄,驾校推广网络营销方案在 ROS Melodic 环境下#xff0c;想要用 Python 实现读取/imu/data话题、完成惯性导航航迹推算#xff08;含静态初始化#xff09;#xff0c;并在 RViz 中实时可视化位姿轨迹一、环境依赖编译# 核心ROS依赖 sudo apt-get install ros-melodic-ros-python ros-melodic-se…在 ROS Melodic 环境下想要用 Python 实现读取/imu/data话题、完成惯性导航航迹推算含静态初始化并在 RViz 中实时可视化位姿轨迹一、环境依赖编译# 核心ROS依赖 sudo apt-get install ros-melodic-ros-python ros-melodic-sensor-msgs ros-melodic-nav-msgs ros-melodic-tf2-ros # 数值计算依赖Python2.7需手动安装 pip2 install numpy transforms3d新建 ROS 工作空间和功能包# 创建工作空间 mkdir -p ~/imu_py_ws/src cd ~/imu_py_ws/src # 新建Python功能包依赖rospy、sensor_msgs等 catkin_create_pkg imu_pose_estimation rospy sensor_msgs nav_msgs tf2_ros tf2_py geometry_msgs # 编译工作空间 cd ~/imu_py_ws catkin_make source devel/setup.bash在imu_pose_estimation/scripts目录下新建imu_inertial_nav.py添加可执行权限chmod x imu_inertial_nav.py代码如下#!/usr/bin/env python2 # -*- coding: utf-8 -*- import rospy import numpy as np from transforms3d.quaternions import quat2mat, mat2quat, qmult from sensor_msgs.msg import Imu from nav_msgs.msg import Odometry from geometry_msgs.msg import TransformStamped, Quaternion, Point, Vector3 import tf2_ros class IMUInertialNavigation: def __init__(self): # 1. 初始化ROS节点和参数 rospy.init_node(imu_inertial_navigation, anonymousTrue) self.rate rospy.Rate(100) # 与IMU采样率匹配默认100Hz # 2. 配置参数 self.static_calib_samples 200 # 静态初始化采集样本数 self.calib_count 0 # 已采集校准样本数 self.is_calibrated False # 是否完成静态初始化 # 3. IMU零偏和重力校准存储 self.gyro_bias np.zeros(3) # 陀螺仪零偏 [x, y, z] self.accel_bias np.zeros(3) # 加速度计零偏 [x, y, z] self.gyro_calib_buffer [] # 陀螺仪校准缓冲区 self.accel_calib_buffer [] # 加速度计校准缓冲区 # 4. 航迹推算状态量世界坐标系ENU初始原点为静态初始化位置 self.pose np.zeros(3) # 位置 [x, y, z] self.velocity np.zeros(3) # 速度 [vx, vy, vz] self.quaternion np.array([1.0, 0.0, 0.0, 0.0]) # 姿态四元数 [w, x, y, z] self.last_time None # 上一帧IMU时间戳 # 5. 发布器初始化用于RViz可视化 self.odom_pub rospy.Publisher(/imu_odom, Odometry, queue_size10) self.tf_broadcaster tf2_ros.TransformBroadcaster() # 6. 订阅IMU话题 rospy.Subscriber(/imu/data, Imu, self.imu_callback) rospy.loginfo(IMU惯性导航节点已启动等待静态初始化保持设备静止...) def imu_callback(self, imu_msg): IMU话题回调函数处理数据采集、校准和航迹推算 # 提取IMU原始数据转为numpy数组 gyro_raw np.array([ imu_msg.angular_velocity.x, imu_msg.angular_velocity.y, imu_msg.angular_velocity.z ]) accel_raw np.array([ imu_msg.linear_acceleration.x, imu_msg.linear_acceleration.y, imu_msg.linear_acceleration.z ]) # 步骤1静态初始化采集样本校准零偏和重力 if not self.is_calibrated: self.static_calibration(gyro_raw, accel_raw, imu_msg.header.stamp) return # 步骤2航迹推算仅在校准完成后执行 current_time imu_msg.header.stamp.to_sec() if self.last_time is None: self.last_time current_time return # 计算时间差dt dt current_time - self.last_time self.last_time current_time # 去除IMU零偏 gyro_corrected gyro_raw - self.gyro_bias accel_corrected accel_raw - self.accel_bias # 步骤3姿态更新四元数积分角速度 self.update_attitude(gyro_corrected, dt) # 步骤4速度更新载体加速度转世界坐标系积分得速度 self.update_velocity(accel_corrected, dt) # 步骤5位置更新积分速度得位置 self.update_position(dt) # 步骤6发布里程计和TF用于RViz可视化 self.publish_odom_and_tf(imu_msg.header.stamp) def static_calibration(self, gyro_raw, accel_raw, stamp): 静态初始化采集样本计算陀螺仪/加速度计零偏校准重力 if self.calib_count self.static_calib_samples: # 存储样本到缓冲区 self.gyro_calib_buffer.append(gyro_raw) self.accel_calib_buffer.append(accel_raw) self.calib_count 1 # 打印校准进度 if self.calib_count % 50 0: rospy.loginfo(静态初始化进度%d/%d, self.calib_count, self.static_calib_samples) return # 校准完成计算零偏取均值 self.gyro_bias np.mean(np.array(self.gyro_calib_buffer), axis0) self.accel_bias np.mean(np.array(self.accel_calib_buffer), axis0) # 校准重力加速度静止时加速度计测量值重力加速度去除零偏后归一化 accel_mean np.mean(np.array(self.accel_calib_buffer), axis0) gravity_vec accel_mean - self.accel_bias self.gravity_magnitude np.linalg.norm(gravity_vec) # 初始化姿态四元数静止时载体z轴与重力方向相反对齐世界坐标系ENU # 简化假设静止时载体水平姿态为单位四元数可根据实际需求优化 self.quaternion np.array([1.0, 0.0, 0.0, 0.0]) # 标记校准完成 self.is_calibrated True self.last_time stamp.to_sec() rospy.loginfo(静态初始化完成) rospy.loginfo(陀螺仪零偏[%.6f, %.6f, %.6f], *self.gyro_bias) rospy.loginfo(加速度计零偏[%.6f, %.6f, %.6f], *self.accel_bias) rospy.loginfo(重力加速度幅值%.6f m/s², self.gravity_magnitude) def update_attitude(self, gyro, dt): 姿态更新通过四元数积分角速度一阶欧拉法 # 角速度转四元数增量 omega np.array([0, gyro[0], gyro[1], gyro[2]]) # [0, wx, wy, wz] q_dot 0.5 * qmult(self.quaternion, omega) # 积分更新四元数 self.quaternion q_dot * dt # 四元数归一化避免数值漂移 self.quaternion / np.linalg.norm(self.quaternion) def update_velocity(self, accel, dt): 速度更新载体加速度转世界坐标系去除重力积分得速度 # 1. 四元数转旋转矩阵载体坐标系→世界坐标系 rot_mat quat2mat(self.quaternion) # 2. 载体加速度转世界坐标系 accel_world np.dot(rot_mat, accel) # 3. 去除重力影响世界坐标系z轴为重力反方向简化为减去[0,0,g] accel_world - np.array([0, 0, self.gravity_magnitude]) # 4. 积分更新速度一阶欧拉法 self.velocity accel_world * dt def update_position(self, dt): 位置更新积分速度得世界坐标系位置 self.pose self.velocity * dt def publish_odom_and_tf(self, stamp): 发布里程计消息和TF变换用于RViz可视化 # 1. 构造Odometry消息 odom_msg Odometry() odom_msg.header.stamp stamp odom_msg.header.frame_id odom # 世界坐标系 odom_msg.child_frame_id base_link # 载体坐标系 # 2. 设置位姿信息 odom_msg.pose.pose.position Point(self.pose[0], self.pose[1], self.pose[2]) odom_msg.pose.pose.orientation Quaternion( xself.quaternion[1], yself.quaternion[2], zself.quaternion[3], wself.quaternion[0] ) # 3. 设置速度信息 odom_msg.twist.twist.linear Vector3(self.velocity[0], self.velocity[1], self.velocity[2]) # 4. 发布里程计 self.odom_pub.publish(odom_msg) # 5. 构造并发布TF变换 tf_msg TransformStamped() tf_msg.header.stamp stamp tf_msg.header.frame_id odom tf_msg.child_frame_id base_link tf_msg.transform.translation.x self.pose[0] tf_msg.transform.translation.y self.pose[1] tf_msg.transform.translation.z self.pose[2] tf_msg.transform.rotation Quaternion( xself.quaternion[1], yself.quaternion[2], zself.quaternion[3], wself.quaternion[0] ) self.tf_broadcaster.sendTransform(tf_msg) def run(self): 节点主循环 while not rospy.is_shutdown(): self.rate.sleep() if __name__ __main__: try: imu_nav IMUInertialNavigation() imu_nav.run() except rospy.ROSInterruptException: pass代码核心模块解读静态初始化模块static_calibration要求设备保持静止采集 200 组 IMU 数据计算陀螺仪 / 加速度计零偏样本均值消除静态噪声校准重力加速度幅值为后续速度更新去除重力影响做准备初始化姿态四元数为单位四元数确保初始姿态对齐世界坐标系。姿态更新模块update_attitude采用四元数积分方式更新姿态避免欧拉角的万向锁问题对去除零偏后的角速度进行一阶欧拉法积分更新四元数四元数归一化避免数值计算导致的漂移。速度 / 位置更新模块先通过旋转矩阵将载体坐标系加速度映射到世界坐标系ENU去除重力影响静止时加速度计仅测量重力避免重力导致的速度漂移采用一阶欧拉法积分分别得到速度和位置简化实现高精度场景可改用龙格 - 库塔法。RViz 可视化模块publish_odom_and_tf发布nav_msgs/Odometry话题提供位姿和速度信息发布tf2_ros变换建立odom世界→base_link载体的坐标系关系消息格式完全兼容 RViz可直接可视化轨迹和坐标系。二、启动并配置# 新开终端3进入工作空间加载环境变量 cd ~/imu_py_ws source devel/setup.bash # 启动Python节点保持设备/Gazebo模型静止完成静态初始化 rosrun imu_pose_estimation imu_inertial_nav.py终端会打印静态初始化进度完成后提示 “静态初始化完成之前的代码Z轴漂移严重限制Z轴的代码如下#!/usr/bin/env python2 # -*- coding: utf-8 -*- import rospy import numpy as np from transforms3d.quaternions import quat2mat, mat2quat, qmult from sensor_msgs.msg import Imu from nav_msgs.msg import Odometry, Path # 新增导入Path消息 from geometry_msgs.msg import TransformStamped, Quaternion, Point, Vector3, PoseStamped # 新增导入PoseStamped import tf2_ros class IMUInertialNavigation: def __init__(self): # 1. 初始化ROS节点和参数 rospy.init_node(imu_inertial_navigation, anonymousTrue) self.rate rospy.Rate(100) # 与IMU采样率匹配默认100Hz # 2. 配置参数 self.static_calib_samples 200 # 静态初始化采集样本数 self.calib_count 0 # 已采集校准样本数 self.is_calibrated False # 是否完成静态初始化 self.max_path_length 1000 # 最大路径缓存长度避免内存溢出 # 3. IMU零偏和重力校准存储 self.gyro_bias np.zeros(3) # 陀螺仪零偏 [x, y, z] self.accel_bias np.zeros(3) # 加速度计零偏 [x, y, z] self.gyro_calib_buffer [] # 陀螺仪校准缓冲区 self.accel_calib_buffer [] # 加速度计校准缓冲区 # 4. 航迹推算状态量世界坐标系ENU初始原点为静态初始化位置 self.pose np.zeros(3) # 位置 [x, y, z] self.velocity np.zeros(3) # 速度 [vx, vy, vz] self.quaternion np.array([1.0, 0.0, 0.0, 0.0]) # 姿态四元数 [w, x, y, z] self.last_time None # 上一帧IMU时间戳 # 5. 发布器初始化用于RViz可视化 self.odom_pub rospy.Publisher(/imu_odom, Odometry, queue_size10) self.tf_broadcaster tf2_ros.TransformBroadcaster() # 新增Path发布器和路径缓存 self.path_pub rospy.Publisher(/imu_path, Path, queue_size100) # 发布/imu_path话题 self.path_msg Path() # 缓存历史路径的Path消息 self.path_msg.header.frame_id odom # 路径的参考坐标系与Odometry一致 # 6. 订阅IMU话题 rospy.Subscriber(/imu/data, Imu, self.imu_callback) # 修正重力幅值默认初始化避免为0 self.gravity_magnitude 9.81 rospy.loginfo(IMU惯性导航节点已启动等待静态初始化保持设备静止...) def imu_callback(self, imu_msg): IMU话题回调函数处理数据采集、校准和航迹推算 # 提取IMU原始数据转为numpy数组 gyro_raw np.array([ imu_msg.angular_velocity.x, imu_msg.angular_velocity.y, imu_msg.angular_velocity.z ]) accel_raw np.array([ imu_msg.linear_acceleration.x, imu_msg.linear_acceleration.y, imu_msg.linear_acceleration.z ]) # 步骤1静态初始化采集样本校准零偏和重力 if not self.is_calibrated: self.static_calibration(gyro_raw, accel_raw, imu_msg.header.stamp) return # 步骤2航迹推算仅在校准完成后执行 current_time imu_msg.header.stamp.to_sec() if self.last_time is None: self.last_time current_time return # 计算时间差dt dt current_time - self.last_time self.last_time current_time # 去除IMU零偏 gyro_corrected gyro_raw - self.gyro_bias accel_corrected accel_raw - self.accel_bias # 步骤3姿态更新四元数积分角速度 self.update_attitude(gyro_corrected, dt) # 步骤4速度更新载体加速度转世界坐标系积分得速度 self.update_velocity(accel_corrected, dt) # 步骤5位置更新积分速度得位置 self.update_position(dt) # 步骤6发布里程计、TF和Path用于RViz可视化 self.publish_odom_and_tf(imu_msg.header.stamp) def static_calibration(self, gyro_raw, accel_raw, stamp): 静态初始化采集样本计算陀螺仪/加速度计零偏校准重力 if self.calib_count self.static_calib_samples: # 存储样本到缓冲区 self.gyro_calib_buffer.append(gyro_raw) self.accel_calib_buffer.append(accel_raw) self.calib_count 1 # 打印校准进度 if self.calib_count % 50 0: rospy.loginfo(静态初始化进度%d/%d, self.calib_count, self.static_calib_samples) return # 校准完成计算零偏取均值 self.gyro_bias np.mean(np.array(self.gyro_calib_buffer), axis0) self.accel_bias np.mean(np.array(self.accel_calib_buffer), axis0) # 修正重力加速度幅值直接使用标准值更稳定 accel_mean np.mean(np.array(self.accel_calib_buffer), axis0) gravity_vec accel_mean self.gravity_magnitude np.linalg.norm(gravity_vec) # 兜底避免重力幅值为0 if self.gravity_magnitude 9.0: self.gravity_magnitude 9.81 # 初始化姿态四元数静止时载体z轴与重力方向相反对齐世界坐标系ENU self.quaternion np.array([1.0, 0.0, 0.0, 0.0]) # 标记校准完成 self.is_calibrated True self.last_time stamp.to_sec() rospy.loginfo(静态初始化完成) rospy.loginfo(陀螺仪零偏[%.6f, %.6f, %.6f], *self.gyro_bias) rospy.loginfo(加速度计零偏[%.6f, %.6f, %.6f], *self.accel_bias) rospy.loginfo(重力加速度幅值%.6f m/s², self.gravity_magnitude) def update_attitude(self, gyro, dt): 姿态更新通过四元数积分角速度一阶欧拉法 # 角速度转四元数增量 omega np.array([0, gyro[0], gyro[1], gyro[2]]) # [0, wx, wy, wz] q_dot 0.5 * qmult(self.quaternion, omega) # 积分更新四元数 self.quaternion q_dot * dt # 四元数归一化避免数值漂移 self.quaternion / np.linalg.norm(self.quaternion) def update_velocity(self, accel, dt): 速度更新载体加速度转世界坐标系去除重力积分得速度 # 1. 四元数转旋转矩阵载体坐标系→世界坐标系 rot_mat quat2mat(self.quaternion) # 2. 载体加速度转世界坐标系 accel_world np.dot(rot_mat, accel) # 3. 去除重力影响世界坐标系z轴为重力反方向 accel_world np.array([0, 0, self.gravity_magnitude]) # 4. 积分更新速度一阶欧拉法 self.velocity accel_world * dt # 新增强制约束Z轴速度为0仅平面运动场景 self.velocity[2] 0.0 # Z轴速度直接置0无垂直方向运动分量 def update_position(self, dt): 位置更新积分速度得世界坐标系位置 self.pose self.velocity * dt # 新增强制约束Z轴位置为0固定在初始高度杜绝Z轴漂移 self.pose[2] 0.0 # Z轴位置固定在初始原点无垂直方向位移 def publish_odom_and_tf(self, stamp): 发布里程计消息、TF变换和Path路径用于RViz可视化 # 1. 构造Odometry消息 odom_msg Odometry() odom_msg.header.stamp stamp odom_msg.header.frame_id odom # 世界坐标系 odom_msg.child_frame_id base_link # 载体坐标系 # 2. 设置位姿信息 odom_msg.pose.pose.position Point(self.pose[0], self.pose[1], self.pose[2]) odom_msg.pose.pose.orientation Quaternion( xself.quaternion[1], yself.quaternion[2], zself.quaternion[3], wself.quaternion[0] ) # 3. 设置速度信息 odom_msg.twist.twist.linear Vector3(self.velocity[0], self.velocity[1], self.velocity[2]) # 4. 发布里程计 self.odom_pub.publish(odom_msg) # 5. 构造并发布TF变换 tf_msg TransformStamped() tf_msg.header.stamp stamp tf_msg.header.frame_id odom tf_msg.child_frame_id base_link tf_msg.transform.translation.x self.pose[0] tf_msg.transform.translation.y self.pose[1] tf_msg.transform.translation.z self.pose[2] tf_msg.transform.rotation Quaternion( xself.quaternion[1], yself.quaternion[2], zself.quaternion[3], wself.quaternion[0] ) self.tf_broadcaster.sendTransform(tf_msg) # 6. 新增构造并发布Path消息历史航迹 pose_stamped PoseStamped() pose_stamped.header.stamp stamp pose_stamped.header.frame_id odom pose_stamped.pose odom_msg.pose.pose # 复用Odometry位姿保证数据一致性 # 7. 添加当前帧位姿到Path缓存限制最大长度避免内存溢出 self.path_msg.poses.append(pose_stamped) # if len(self.path_msg.poses) self.max_path_length: # self.path_msg.poses.pop(0) # 删除最旧的一帧 # 8. 更新Path消息时间戳并发布 self.path_msg.header.stamp stamp self.path_pub.publish(self.path_msg) def run(self): 节点主循环 while not rospy.is_shutdown(): self.rate.sleep() if __name__ __main__: try: imu_nav IMUInertialNavigation() imu_nav.run() except rospy.ROSInterruptException: pass播放数据包