当前位置：首页 > news >正文

RealSense 相机 | 读取IMU | 解决权限问题 | 提供示例程序

news 2025/6/24 11:29:51

在使用 RealSense相机系列相机时，比如D435i、D455时，需要用到IMU数据；

但在访问IMU时，现实权限问题，无法正常读取。

本文先解决解决RealSense相机IMU访问权限问题，再分析读取IMU的示例程序。

1、解决RealSense相机IMU访问权限问题

报错信息：

Failed to open scan_element /sys/devices/pci0000:c0/0000:c0:07.1/0000:c4:00.4/usb2/2-1/2-1.2/2-1.2:1.5/0003:8086:0B3A.0007/HID-SENSOR-200076.3.auto/iio:device1/scan_elements/in_anglvel_x_en Last Error: Permission denied

程序启动失败是因为没有权限访问 RealSense 设备的 IMU（惯性测量单元）数据。

错误信息 Permission denied 清晰地表明，当前用户没有权限访问 /sys/devices/.../in_anglvel_x_en 这个系统文件。

在 Linux 系统里，访问硬件设备一般需要 root 权限或者相应的udev 规则支持。

解决方案：配置 udev 规则

这是推荐的做法，能够让普通用户也有权限访问 RealSense 设备。

# 下载RealSense的udev规则文件
wget https://raw.githubusercontent.com/IntelRealSense/librealsense/master/config/99-realsense-libusb.rules -O /etc/udev/rules.d/99-realsense-libusb.rules# 重新加载udev规则
sudo udevadm control --reload-rules 
sudo udevadm trigger# 拔出并重新插入RealSense设备

测试一下是否开启成功：

import pyrealsense2 as rs
import cv2
import numpy as np# 查询设备信息
ctx = rs.context()
devices = ctx.query_devices()
if len(devices) == 0:raise RuntimeError("未检测到 RealSense 设备")
device = devices[0]
sensors = device.query_sensors()
sensor_names = [s.get_info(rs.camera_info.name) for s in sensors]
print("设备传感器:", sensor_names)# 构建配置
pipeline = rs.pipeline()
config = rs.config()config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)try:config.enable_stream(rs.stream.infrared, 1, 640, 480, rs.format.y8, 30)config.enable_stream(rs.stream.infrared, 2, 640, 480, rs.format.y8, 30)
except Exception as e:print("IR流不支持：", e)try:config.enable_stream(rs.stream.accel, rs.format.motion_xyz32f, 200)config.enable_stream(rs.stream.gyro, rs.format.motion_xyz32f, 200)
except Exception as e:print("IMU流不支持：", e)# 启动
try:profile = pipeline.start(config)print("所有可用流已成功启动")pipeline.stop()
except Exception as e:print("启动失败:", e)

打印信息：

设备传感器: ['Stereo Module', 'RGB Camera', 'Motion Module']
所有可用流已成功启动

其中，Motion Module表示相机中有IMU模块。

2、分析RealSense相机IMU的格式和帧率

示例程序：

import pyrealsense2 as rsdef list_imu_profiles():# 创建上下文对象ctx = rs.context()# 检查是否有设备连接if len(ctx.devices) == 0:print("未检测到RealSense设备，请确保相机已连接。")return# 获取第一个设备device = ctx.devices[0]print(f"找到设备: {device.get_info(rs.camera_info.name)}")# 枚举所有可能的IMU配置print("\n支持的IMU配置:")print("=" * 60)print("{:<10} {:<15} {:<15} {:<15} {:<10}".format("流类型", "格式", "分辨率", "帧率", "是否默认"))print("-" * 60)# 遍历所有可用的传感器for sensor in device.sensors:# 检查是否为IMU传感器if "Motion" in sensor.get_info(rs.camera_info.name):# 遍历所有可能的配置for profile in sensor.get_stream_profiles():stream = profile.stream_type()if stream == rs.stream.accel or stream == rs.stream.gyro:stream_name = "加速度计" if stream == rs.stream.accel else "陀螺仪"# 将格式对象转换为字符串表示format_str = str(profile.format())fps = profile.fps()is_default = profile.is_default()# 打印配置信息print("{:<10} {:<15} {:<15} {:<15} {:<10}".format(stream_name, format_str, "N/A", fps, is_default))print("=" * 60)if __name__ == "__main__":list_imu_profiles()

打印信息：

找到设备: Intel RealSense D435I

支持的IMU配置:
============================================================
流类型格式分辨率帧率是否默认
------------------------------------------------------------
加速度计 format.motion_xyz32f N/A 200 0
加速度计 format.motion_xyz32f N/A 100 1
陀螺仪 format.motion_xyz32f N/A 400 0
陀螺仪 format.motion_xyz32f N/A 200 1
============================================================

3、分析分析RealSense相机坐标系

下面是相机的坐标系：

比如使用D435i相机，传感器分布：

4、读取IMU数据

示例程序：

import pyrealsense2 as rs
import numpy as np
import threading
import time
from collections import dequeclass RealSenseIMUPoseEstimator:def __init__(self):# 初始化相机和数据流self.pipeline = rs.pipeline()self.config = rs.config()# 仅配置IMU流，不配置彩色和深度流self.config.enable_stream(rs.stream.accel, rs.format.motion_xyz32f, 200)self.config.enable_stream(rs.stream.gyro, rs.format.motion_xyz32f, 200)# 姿态解算参数self.imu_data_lock = threading.Lock()self.accel_data = deque(maxlen=10)self.gyro_data = deque(maxlen=10)# 姿态四元数，初始化为[w, x, y, z]self.quaternion = np.array([1.0, 0.0, 0.0, 0.0])# 时间戳self.last_timestamp = None# 线程控制self.is_running = False# 互补滤波参数self.filter_beta = 0.05  # 互补滤波器参数，越小表示更信任陀螺仪def start(self):# 启动相机profile = self.pipeline.start(self.config)print("IMU数据采集已启动")# 启动IMU数据收集和解算线程self.is_running = Trueself.imu_thread = threading.Thread(target=self._process_imu_data)self.imu_thread.daemon = Trueself.imu_thread.start()try:# 主线程等待用户中断print("按Ctrl+C停止程序")while self.is_running:time.sleep(1)except KeyboardInterrupt:print("\n程序已停止")finally:# 停止程序时释放资源self.is_running = Falseself.imu_thread.join()self.pipeline.stop()def _process_imu_data(self):# 处理IMU数据的线程函数while self.is_running:try:# 等待获取IMU帧frames = self.pipeline.wait_for_frames(timeout_ms=1000)# 处理IMU数据accel_frame = frames.first_or_default(rs.stream.accel)gyro_frame = frames.first_or_default(rs.stream.gyro)if accel_frame and gyro_frame:accel = accel_frame.as_motion_frame().get_motion_data()gyro = gyro_frame.as_motion_frame().get_motion_data()timestamp = accel_frame.get_timestamp() / 1000.0  # 转换为秒with self.imu_data_lock:self.accel_data.append((accel.x, accel.y, accel.z))self.gyro_data.append((gyro.x, gyro.y, gyro.z))# 姿态解算self._update_pose(accel, gyro, timestamp)# 打印姿态数据self._print_pose()except Exception as e:if self.is_running:print(f"IMU数据收集错误: {e}")def _update_pose(self, accel, gyro, timestamp):# 更新姿态四元数if self.last_timestamp is None:self.last_timestamp = timestampreturn# 计算时间间隔(秒)dt = timestamp - self.last_timestampself.last_timestamp = timestamp# 转换为numpy数组accel = np.array([accel.x, accel.y, accel.z])gyro = np.array([gyro.x, gyro.y, gyro.z])# 归一化加速度计数据accel_norm = np.linalg.norm(accel)if accel_norm > 0:accel = accel / accel_norm# 基于陀螺仪更新四元数q_dot = self._quaternion_multiply(self.quaternion, [0, gyro[0], gyro[1], gyro[2]]) * 0.5self.quaternion = self.quaternion + q_dot * dt# 归一化四元数self.quaternion = self.quaternion / np.linalg.norm(self.quaternion)# 基于加速度计估计重力方向# 并使用互补滤波融合两种估计if len(self.accel_data) > 5:  # 确保有足够的数据# 加速度计估计的重力方向accel_gravity = self._accel_to_gravity(accel)# 四元数估计的重力方向q_gravity = self._quaternion_multiply(self._quaternion_multiply(self.quaternion, [0, 0, 0, 1]),self._quaternion_conjugate(self.quaternion))[1:]  # 取向量部分# 计算误差并应用互补滤波error = np.cross(q_gravity, accel_gravity)# 仅更新四元数的向量部分(x,y,z)self.quaternion[1:] = self.quaternion[1:] + error * self.filter_beta * dt# 重新归一化四元数self.quaternion = self.quaternion / np.linalg.norm(self.quaternion)def _accel_to_gravity(self, accel):# 从加速度计数据估计重力方向# 加速度计在静止时测量的是重力加速度return acceldef _quaternion_multiply(self, q1, q2):# 四元数乘法w1, x1, y1, z1 = q1w2, x2, y2, z2 = q2w = w1*w2 - x1*x2 - y1*y2 - z1*z2x = w1*x2 + x1*w2 + y1*z2 - z1*y2y = w1*y2 - x1*z2 + y1*w2 + z1*x2z = w1*z2 + x1*y2 - y1*x2 + z1*w2return np.array([w, x, y, z])def _quaternion_conjugate(self, q):# 四元数共轭return np.array([q[0], -q[1], -q[2], -q[3]])def _quaternion_to_euler(self, q):# 四元数转欧拉角(roll, pitch, yaw)w, x, y, z = q# 滚转(绕x轴)sinr_cosp = 2 * (w * x + y * z)cosr_cosp = 1 - 2 * (x * x + y * y)roll = np.arctan2(sinr_cosp, cosr_cosp)# 俯仰(绕y轴)sinp = 2 * (w * y - z * x)if abs(sinp) >= 1:pitch = np.copysign(np.pi / 2, sinp)  # 万向锁else:pitch = np.arcsin(sinp)# 偏航(绕z轴)siny_cosp = 2 * (w * z + x * y)cosy_cosp = 1 - 2 * (y * y + z * z)yaw = np.arctan2(siny_cosp, cosy_cosp)return np.degrees([roll, pitch, yaw])  # 转换为角度def _print_pose(self):# 打印姿态数据euler_angles = self._quaternion_to_euler(self.quaternion)roll, pitch, yaw = euler_angles# 格式化输出print("四元素：", self.quaternion)print(f"姿态欧拉角: 滚转={roll:.2f}°, 俯仰={pitch:.2f}°, 偏航={yaw:.2f}°")# print(f"姿态: 滚转={roll:.2f}°, 俯仰={pitch:.2f}°, 偏航={yaw:.2f}°", end='\r')if __name__ == "__main__":estimator = RealSenseIMUPoseEstimator()estimator.start()

打印信息：