传感器层交互：摄像头、IMU、深度传感器与GPS的集成与应用
·
传感器层交互：摄像头、IMU、深度传感器与GPS的集成与应用
传感器是现代智能设备感知世界的"眼睛"与"触觉"。本文深入探讨摄像头、IMU（惯性测量单元）、深度传感器和GPS等核心传感器之间的交互机制，以及如何通过它们的协同工作实现更精确的环境感知和定位。
一、传感器基础与原理
1. 摄像头(Camera)
工作原理：
摄像头通过光学系统将光线聚焦到感光元件(通常是CMOS或CCD)上，感光元件将光信号转换为电信号，经过模数转换器(ADC)转换为数字信号，最终形成图像。
主要参数：
- 分辨率：一般以像素表示(例如1920×1080)
- 帧率(FPS)：每秒捕获的图像帧数
- 视场角(FOV)：表示可见范围的角度大小
- 光圈大小(f值)：控制进光量
- 传感器尺寸：影响感光能力和画质
典型输出数据：
{
"timestamp": 1647821023.456,
"frame": [图像数据，通常为RGB或YUV格式],
"resolution": {"width": 1920, "height": 1080},
"format": "RGB24"
}
2. IMU(惯性测量单元)
工作原理：
IMU通常由加速度计、陀螺仪和(有时)磁力计组成，用于测量设备的运动状态。
- 加速度计：测量线性加速度(包括重力)
- 陀螺仪：测量角速度
- 磁力计：测量磁场强度，用于确定朝向
主要参数：
- 采样率：每秒读取的数据点数
- 测量范围：例如±2g、±4g、±8g(加速度计)
- 灵敏度：输出变化与输入变化的比率
- 零偏漂移：静止状态下传感器输出的变化
典型输出数据：
{
"timestamp": 1647821023.456,
"accelerometer": {"x": 0.12, "y": 9.81, "z": 0.34}, // 单位：m/s²
"gyroscope": {"x": 0.01, "y": 0.02, "z": 0.03}, // 单位：rad/s
"magnetometer": {"x": 23.4, "y": -12.1, "z": 42.5} // 单位：μT
}
3. 深度传感器
常见类型：
- 结构光：投射已知图案并分析变形(如iPhone的Face ID)
- ToF(飞行时间)：测量光从传感器到物体再返回的时间
- 立体视觉：利用双目或多目相机的视差计算深度
主要参数：
- 深度范围：可测量的最小和最大距离
- 分辨率：深度图像的像素数量
- 帧率：每秒捕获的深度图数量
- 精度：距离测量的准确度
典型输出数据：
{
"timestamp": 1647821023.456,
"depth_map": [深度值矩阵],
"resolution": {"width": 640, "height": 480},
"min_depth": 0.5, // 单位：米
"max_depth": 5.0 // 单位：米
}
4. GPS(全球定位系统)
工作原理：
GPS接收器接收多颗卫星发送的信号，通过测量信号传播时间计算接收器与各卫星的距离，然后通过三角测量法确定接收器的位置。
主要参数：
- 精度：位置测量的准确度(水平和垂直)
- 更新率：位置信息更新的频率
- 捕获时间：首次获取定位所需的时间(冷启动、温启动、热启动)
- 信道数：可同时跟踪的卫星数量
典型输出数据：
{
"timestamp": 1647821023.456,
"latitude": 37.7749, // 单位：度
"longitude": -122.4194, // 单位：度
"altitude": 10.5, // 单位：米
"accuracy": 5.2, // 单位：米
"speed": 1.2, // 单位：m/s
"satellites": 8 // 可见卫星数
}
二、传感器融合基础
1. 传感器融合概念
传感器融合是将多个传感器的数据结合起来，以获得比单个传感器更准确、完整的信息。这种方法能有效补偿各类传感器的局限性，提高系统的鲁棒性和精度。
主要优势：
- 提高测量精度和可靠性
- 扩展感知范围和能力
- 降低单一传感器失效的风险
- 减少测量噪声和不确定性
2. 传感器融合算法
卡尔曼滤波(Kalman Filter)
卡尔曼滤波是一种递归估计算法，特别适合处理有噪声的线性系统。
基本原理：
- 预测步骤：利用系统模型预测当前状态
- 更新步骤：结合测量数据更新状态估计
# 简化版卡尔曼滤波实现
def kalman_filter(z, x, P, F, H, R, Q):
# 预测步骤
x = F @ x # 状态预测
P = F @ P @ F.T + Q # 协方差预测
# 更新步骤
y = z - H @ x # 测量残差
S = H @ P @ H.T + R # 残差协方差
K = P @ H.T @ np.linalg.inv(S) # 卡尔曼增益
x = x + K @ y # 状态更新
P = (np.eye(len(x)) - K @ H) @ P # 协方差更新
return x, P
扩展卡尔曼滤波(Extended Kalman Filter, EKF)
EKF是卡尔曼滤波的非线性扩展版本，适用于非线性系统，通过在工作点处的线性化处理来近似非线性系统。
粒子滤波(Particle Filter)
粒子滤波是一种基于Monte Carlo方法的非参数贝叶斯滤波技术，特别适合处理非线性、非高斯系统。
# 简化版粒子滤波实现
def particle_filter(particles, weights, z, motion_model, measurement_model):
# 预测
particles = [motion_model(p) for p in particles]
# 更新权重
weights = [measurement_model(p, z) * w for p, w in zip(particles, weights)]
weights = weights / np.sum(weights) # 归一化
# 重采样
indices = np.random.choice(len(particles), size=len(particles), p=weights)
particles = [particles[i] for i in indices]
weights = np.ones(len(particles)) / len(particles)
return particles, weights
互补滤波(Complementary Filter)
互补滤波是一种简单但有效的方法，特别适用于IMU数据处理，它结合了加速度计的长期稳定性和陀螺仪的短期准确性。
# 互补滤波简单实现
def complementary_filter(angle, gyro, accel, dt, alpha=0.98):
# 角度 = 高通滤波(陀螺仪) + 低通滤波(加速度计)
angle = alpha * (angle + gyro * dt) + (1 - alpha) * accel
return angle
三、摄像头与IMU融合
1. 视觉惯性里程计(Visual-Inertial Odometry, VIO)
VIO结合摄像头和IMU数据实现精确的运动跟踪，特别适用于AR/VR、无人机和机器人领域。
主要类型：
- 松耦合(Loosely-coupled)：分别处理视觉和惯性数据，再进行融合
- 紧耦合(Tightly-coupled)：在一个统一框架内同时处理视觉和惯性测量
典型算法：
- MSCKF(Multi-State Constraint Kalman Filter)
- VINS(Visual-Inertial Navigation System)
- OKVIS(Open Keyframe-based Visual-Inertial SLAM)
2. 实现示例：基于EKF的VIO系统
import numpy as np
import cv2
class VIO_System:
def __init__(self):
# 状态向量: [位置, 速度, 姿态, 加速度计偏差, 陀螺仪偏差]
self.state = np.zeros(16)
self.P = np.eye(16) # 协方差矩阵
# 设置相机参数
self.camera_matrix = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
self.dist_coeffs = np.zeros(4) # 假设无畸变
# 特征跟踪器
self.feature_tracker = cv2.FastFeatureDetector_create(threshold=25)
self.lk_params = dict(winSize=(21, 21),
maxLevel=3,
criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 30, 0.01))
self.prev_frame = None
self.prev_features = None
def process_imu_data(self, acc, gyro, dt):
"""处理IMU数据，通过EKF预测步骤更新状态"""
# 提取当前状态
pos = self.state[0:3]
vel = self.state[3:6]
quat = self.state[6:10] # 四元数表示姿态
acc_bias = self.state[10:13]
gyro_bias = self.state[13:16]
# 校正IMU读数
acc_corrected = acc - acc_bias
gyro_corrected = gyro - gyro_bias
# 更新姿态(基于四元数积分)
quat_new = self._integrate_rotation(quat, gyro_corrected, dt)
# 考虑重力并更新速度
gravity = np.array([0, 0, 9.81]) # 地球重力
rot_matrix = self._quaternion_to_rotation_matrix(quat)
acc_global = rot_matrix @ acc_corrected - gravity
vel_new = vel + acc_global * dt
# 更新位置
pos_new = pos + vel * dt + 0.5 * acc_global * dt * dt
# 更新状态
self.state[0:3] = pos_new
self.state[3:6] = vel_new
self.state[6:10] = quat_new
# 更新状态协方差(完整EKF实现需要雅可比矩阵)
# 简化版：仅添加过程噪声
process_noise = np.eye(16) * 0.01
self.P = self.P + process_noise * dt
def process_camera_frame(self, frame):
"""处理摄像头帧，更新视觉测量"""
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
if self.prev_frame is None:
# 首次处理，仅检测特征点
self.prev_features = self.feature_tracker.detect(gray)
self.prev_features = np.array([x.pt for x in self.prev_features], dtype=np.float32)
else:
# 计算光流以跟踪特征点
new_features, status, _ = cv2.calcOpticalFlowPyrLK(
self.prev_frame, gray, self.prev_features, None, **self.lk_params)
# 过滤有效跟踪点
good_old = self.prev_features[status == 1]
good_new = new_features[status == 1]
if len(good_old) > 8: # 至少需要8个点计算本质矩阵
# 计算本质矩阵
E, mask = cv2.findEssentialMat(good_new, good_old, self.camera_matrix,
method=cv2.RANSAC, prob=0.999, threshold=1.0)
# 从本质矩阵恢复相机运动
_, R, t, _ = cv2.recoverPose(E, good_new, good_old, self.camera_matrix, mask=mask)
# 通过EKF更新阶段整合视觉测量
self._update_with_visual_measurement(R, t)
# 更新特征点
self.prev_features = good_new.reshape(-1, 1, 2)
self.prev_frame = gray
def _integrate_rotation(self, quat, gyro, dt):
"""通过角速度积分更新四元数"""
# 简化实现，实际应考虑四元数的正确积分
return quat + dt * 0.5 * self._quaternion_multiply(quat, np.array([0, gyro[0], gyro[1], gyro[2]]))
def _quaternion_to_rotation_matrix(self, q):
"""四元数转旋转矩阵"""
# 简化实现
return np.eye(3) # 实际需要计算正确的旋转矩阵
def _quaternion_multiply(self, q1, q2):
"""四元数乘法"""
# 简化实现
return q1 # 实际需要正确计算四元数乘积
def _update_with_visual_measurement(self, R, t):
"""使用视觉估计的旋转和平移更新EKF"""
# 构建测量模型和雅可比矩阵(简化)
H = np.zeros((6, 16))
H[0:3, 0:3] = np.eye(3) # 位置对应
H[3:6, 6:9] = np.eye(3) # 姿态对应(简化)
# 转换视觉估计为合适的格式
visual_measurement = np.zeros(6)
visual_measurement[0:3] = t.flatten()
# 从旋转矩阵提取欧拉角(简化)
visual_measurement[3:6] = np.array([0, 0, 0]) # 实际应提取正确的欧拉角
# 测量噪声
R_noise = np.eye(6) * 0.1
# 卡尔曼增益
K = self.P @ H.T @ np.linalg.inv(H @ self.P @ H.T + R_noise)
# 更新状态
innovation = visual_measurement - H @ self.state
self.state = self.state + K @ innovation
# 更新协方差
self.P = (np.eye(16) - K @ H) @ self.P
3. 应用案例：AR中的姿态跟踪
在增强现实(AR)应用中，准确的姿态跟踪是实现虚拟内容稳定叠加的关键。摄像头与IMU融合能在快速运动或光照变化时保持跟踪稳定性。
// Unity C#中的简化AR姿态跟踪示例
using UnityEngine;
using UnityEngine.XR.ARFoundation;
public class ARPoseTracker : MonoBehaviour
{
[SerializeField]
private ARSession arSession;
[SerializeField]
private GameObject virtualObject;
private ARCameraManager cameraManager;
private bool isTrackingStable = false;
// 滤波参数
private Vector3 filteredPosition;
private Quaternion filteredRotation;
private float positionSmoothFactor = 0.1f;
private float rotationSmoothFactor = 0.1f;
private void Start()
{
cameraManager = arSession.GetComponentInChildren<ARCameraManager>();
cameraManager.frameReceived += OnCameraFrameReceived;
// 初始化滤波值
filteredPosition = transform.position;
filteredRotation = transform.rotation;
}
private void OnCameraFrameReceived(ARCameraFrameEventArgs args)
{
// 检查跟踪状态
if (args.displayMatrix.HasValue)
{
isTrackingStable = true;
}
else
{
isTrackingStable = false;
}
}
private void Update()
{
if (isTrackingStable)
{
// 获取当前相机姿态(基于相机和IMU融合)
Vector3 currentPosition = Camera.main.transform.position;
Quaternion currentRotation = Camera.main.transform.rotation;
// 应用互补滤波减少抖动
filteredPosition = Vector3.Lerp(filteredPosition, currentPosition, positionSmoothFactor);
filteredRotation = Quaternion.Slerp(filteredRotation, currentRotation, rotationSmoothFactor);
// 更新虚拟对象位置
if (virtualObject != null)
{
// 根据相机姿态放置虚拟对象
virtualObject.transform.position = filteredPosition + filteredRotation * Vector3.forward * 2.0f;
virtualObject.transform.rotation = filteredRotation;
}
}
else
{
Debug.Log("AR跟踪不稳定");
}
}
private void OnDestroy()
{
if (cameraManager != null)
cameraManager.frameReceived -= OnCameraFrameReceived;
}
}
四、深度传感器与摄像头融合
1. RGB-D融合基础
RGB-D融合将普通RGB图像与深度信息结合，可创建详细的3D场景重建，并增强物体识别和分割能力。
主要应用：
- 3D场景重建与建模
- 物体检测与分割
- 手势识别
- 增强现实中的遮挡处理
2. 点云生成与处理
import numpy as np
import cv2
import open3d as o3d
def create_point_cloud_from_rgbd(rgb_image, depth_image, camera_intrinsics):
"""从RGB和深度图像创建点云"""
# 转换图像格式
rgb = np.asarray(rgb_image)
depth = np.asarray(depth_image).astype(np.float32) / 1000.0 # 假设深度单位为毫米，转换为米
# 创建Open3D图像对象
color_o3d = o3d.geometry.Image(rgb)
depth_o3d = o3d.geometry.Image(depth)
# 创建RGBD图像
rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
color_o3d, depth_o3d,
depth_scale=1.0, # 因为已经转换为米
depth_trunc=3.0, # 最大深度，超过该值的点将被忽略
convert_rgb_to_intensity=False
)
# 设置相机内参
intrinsic = o3d.camera.PinholeCameraIntrinsic()
intrinsic.set_intrinsics(
width=rgb.shape[1],
height=rgb.shape[0],
fx=camera_intrinsics[0, 0],
fy=camera_intrinsics[1, 1],
cx=camera_intrinsics[0, 2],
cy=camera_intrinsics[1, 2]
)
# 创建点云
pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd, intrinsic)
# 转换坐标系(相机坐标系->世界坐标系)
pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
return pcd
def process_point_cloud(pcd, voxel_size=0.02):
"""处理点云：降采样、去除离群点、估计法线"""
# 体素降采样
pcd_down = pcd.voxel_down_sample(voxel_size)
# 估计法线
pcd_down.estimate_normals(
search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30)
)
# 移除离群点
cl, ind = pcd_down.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
pcd_filtered = pcd_down.select_by_index(ind)
return pcd_filtered
def reconstruct_mesh_from_point_cloud(pcd):
"""从点云重建3D网格"""
# 计算点云密度以确定适当的半径
distances = pcd.compute_nearest_neighbor_distance()
avg_dist = np.mean(distances)
radius = 3 * avg_dist
# 泊松表面重建
mesh, densities = o3d.geometry.TriangleMesh.create_from_point_cloud_poisson(
pcd, depth=9, width=0, scale=1.1, linear_fit=False
)
# 移除低密度顶点
vertices_to_remove = densities < np.quantile(densities, 0.1)
mesh.remove_vertices_by_mask(vertices_to_remove)
return mesh
3. 深度增强的人脸识别
深度信息可以显著提高人脸识别的安全性，防止照片欺骗。
import cv2
import numpy as np
import dlib
class DepthEnhancedFaceRecognition:
def __init__(self):
# 加载人脸检测器和特征提取器
self.detector = dlib.get_frontal_face_detector()
self.predictor = dlib.shape_predictor("shape_predictor_68_face_landmarks.dat")
self.face_rec = dlib.face_recognition_model_v1("dlib_face_recognition_resnet_model_v1.dat")
# 人脸编码数据库
self.known_face_encodings = []
self.known_face_names = []
def register_face(self, rgb_image, depth_image, name):
"""注册新面孔到数据库"""
# 检测人脸
faces = self.detector(rgb_image)
if len(faces) == 0:
return False, "没有检测到人脸"
face = faces[0] # 取第一个人脸
# 检查是否为真实人脸(通过深度一致性)
if not self._check_face_depth_consistency(depth_image, face):
return False, "可能是平面照片，深度不一致"
# 提取人脸特征
landmarks = self.predictor(rgb_image, face)
face_encoding = self.face_rec.compute_face_descriptor(rgb_image, landmarks)
# 添加到数据库
self.known_face_encodings.append(np.array(face_encoding))
self.known_face_names.append(name)
return True, "成功注册人脸"
def identify_face(self, rgb_image, depth_image, tolerance=0.6):
"""识别人脸并进行活体检测"""
# 检测人脸
faces = self.detector(rgb_image)
if len(faces) == 0:
return None, "没有检测到人脸"
face = faces[0] # 取第一个人脸
# 活体检测
if not self._check_face_depth_consistency(depth_image, face):
return None, "活体检测失败，可能是照片"
# 人脸特征提取
landmarks = self.predictor(rgb_image, face)
face_encoding = np.array(self.face_rec.compute_face_descriptor(rgb_image, landmarks))
if len(self.known_face_encodings) == 0:
return None, "数据库中没有注册的人脸"
# 人脸匹配
face_distances = np.linalg.norm(self.known_face_encodings - face_encoding, axis=1)
best_match_index = np.argmin(face_distances)
if face_distances[best_match_index] <= tolerance:
return self.known_face_names[best_match_index], "识别成功"
else:
return None, "未找到匹配的人脸"
def _check_face_depth_consistency(self, depth_image, face):
"""检查人脸深度是否一致，用于活体检测"""
# 提取人脸区域的深度值
face_depth = depth_image[face.top():face.bottom(), face.left():face.right()]
# 计算深度方差，平面照片的方差应该很小
depth_variance = np.var(face_depth[face_depth > 0]) # 忽略深度为0的点
# 计算深度梯度
if face_depth.size > 0:
dx = cv2.Sobel(face_depth, cv2.CV_64F, 1, 0, ksize=3)
dy = cv2.Sobel(face_depth, cv2.CV_64F, 0, 1, ksize=3)
gradient_magnitude = np.sqrt(dx**2 + dy**2)
avg_gradient = np.mean(gradient_magnitude)
# 真实人脸应有一定的深度变化
return depth_variance > 50.0 and avg_gradient > 5.0
return False
五、IMU与GPS融合
1. 定位精度提升
IMU与GPS融合可以在GPS信号较弱或短时间丢失时保持定位，并提高整体定位精度和稳定性。
主要优势：
- 提高高动态情况下的定位精度
- 弥补GPS信号受阻时的定位能力
- 增强导航系统的可靠性和连续性
2. 卡尔曼滤波实现GPS-IMU融合
import numpy as np
from scipy.linalg import block_diag
class GPS_IMU_Fusion:
def __init__(self):
# 状态向量: [x, y, z, vx, vy, vz]
self.state = np.zeros(6)
# 状态协方差矩阵
self.P = np.eye(6) * 100 # 初始不确定性较大
# 过程噪声协方差
self.Q = np.diag([0.01, 0.01, 0.01, 0.1, 0.1, 0.1])
# GPS测量噪声协方差
self.R_gps = np.diag([10.0, 10.0, 15.0]) # 位置精度（米）
# 状态转移矩阵
self.F = np.eye(6)
# 上次更新时间
self.last_time = None
# 是否有效初始化
self.initialized = False
def initialize(self, gps_data):
"""使用GPS数据初始化滤波器"""
lat, lon, alt = gps_data['latitude'], gps_data['longitude'], gps_data['altitude']
# 转换为ECEF或ENU坐标系(简化版：假设已转换)
x, y, z = self._geodetic_to_local(lat, lon, alt)
# 初始化状态
self.state[0:3] = [x, y, z]
self.last_time = gps_data['timestamp']
self.initialized = True
def predict(self, imu_data):
"""使用IMU数据预测状态"""
if not self.initialized:
return
# 计算时间增量
current_time = imu_data['timestamp']
if self.last_time is None:
self.last_time = current_time
return
dt = current_time - self.last_time
if dt <= 0:
return
# 更新状态转移矩阵
self.F[0, 3] = dt
self.F[1, 4] = dt
self.F[2, 5] = dt
# 获取加速度并校正重力
acc = np.array([imu_data['accelerometer']['x'],
imu_data['accelerometer']['y'],
imu_data['accelerometer']['z']])
# 简化版：假设已知方向，可以移除重力影响(实际需要姿态估计)
# 加速度直接积分得到速度增量(简化)
acc_without_gravity = acc - np.array([0, 0, 9.81]) # 简单移除重力
velocity_increment = acc_without_gravity * dt
# 预测状态
self.state = self.F @ self.state
# 添加加速度影响(简化)
self.state[3:6] += velocity_increment
# 预测协方差
self.P = self.F @ self.P @ self.F.T + self.Q * dt
# 更新时间
self.last_time = current_time
def update_with_gps(self, gps_data):
"""使用GPS测量更新状态"""
if not self.initialized:
self.initialize(gps_data)
return
# 转换GPS数据到局部坐标
lat, lon, alt = gps_data['latitude'], gps_data['longitude'], gps_data['altitude']
x, y, z = self._geodetic_to_local(lat, lon, alt)
# 测量值
z = np.array([x, y, z])
# 测量矩阵 H: 只观测位置
H = np.zeros((3, 6))
H[0, 0] = H[1, 1] = H[2, 2] = 1.0
# 计算卡尔曼增益
S = H @ self.P @ H.T + self.R_gps
K = self.P @ H.T @ np.linalg.inv(S)
# 更新状态
innovation = z - H @ self.state
self.state = self.state + K @ innovation
# 更新协方差
self.P = (np.eye(6) - K @ H) @ self.P
def _geodetic_to_local(self, lat, lon, alt):
"""将GPS经纬度转换为局部坐标系(ENU)
实际应用中应使用专业的坐标转换库
这里仅作示意，返回简化值
"""
# 此处需要实际的坐标转换
# 简化示例，假设已转换
return lat * 111320.0, lon * 111320.0 * np.cos(np.radians(lat)), alt
3. 应用案例：自动驾驶中的定位系统
在自动驾驶中，精确定位至关重要。结合IMU和GPS可以在城市峡谷或隧道等GPS信号不佳的环境中保持准确定位。
// C++自动驾驶定位系统示例
#include <Eigen/Dense>
#include <vector>
#include <chrono>
#include <iostream>
class AutoDriveLocalization {
private:
// 状态向量及协方差
Eigen::VectorXd state; // [x, y, heading, velocity, angular_velocity]
Eigen::MatrixXd P; // 状态协方差
// 系统矩阵
Eigen::MatrixXd F; // 状态转移矩阵
Eigen::MatrixXd Q; // 过程噪声
// 传感器数据历史
struct SensorData {
double timestamp;
Eigen::Vector3d acceleration; // IMU加速度
Eigen::Vector3d gyro; // IMU角速度
bool has_gps;
Eigen::Vector3d gps_position; // GPS位置
double gps_accuracy; // GPS精度
};
std::vector<SensorData> sensor_history;
// 上次更新时间
double last_timestamp;
// 地图匹配功能
bool use_map_matching;
// 添加地图相关参数...
public:
AutoDriveLocalization(bool enable_map_matching = true) : use_map_matching(enable_map_matching) {
// 初始化状态向量 [x, y, heading, velocity, angular_velocity]
state = Eigen::VectorXd::Zero(5);
P = Eigen::MatrixXd::Identity(5, 5) * 100.0; // 较大的初始不确定性
// 初始化系统矩阵
F = Eigen::MatrixXd::Identity(5, 5);
Q = Eigen::MatrixXd::Identity(5, 5) * 0.01;
last_timestamp = 0.0;
}
void initialize_with_gps(const Eigen::Vector3d& gps_position, double heading = 0.0) {
state(0) = gps_position(0); // x
state(1) = gps_position(1); // y
state(2) = heading; // 朝向角
std::cout << "系统初始化，位置: (" << state(0) << ", " << state(1) << "), 朝向: " << state(2) << std::endl;
}
void process_imu_data(double timestamp, const Eigen::Vector3d& acceleration, const Eigen::Vector3d& gyro) {
// 计算时间增量
double dt = 0.0;
if (last_timestamp > 0.0) {
dt = timestamp - last_timestamp;
}
last_timestamp = timestamp;
if (dt <= 0.0) return;
// 记录传感器数据
SensorData data;
data.timestamp = timestamp;
data.acceleration = acceleration;
data.gyro = gyro;
data.has_gps = false;
sensor_history.push_back(data);
if (sensor_history.size() > 1000) {
sensor_history.erase(sensor_history.begin());
}
// 更新状态转移矩阵
F(0, 3) = dt * std::cos(state(2)); // x位置随速度的变化
F(1, 3) = dt * std::sin(state(2)); // y位置随速度的变化
F(2, 4) = dt; // 朝向随角速度的变化
// 预测步骤
// 由IMU加速度估计线性加速度(简化)
double accel_magnitude = acceleration.norm() - 9.81; // 简化：移除重力
// 使用当前角速度直接代替状态中的角速度
state(4) = gyro(2); // 假设z轴为垂直轴
// 更新速度
state(3) += accel_magnitude * dt;
// 预测位置和朝向
state(0) += state(3) * dt * std::cos(state(2));
state(1) += state(3) * dt * std::sin(state(2));
state(2) += state(4) * dt;
// 规范化朝向角到[-π, π]
state(2) = std::atan2(std::sin(state(2)), std::cos(state(2)));
// 更新协方差
P = F * P * F.transpose() + Q * dt;
std::cout << "IMU更新 - 位置: (" << state(0) << ", " << state(1)
<< "), 朝向: " << state(2) << ", 速度: " << state(3) << std::endl;
}
void process_gps_data(double timestamp, const Eigen::Vector3d& gps_position, double accuracy) {
// 记录GPS数据
if (!sensor_history.empty() && sensor_history.back().timestamp == timestamp) {
sensor_history.back().has_gps = true;
sensor_history.back().gps_position = gps_position;
sensor_history.back().gps_accuracy = accuracy;
} else {
SensorData data;
data.timestamp = timestamp;
data.has_gps = true;
data.gps_position = gps_position;
data.gps_accuracy = accuracy;
data.acceleration = Eigen::Vector3d::Zero();
data.gyro = Eigen::Vector3d::Zero();
sensor_history.push_back(data);
}
// 设置测量矩阵(仅测量位置)
Eigen::MatrixXd H = Eigen::MatrixXd::Zero(2, 5);
H(0, 0) = 1.0; // 测量x位置
H(1, 1) = 1.0; // 测量y位置
// 设置测量噪声
Eigen::MatrixXd R = Eigen::MatrixXd::Identity(2, 2) * (accuracy * accuracy);
// 实际测量值
Eigen::VectorXd z(2);
z << gps_position(0), gps_position(1);
// 预测的测量值
Eigen::VectorXd z_pred = H * state;
// 创新(残差)
Eigen::VectorXd y = z - z_pred;
// 创新协方差
Eigen::MatrixXd S = H * P * H.transpose() + R;
// 卡尔曼增益
Eigen::MatrixXd K = P * H.transpose() * S.inverse();
// 更新状态
state = state + K * y;
// 更新状态协方差
P = (Eigen::MatrixXd::Identity(5, 5) - K * H) * P;
std::cout << "GPS更新 - 位置: (" << state(0) << ", " << state(1)
<< "), 精度: " << accuracy << std::endl;
// 如果启用地图匹配，使用道路网络进一步优化位置
if (use_map_matching) {
map_matching();
}
}
void map_matching() {
// 在实际应用中，这里会实现地图匹配算法
// 将估计位置吸附到最可能的道路上
std::cout << "执行地图匹配..." << std::endl;
// 简化示例：仅打印日志
std::cout << "地图匹配后位置: (" << state(0) << ", " << state(1) << ")" << std::endl;
}
Eigen::Vector3d get_current_position() const {
return Eigen::Vector3d(state(0), state(1), 0.0);
}
double get_current_heading() const {
return state(2);
}
double get_current_velocity() const {
return state(3);
}
Eigen::MatrixXd get_position_covariance() const {
// 返回位置的协方差矩阵(2x2)
Eigen::MatrixXd pos_cov(2, 2);
pos_cov << P(0, 0), P(0, 1),
P(1, 0), P(1, 1);
return pos_cov;
}
};
六、多传感器综合融合
1. 视觉-惯性-深度-GPS集成系统
整合所有传感器可以构建全面的环境感知和定位系统，每种传感器均可弥补其他传感器的不足。
融合策略：
- 多级融合：先进行底层传感器融合(如视觉-惯性)，再进行高层融合
- 松耦合vs紧耦合：根据应用需求选择融合紧密程度
- 异步融合：处理不同传感器的不同采样率
2. 机器人SLAM系统示例
import numpy as np
import cv2
import g2o
import time
class MultiSensorSLAM:
def __init__(self):
# 初始化SLAM系统
self.poses = [] # 机器人位姿历史
self.landmarks = {} # 特征点/路标点
# 初始化优化器
self.optimizer = g2o.SparseOptimizer()
solver = g2o.BlockSolverSE3(g2o.LinearSolverEigenSE3())
solver = g2o.OptimizationAlgorithmLevenberg(solver)
self.optimizer.set_algorithm(solver)
# 传感器参数
self.camera_matrix = np.array([[720.0, 0, 320.0], [0, 720.0, 240.0], [0, 0, 1]])
# 特征提取与跟踪
self.orb = cv2.ORB_create()
self.bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
# 上一帧数据
self.prev_frame = None
self.prev_kp = None
self.prev_des = None
# 当前位姿(初始位置是原点)
self.current_pose = np.eye(4)
# 是否初始化
self.is_initialized = False
def initialize(self, rgb_image, depth_image, imu_data, gps_data=None):
"""初始化SLAM系统"""
# 提取特征
gray = cv2.cvtColor(rgb_image, cv2.COLOR_BGR2GRAY)
kp, des = self.orb.detectAndCompute(gray, None)
# 保存第一帧
self.prev_frame = gray
self.prev_kp = kp
self.prev_des = des
# 设置初始位置
if gps_data is not None:
# 如果有GPS，用GPS初始化位置(简化表示)
self.current_pose[0, 3] = gps_data['x']
self.current_pose[1, 3] = gps_data['y']
self.current_pose[2, 3] = gps_data['z']
# 添加初始位姿到优化器
pose_vertex = g2o.VertexSE3Expmap()
pose_vertex.set_id(0)
pose_vertex.set_estimate(self._pose_to_g2o(self.current_pose))
pose_vertex.set_fixed(True) # 固定第一帧
self.optimizer.add_vertex(pose_vertex)
# 记录位姿
self.poses.append(self.current_pose.copy())
# 初始化完成
self.is_initialized = True
print("SLAM系统初始化完成")
def process_frame(self, rgb_image, depth_image, imu_data, gps_data=None):
"""处理新的数据帧"""
if not self.is_initialized:
self.initialize(rgb_image, depth_image, imu_data, gps_data)
return
# 提取当前帧特征
gray = cv2.cvtColor(rgb_image, cv2.COLOR_BGR2GRAY)
curr_kp, curr_des = self.orb.detectAndCompute(gray, None)
# 特征匹配
matches = self.bf.match(self.prev_des, curr_des)
matches = sorted(matches, key=lambda x: x.distance)
# 用前30%的匹配点计算相机运动
num_good_matches = int(len(matches) * 0.3)
good_matches = matches[:num_good_matches]
# 提取匹配点
prev_pts = np.float32([self.prev_kp[m.queryIdx].pt for m in good_matches])
curr_pts = np.float32([curr_kp[m.trainIdx].pt for m in good_matches])
# 使用PnP(Perspective-n-Point)估计相机运动
# 首先获取特征点的3D位置(使用深度图)
pts_3d = []
for pt in prev_pts:
u, v = int(pt[0]), int(pt[1])
if 0 <= u < depth_image.shape[1] and 0 <= v < depth_image.shape[0]:
z = depth_image[v, u] / 1000.0 # 假设深度单位是毫米，转换为米
if z > 0:
x = (u - self.camera_matrix[0, 2]) * z / self.camera_matrix[0, 0]
y = (v - self.camera_matrix[1, 2]) * z / self.camera_matrix[1, 1]
pts_3d.append([x, y, z])
pts_3d = np.array(pts_3d)
if len(pts_3d) >= 6: # 需要至少6个点
# 使用PnP算法估计相机位姿
_, rvec, tvec, inliers = cv2.solvePnPRansac(
pts_3d, curr_pts[:len(pts_3d)], self.camera_matrix, None)
# 将旋转向量转换为旋转矩阵
rmat, _ = cv2.Rodrigues(rvec)
# 构建变换矩阵
transform = np.eye(4)
transform[:3, :3] = rmat
transform[:3, 3] = tvec.squeeze()
# 获取IMU数据估计的位姿变化(简化)
imu_delta_pose = self._estimate_pose_from_imu(imu_data)
# 融合视觉和IMU的位姿估计(简单加权平均)
vision_weight = 0.7
imu_weight = 0.3
delta_pose = np.eye(4)
delta_pose[:3, :3] = vision_weight * transform[:3, :3] + imu_weight * imu_delta_pose[:3, :3]
delta_pose[:3, 3] = vision_weight * transform[:3, 3] + imu_weight * imu_delta_pose[:3, 3]
# 如果有GPS数据，进一步融合
if gps_data is not None:
self._fuse_with_gps(delta_pose, gps_data)
# 更新当前位姿
self.current_pose = self.current_pose @ delta_pose
# 添加新的位姿顶点到优化器
pose_id = len(self.poses)
pose_vertex = g2o.VertexSE3Expmap()
pose_vertex.set_id(pose_id)
pose_vertex.set_estimate(self._pose_to_g2o(self.current_pose))
self.optimizer.add_vertex(pose_vertex)
# 添加位姿间的边(约束)
edge = g2o.EdgeSE3Expmap()
edge.set_vertex(0, self.optimizer.vertex(pose_id - 1))
edge.set_vertex(1, self.optimizer.vertex(pose_id))
edge.set_measurement(self._pose_to_g2o(delta_pose))
# 设置信息矩阵(协方差的逆)
information = np.eye(6) * 100 # 简化版，实际应根据匹配质量和IMU可靠性调整
edge.set_information(information)
self.optimizer.add_edge(edge)
# 处理观测到的特征点/路标
self._process_landmarks(pts_3d, curr_pts, pose_id)
# 优化图(每10帧执行一次全局优化)
if pose_id % 10 == 0:
self.optimizer.initialize_optimization()
self.optimizer.optimize(10) # 10次迭代
# 更新位姿
for i in range(len(self.poses)):
vertex = self.optimizer.vertex(i)
self.poses[i] = self._g2o_to_pose(vertex.estimate())
self.current_pose = self.poses[-1]
# 记录当前位姿
self.poses.append(self.current_pose.copy())
# 更新上一帧数据
self.prev_frame = gray
self.prev_kp = curr_kp
self.prev_des = curr_des
def _estimate_pose_from_imu(self, imu_data):
"""从IMU数据估计位姿变化"""
# 简化的IMU处理，实际应用中需要更复杂的处理
# 从加速度和角速度积分得到位置和姿态变化
dt = 0.1 # 假设时间间隔
acc = np.array([imu_data['accelerometer']['x'],
imu_data['accelerometer']['y'],
imu_data['accelerometer']['z']])
gyro = np.array([imu_data['gyroscope']['x'],
imu_data['gyroscope']['y'],
imu_data['gyroscope']['z']])
# 简化：假设绕z轴旋转
theta = gyro[2] * dt
# 构建旋转矩阵(简化为2D平面旋转)
R = np.eye(3)
R[0, 0] = np.cos(theta)
R[0, 1] = -np.sin(theta)
R[1, 0] = np.sin(theta)
R[1, 1] = np.cos(theta)
# 计算位移(简化)
acc_without_gravity = acc - np.array([0, 0, 9.81]) # 移除重力(简化)
displacement = 0.5 * acc_without_gravity * dt * dt
# 构建变换矩阵
transform = np.eye(4)
transform[:3, :3] = R
transform[:3, 3] = displacement
return transform
def _fuse_with_gps(self, delta_pose, gps_data):
"""融合GPS数据进一步优化位姿"""
# 简化的GPS融合
# 实际应用中需要坐标系转换和卡尔曼滤波融合
# 根据GPS数据调整delta_pose中的位移部分
gps_weight = 0.2 # GPS权重
# 假设GPS数据已转换为相同坐标系
gps_position = np.array([gps_data['x'], gps_data['y'], gps_data['z']])
# 预测的新位置
predicted_position = self.current_pose[:3, 3] + delta_pose[:3, 3]
# 计算GPS与预测位置的差值
position_error = gps_position - predicted_position
# 根据GPS调整位移
delta_pose[:3, 3] += gps_weight * position_error
def _process_landmarks(self, pts_3d, curr_pts, pose_id):
"""处理观测到的特征点/路标"""
for i in range(len(pts_3d)):
# 将3D点从相机坐标系转换到世界坐标系
pt_world = self.current_pose @ np.append(pts_3d[i], 1)
# 生成唯一ID
pt_id = len(self.landmarks) + 1000
# 添加路标点到优化器(如果是新点)
if pt_id not in self.landmarks:
landmark = g2o.VertexPointXYZ()
landmark.set_id(pt_id)
landmark.set_estimate(pt_world[:3])
self.optimizer.add_vertex(landmark)
self.landmarks[pt_id] = pt_world[:3]
# 添加相机-路标观测边
edge = g2o.EdgeSE3PointXYZ()
edge.set_vertex(0, self.optimizer.vertex(pose_id))
edge.set_vertex(1, self.optimizer.vertex(pt_id))
# 设置测量值(在相机坐标系中的3D点)
edge.set_measurement(pts_3d[i])
# 设置信息矩阵
information = np.eye(3) * 1000 # 简化，实际应根据深度不确定性设置
edge.set_information(information)
self.optimizer.add_edge(edge)
def _pose_to_g2o(self, pose):
"""将4x4变换矩阵转换为g2o SE3Quat"""
R = pose[:3, :3]
t = pose[:3, 3]
# 转换为g2o::SE3Quat
# 实际代码需要根据g2o接口进行正确转换
return g2o.SE3Quat(R, t)
def _g2o_to_pose(self, g2o_pose):
"""将g2o SE3Quat转换为4x4变换矩阵"""
# 从g2o::SE3Quat获取旋转和平移
# 实际代码需要根据g2o接口进行正确转换
R = g2o_pose.rotation().matrix()
t = g2o_pose.translation()
# 构建4x4变换矩阵
pose = np.eye(4)
pose[:3, :3] = R
pose[:3, 3] = t
return pose
def get_current_pose(self):
"""获取当前位姿"""
return self.current_pose
def get_trajectory(self):
"""获取完整轨迹"""
return self.poses
def get_map_points(self):
"""获取地图点"""
return self.landmarks
3. 自动驾驶感知系统
// C++自动驾驶感知系统示例
#include <vector>
#include <unordered_map>
#include <memory>
#include <iostream>
// 前向声明
class SensorData;
class Object;
class Camera;
class IMU;
class LiDAR;
class GPS;
class Fusion;
// 传感器数据基类
class SensorData {
public:
double timestamp;
SensorData(double time) : timestamp(time) {}
virtual ~SensorData() = default;
};
// 相机数据
class CameraData : public SensorData {
public:
// 简化：假设图像数据是一个指针
void* image_data;
int width, height, channels;
CameraData(double time, void* data, int w, int h, int c)
: SensorData(time), image_data(data), width(w), height(h), channels(c) {}
};
// IMU数据
class IMUData : public SensorData {
public:
double acc_x, acc_y, acc_z; // 加速度
double gyro_x, gyro_y, gyro_z; // 角速度
IMUData(double time, double ax, double ay, double az, double gx, double gy, double gz)
: SensorData(time), acc_x(ax), acc_y(ay), acc_z(az), gyro_x(gx), gyro_y(gy), gyro_z(gz) {}
};
// LiDAR数据
class LiDARData : public SensorData {
public:
// 简化：假设点云是3D点的vector
std::vector<std::array<float, 3>> point_cloud;
LiDARData(double time, const std::vector<std::array<float, 3>>& points)
: SensorData(time), point_cloud(points) {}
};
// GPS数据
class GPSData : public SensorData {
public:
double latitude, longitude, altitude;
double accuracy;
GPSData(double time, double lat, double lon, double alt, double acc)
: SensorData(time), latitude(lat), longitude(lon), altitude(alt), accuracy(acc) {}
};
// 检测到的物体类
class Object {
public:
enum class Type {
UNKNOWN,
VEHICLE,
PEDESTRIAN,
CYCLIST,
TRAFFIC_SIGN
};
Type type;
std::array<float, 3> position; // 世界坐标中的位置
std::array<float, 3> dimensions; // 长宽高
float orientation; // 朝向角
float confidence; // 检测置信度
// 目标的ID，用于跟踪
int tracking_id;
// 目标的速度
std::array<float, 3> velocity;
Object(Type t, const std::array<float, 3>& pos, const std::array<float, 3>& dim,
float orient, float conf)
: type(t), position(pos), dimensions(dim), orientation(orient), confidence(conf),
tracking_id(-1), velocity({0.0f, 0.0f, 0.0f}) {}
};
// 相机传感器
class Camera {
private:
int id;
std::array<double, 9> intrinsics; // 内参矩阵
std::array<double, 6> extrinsics; // 外参(位置和姿态)
public:
Camera(int camera_id, const std::array<double, 9>& intr, const std::array<double, 6>& extr)
: id(camera_id), intrinsics(intr), extrinsics(extr) {}
std::vector<Object> detect_objects(const CameraData& data) {
// 在实际应用中，这里会调用计算机视觉算法进行物体检测
std::cout << "相机 " << id << " 正在检测物体..." << std::endl;
// 简化：返回一些模拟的检测结果
std::vector<Object> detected_objects;
// 模拟检测到一辆车
detected_objects.emplace_back(
Object::Type::VEHICLE,
std::array<float, 3>{10.0f, 0.0f, 0.0f}, // 位置
std::array<float, 3>{4.5f, 2.0f, 1.5f}, // 尺寸
0.0f, // 朝向
0.95f // 置信度
);
// 模拟检测到一个行人
detected_objects.emplace_back(
Object::Type::PEDESTRIAN,
std::array<float, 3>{5.0f, -2.0f, 0.0f}, // 位置
std::array<float, 3>{0.5f, 0.5f, 1.8f}, // 尺寸
0.0f, // 朝向
0.85f // 置信度
);
std::cout << "相机检测到 " << detected_objects.size() << " 个物体" << std::endl;
return detected_objects;
}
const std::array<double, 9>& get_intrinsics() const {
return intrinsics;
}
const std::array<double, 6>& get_extrinsics() const {
return extrinsics;
}
};
// IMU传感器
class IMU {
private:
std::array<double, 3> position; // IMU在车辆上的位置
std::array<double, 3> orientation; // IMU的朝向
public:
IMU(const std::array<double, 3>& pos, const std::array<double, 3>& orient)
: position(pos), orientation(orient) {}
std::array<double, 6> get_motion(const IMUData& data) {
// 处理IMU数据，估计运动
std::cout << "处理IMU数据..." << std::endl;
// 简化：返回线性加速度和角速度
std::array<double, 6> motion = {
data.acc_x, data.acc_y, data.acc_z, // 线性加速度
data.gyro_x, data.gyro_y, data.gyro_z // 角速度
};
return motion;
}
};
// LiDAR传感器
class LiDAR {
private:
int id;
std::array<double, 6> extrinsics; // 外参(位置和姿态)
public:
LiDAR(int lidar_id, const std::array<double, 6>& extr)
: id(lidar_id), extrinsics(extr) {}
std::vector<Object> detect_objects(const LiDARData& data) {
// 在实际应用中，这里会处理点云数据并检测物体
std::cout << "LiDAR " << id << " 正在检测物体，点云包含 "
<< data.point_cloud.size() << " 个点" << std::endl;
// 简化：返回一些模拟的检测结果
std::vector<Object> detected_objects;
// 模拟检测到一辆车
detected_objects.emplace_back(
Object::Type::VEHICLE,
std::array<float, 3>{12.0f, 0.5f, 0.0f}, // 位置
std::array<float, 3>{4.6f, 2.1f, 1.6f}, // 尺寸
0.1f, // 朝向
0.98f // 置信度
);
// 模拟检测到一个骑车人
detected_objects.emplace_back(
Object::Type::CYCLIST,
std::array<float, 3>{8.0f, -3.0f, 0.0f}, // 位置
std::array<float, 3>{1.8f, 0.8f, 1.7f}, // 尺寸
0.2f, // 朝向
0.92f // 置信度
);
std::cout << "LiDAR检测到 " << detected_objects.size() << " 个物体" << std::endl;
return detected_objects;
}
const std::array<double, 6>& get_extrinsics() const {
return extrinsics;
}
};
// GPS传感器
class GPS {
private:
std::array<double, 3> position; // GPS天线在车辆上的位置
public:
GPS(const std::array<double, 3>& pos) : position(pos) {}
std::array<double, 3> get_position(const GPSData& data) {
// 处理GPS数据，获取全球位置
std::cout << "处理GPS数据..." << std::endl;
// 简化：将经纬度转换为局部坐标(实际应使用专业库)
// 这里只是一个非常简化的示例
double x = data.longitude * 111320.0; // 约为每度经度的米数(赤道附近)
double y = data.latitude * 110540.0; // 约为每度纬度的米数
double z = data.altitude;
std::cout << "GPS位置: (" << x << ", " << y << ", " << z << ")" << std::endl;
return {x, y, z};
}
};
// 传感器融合系统
class Fusion {
private:
std::shared_ptr<Camera> camera;
std::shared_ptr<IMU> imu;
std::shared_ptr<LiDAR> lidar;
std::shared_ptr<GPS> gps;
// 物体跟踪状态
std::unordered_map<int, Object> tracked_objects;
int next_tracking_id = 0;
// 当前位置和姿态估计
std::array<double, 3> position = {0.0, 0.0, 0.0};
std::array<double, 3> orientation = {0.0, 0.0, 0.0};
std::array<double, 3> velocity = {0.0, 0.0, 0.0};
double last_update_time = 0.0;
public:
Fusion(std::shared_ptr<Camera> cam, std::shared_ptr<IMU> imu_sensor,
std::shared_ptr<LiDAR> lidar_sensor, std::shared_ptr<GPS> gps_sensor)
: camera(cam), imu(imu_sensor), lidar(lidar_sensor), gps(gps_sensor) {}
void process_camera_data(const CameraData& data) {
// 处理相机数据
std::vector<Object> camera_objects = camera->detect_objects(data);
// 更新跟踪状态(简化)
for (auto& obj : camera_objects) {
update_tracking(obj, data.timestamp);
}
}
void process_imu_data(const IMUData& data) {
// 处理IMU数据
std::array<double, 6> motion = imu->get_motion(data);
// 更新位置和姿态估计
if (last_update_time > 0.0) {
double dt = data.timestamp - last_update_time;
// 更新姿态(简化)
orientation[0] += motion[3] * dt; // roll
orientation[1] += motion[4] * dt; // pitch
orientation[2] += motion[5] * dt; // yaw
// 更新速度(简化)
velocity[0] += motion[0] * dt;
velocity[1] += motion[1] * dt;
velocity[2] += (motion[2] - 9.81) * dt; // 减去重力
// 更新位置(简化)
position[0] += velocity[0] * dt;
position[1] += velocity[1] * dt;
position[2] += velocity[2] * dt;
}
last_update_time = data.timestamp;
}
void process_lidar_data(const LiDARData& data) {
// 处理LiDAR数据
std::vector<Object> lidar_objects = lidar->detect_objects(data);
// 更新跟踪状态
for (auto& obj : lidar_objects) {
update_tracking(obj, data.timestamp);
}
}
void process_gps_data(const GPSData& data) {
// 处理GPS数据
std::array<double, 3> gps_position = gps->get_position(data);
// 融合GPS位置与当前估计(简化的卡尔曼滤波)
double gps_weight = 0.3; // GPS权重
double imu_weight = 0.7; // IMU权重
// 简单加权平均
position[0] = imu_weight * position[0] + gps_weight * gps_position[0];
position[1] = imu_weight * position[1] + gps_weight * gps_position[1];
position[2] = imu_weight * position[2] + gps_weight * gps_position[2];
}
void update_tracking(Object& obj, double timestamp) {
// 简化的目标跟踪逻辑
// 寻找最近的已跟踪目标
int best_match_id = -1;
float min_distance = 5.0f; // 最大匹配距离阈值
for (const auto& pair : tracked_objects) {
const Object& tracked_obj = pair.second;
// 如果类型不同，跳过
if (tracked_obj.type != obj.type) continue;
// 计算距离
float dx = tracked_obj.position[0] - obj.position[0];
float dy = tracked_obj.position[1] - obj.position[1];
float dz = tracked_obj.position[2] - obj.position[2];
float distance = std::sqrt(dx*dx + dy*dy + dz*dz);
if (distance < min_distance) {
min_distance = distance;
best_match_id = pair.first;
}
}
if (best_match_id >= 0) {
// 找到匹配的已跟踪目标，更新
Object& tracked_obj = tracked_objects[best_match_id];
// 计算速度(简化)
float dt = timestamp - last_update_time;
if (dt > 0) {
tracked_obj.velocity[0] = (obj.position[0] - tracked_obj.position[0]) / dt;
tracked_obj.velocity[1] = (obj.position[1] - tracked_obj.position[1]) / dt;
tracked_obj.velocity[2] = (obj.position[2] - tracked_obj.position[2]) / dt;
}
// 更新位置和其他属性
tracked_obj.position = obj.position;
tracked_obj.dimensions = obj.dimensions;
tracked_obj.orientation = obj.orientation;
tracked_obj.confidence = std::max(tracked_obj.confidence, obj.confidence);
// 设置跟踪ID
obj.tracking_id = best_match_id;
} else {
// 创建新的跟踪目标
obj.tracking_id = next_tracking_id++;
tracked_objects[obj.tracking_id] = obj;
}
}
const std::unordered_map<int, Object>& get_tracked_objects() const {
return tracked_objects;
}
std::array<double, 3> get_position() const {
return position;
}
std::array<double, 3> get_orientation() const {
return orientation;
}
std::array<double, 3> get_velocity() const {
return velocity;
}
};
int main() {
// 创建传感器对象
auto camera = std::make_shared<Camera>(
0, // ID
std::array<double, 9>{720.0, 0.0, 320.0, 0.0, 720.0, 240.0, 0.0, 0.0, 1.0}, // 内参
std::array<double, 6>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0} // 外参
);
auto imu = std::make_shared<IMU>(
std::array<double, 3>{0.0, 0.0, 0.0}, // 位置
std::array<double, 3>{0.0, 0.0, 0.0} // 朝向
);
auto lidar = std::make_shared<LiDAR>(
0, // ID
std::array<double, 6>{0.0, 0.0, 1.8, 0.0, 0.0, 0.0} // 外参
);
auto gps = std::make_shared<GPS>(
std::array<double, 3>{0.0, 0.0, 1.5} // 位置
);
// 创建融合系统
Fusion fusion(camera, imu, lidar, gps);
// 模拟传感器数据
double timestamp = 0.0;
// 模拟相机数据
CameraData camera_data(timestamp, nullptr, 640, 480, 3);
// 模拟IMU数据
IMUData imu_data(timestamp, 0.1, 0.0, 9.81, 0.01, 0.02, 0.005);
// 模拟LiDAR数据
std::vector<std::array<float, 3>> point_cloud;
// 添加一些模拟的点云数据
for (int i = 0; i < 1000; ++i) {
point_cloud.push_back({float(i % 20), float((i / 20) % 20 - 10), float(i % 5)});
}
LiDARData lidar_data(timestamp, point_cloud);
// 模拟GPS数据
GPSData gps_data(timestamp, 37.7749, -122.4194, 10.0, 2.5);
// 处理数据
fusion.process_camera_data(camera_data);
fusion.process_imu_data(imu_data);
fusion.process_lidar_data(lidar_data);
fusion.process_gps_data(gps_data);
// 获取跟踪的物体
const auto& tracked_objects = fusion.get_tracked_objects();
std::cout << "跟踪物体数量: " << tracked_objects.size() << std::endl;
for (const auto& pair : tracked_objects) {
const Object& obj = pair.second;
std::cout << "物体ID: " << obj.tracking_id << ", 类型: ";
switch (obj.type) {
case Object::Type::VEHICLE:
std::cout << "车辆";
break;
case Object::Type::PEDESTRIAN:
std::cout << "行人";
break;
case Object::Type::CYCLIST:
std::cout << "骑车人";
break;
case Object::Type::TRAFFIC_SIGN:
std::cout << "交通标志";
break;
default:
std::cout << "未知";
}
std::cout << ", 位置: (" << obj.position[0] << ", " << obj.position[1] << ", " << obj.position[2] << ")"
<< ", 速度: (" << obj.velocity[0] << ", " << obj.velocity[1] << ", " << obj.velocity[2] << ")"
<< ", 置信度: " << obj.confidence
<< std::endl;
}
// 获取自身位置和姿态
auto position = fusion.get_position();
auto orientation = fusion.get_orientation();
auto velocity = fusion.get_velocity();
std::cout << "自身位置: (" << position[0] << ", " << position[1] << ", " << position[2] << ")" << std::endl;
std::cout << "自身姿态: (" << orientation[0] << ", " << orientation[1] << ", " << orientation[2] << ")" << std::endl;
std::cout << "自身速度: (" << velocity[0] << ", " << velocity[1] << ", " << velocity[2] << ")" << std::endl;
return 0;
}
七、传感器同步与校准
1. 时间同步策略
为了准确融合多传感器数据，必须解决不同传感器的时间戳不同步问题。
主要方法：
- 硬件同步：通过硬件触发信号确保同步采集
- 时间戳对齐：将不同传感器数据根据时间戳插值或重采样
- 滑动窗口方法：在时间窗口内收集传感器数据，然后一起处理
import numpy as np
from scipy.interpolate import interp1d
class SensorSynchronizer:
def __init__(self, buffer_size=200):
# 数据缓冲区
self.camera_buffer = [] # [(timestamp, data), ...]
self.imu_buffer = [] # [(timestamp, data), ...]
self.lidar_buffer = [] # [(timestamp, data), ...]
self.gps_buffer = [] # [(timestamp, data), ...]
# 最大缓冲区大小
self.buffer_size = buffer_size
# 上次处理的时间戳
self.last_processed_time = 0
def add_camera_data(self, timestamp, data):
"""添加相机数据到缓冲区"""
self._add_to_buffer(self.camera_buffer, timestamp, data)
def add_imu_data(self, timestamp, data):
"""添加IMU数据到缓冲区"""
self._add_to_buffer(self.imu_buffer, timestamp, data)
def add_lidar_data(self, timestamp, data):
"""添加LiDAR数据到缓冲区"""
self._add_to_buffer(self.lidar_buffer, timestamp, data)
def add_gps_data(self, timestamp, data):
"""添加GPS数据到缓冲区"""
self._add_to_buffer(self.gps_buffer, timestamp, data)
def _add_to_buffer(self, buffer, timestamp, data):
"""添加数据到指定缓冲区"""
buffer.append((timestamp, data))
buffer.sort(key=lambda x: x[0]) # 按时间戳排序
# 保持缓冲区大小
if len(buffer) > self.buffer_size:
buffer.pop(0)
def get_synchronized_data(self, target_time=None, max_time_diff=0.1):
"""获取同步的传感器数据
Args:
target_time: 目标时间戳，如果为None则使用最新数据时间
max_time_diff: 允许的最大时间差(秒)
Returns:
同步后的传感器数据字典，如果某传感器数据不可用则为None
"""
# 如果未指定目标时间，使用最新数据的时间
if target_time is None:
timestamps = []
if self.camera_buffer: timestamps.append(self.camera_buffer[-1][0])
if self.imu_buffer: timestamps.append(self.imu_buffer[-1][0])
if self.lidar_buffer: timestamps.append(self.lidar_buffer[-1][0])
if self.gps_buffer: timestamps.append(self.gps_buffer[-1][0])
if not timestamps:
return None # 没有可用数据
target_time = min(timestamps) # 使用最早的最新数据时间
# 确保目标时间比上次处理的时间更新
if target_time <= self.last_processed_time:
return None
self.last_processed_time = target_time
# 获取同步数据
camera_data = self._get_data_at_time(self.camera_buffer, target_time, max_time_diff)
imu_data = self._interpolate_imu_data(target_time, max_time_diff)
lidar_data = self._get_data_at_time(self.lidar_buffer, target_time, max_time_diff)
gps_data = self._get_data_at_time(self.gps_buffer, target_time, max_time_diff)
return {
'timestamp': target_time,
'camera': camera_data,
'imu': imu_data,
'lidar': lidar_data,
'gps': gps_data
}
def _get_data_at_time(self, buffer, target_time, max_time_diff):
"""获取指定时间的数据，如果时间差太大则返回None"""
if not buffer:
return None
# 找到最接近目标时间的数据
closest_data = None
min_time_diff = float('inf')
for timestamp, data in buffer:
time_diff = abs(timestamp - target_time)
if time_diff < min_time_diff:
min_time_diff = time_diff
closest_data = data
# 如果时间差太大，返回None
if min_time_diff > max_time_diff:
return None
return closest_data
def _interpolate_imu_data(self, target_time, max_time_diff):
"""插值计算目标时间的IMU数据"""
if len(self.imu_buffer) < 2:
return self._get_data_at_time(self.imu_buffer, target_time, max_time_diff)
# 找到目标时间附近的两个IMU数据点
before_idx = None
after_idx = None
for i, (timestamp, _) in enumerate(self.imu_buffer):
if timestamp <= target_time:
before_idx = i
if timestamp >= target_time and after_idx is None:
after_idx = i
# 检查是否有足够数据进行插值
if before_idx is None or after_idx is None or before_idx == after_idx:
return self._get_data_at_time(self.imu_buffer, target_time, max_time_diff)
t1, data1 = self.imu_buffer[before_idx]
t2, data2 = self.imu_buffer[after_idx]
# 检查时间差是否在可接受范围内
if target_time - t1 > max_time_diff or t2 - target_time > max_time_diff:
return self._get_data_at_time(self.imu_buffer, target_time, max_time_diff)
# 线性插值
alpha = (target_time - t1) / (t2 - t1)
# 假设IMU数据有acc和gyro字段
interpolated_data = {
'accelerometer': {
'x': data1['accelerometer']['x'] * (1 - alpha) + data2['accelerometer']['x'] * alpha,
'y': data1['accelerometer']['y'] * (1 - alpha) + data2['accelerometer']['y'] * alpha,
'z': data1['accelerometer']['z'] * (1 - alpha) + data2['accelerometer']['z'] * alpha
},
'gyroscope': {
'x': data1['gyroscope']['x'] * (1 - alpha) + data2['gyroscope']['x'] * alpha,
'y': data1['gyroscope']['y'] * (1 - alpha) + data2['gyroscope']['y'] * alpha,
'z': data1['gyroscope']['z'] * (1 - alpha) + data2['gyroscope']['z'] * alpha
}
}
return interpolated_data
def clear_old_data(self, time_threshold):
"""清除早于指定时间的数据"""
self._clear_old_data_from_buffer(self.camera_buffer, time_threshold)
self._clear_old_data_from_buffer(self.imu_buffer, time_threshold)
self._clear_old_data_from_buffer(self.lidar_buffer, time_threshold)
self._clear_old_data_from_buffer(self.gps_buffer, time_threshold)
def _clear_old_data_from_buffer(self, buffer, time_threshold):
"""从指定缓冲区清除早于阈值的数据"""
while buffer and buffer[0][0] < time_threshold:
buffer.pop(0)
2. 空间校准方法
不同传感器之间需要进行空间校准，确定它们之间的位置和方向关系，这种关系通常用外参(extrinsic parameters)表示。
主要校准方法：
- 目标板校准：使用特殊标定板同时被多个传感器观测
- 相互信息最大化：通过最大化不同传感器数据的相互信息获取变换
- 手眼校准：机器人领域常用的校准方法
import numpy as np
import cv2
class SensorCalibrator:
def __init__(self):
# 校准结果
self.camera_to_lidar = None # 相机到LiDAR的变换
self.camera_to_imu = None # 相机到IMU的变换
self.imu_to_gps = None # IMU到GPS的变换
def calibrate_camera_lidar(self, image_points, lidar_points):
"""校准相机与LiDAR
Args:
image_points: 图像中检测到的特征点(2D)
lidar_points: 对应的LiDAR点(3D)
Returns:
变换矩阵，从LiDAR坐标系到相机坐标系
"""
assert len(image_points) == len(lidar_points), "点对数量不匹配"
assert len(image_points) >= 6, "需要至少6个点对进行PnP求解"
# 假设相机内参已知
camera_matrix = np.array([[720.0, 0, 320.0], [0, 720.0, 240.0], [0, 0, 1]])
dist_coeffs = np.zeros(4) # 假设无畸变
# 使用PnP算法求解相机位姿
_, rvec, tvec = cv2.solvePnP(
np.array(lidar_points, dtype=np.float32),
np.array(image_points, dtype=np.float32),
camera_matrix, dist_coeffs
)
# 将旋转向量转换为旋转矩阵
R, _ = cv2.Rodrigues(rvec)
# 构建变换矩阵
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = tvec.squeeze()
self.camera_to_lidar = T
return T
def calibrate_camera_imu(self, camera_poses, imu_poses):
"""校准相机与IMU
Args:
camera_poses: 相机位姿列表 [T1, T2, ...]
imu_poses: 对应的IMU位姿列表 [T1, T2, ...]
Returns:
从IMU坐标系到相机坐标系的变换矩阵
"""
# 使用手眼校准方法
# 实际应用中应使用更复杂的算法和多次采样优化
# 简化版本：使用平均变换
transforms = []
for cam_pose, imu_pose in zip(camera_poses, imu_poses):
# 计算从IMU到相机的变换
# T_cam = T_imu * T_imu_to_cam
# 因此 T_imu_to_cam = inv(T_imu) * T_cam
T_imu_to_cam = np.linalg.inv(imu_pose) @ cam_pose
transforms.append(T_imu_to_cam)
# 计算平均变换(简化)
# 实际应该使用SE3流形上的平均
avg_transform = np.mean(np.array(transforms), axis=0)
# 确保旋转部分是有效的旋转矩阵
U, _, Vt = np.linalg.svd(avg_transform[:3, :3])
avg_transform[:3, :3] = U @ Vt
self.camera_to_imu = avg_transform
return avg_transform
def calibrate_imu_gps(self, imu_trajectory, gps_trajectory):
"""校准IMU与GPS
Args:
imu_trajectory: IMU轨迹点列表 [(x1,y1,z1), (x2,y2,z2), ...]
gps_trajectory: 对应的GPS轨迹点列表 [(x1,y1,z1), (x2,y2,z2), ...]
Returns:
从GPS坐标系到IMU坐标系的变换
"""
# 使用Umeyama算法求解刚体变换
# 在实际应用中，GPS和IMU轨迹需要先在时间上同步
imu_points = np.array(imu_trajectory)
gps_points = np.array(gps_trajectory)
# 计算质心
imu_centroid = np.mean(imu_points, axis=0)
gps_centroid = np.mean(gps_points, axis=0)
# 去中心化
imu_centered = imu_points - imu_centroid
gps_centered = gps_points - gps_centroid
# 计算协方差矩阵
H = imu_centered.T @ gps_centered
# SVD分解
U, _, Vt = np.linalg.svd(H)
# 计算旋转矩阵
R = U @ Vt
# 确保是有效的旋转矩阵(行列式为1)
if np.linalg.det(R) < 0:
Vt[-1, :] *= -1
R = U @ Vt
# 计算平移向量
t = gps_centroid - R @ imu_centroid
# 构建变换矩阵
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = t
self.imu_to_gps = T
return T
def transform_point(self, point, source_frame, target_frame):
"""将点从源坐标系转换到目标坐标系
Args:
point: 3D点坐标 [x, y, z]
source_frame: 源坐标系名称 ('camera', 'lidar', 'imu', 'gps')
target_frame: 目标坐标系名称 ('camera', 'lidar', 'imu', 'gps')
Returns:
目标坐标系中的点坐标
"""
# 确保所有校准都已完成
if self.camera_to_lidar is None or self.camera_to_imu is None or self.imu_to_gps is None:
raise ValueError("未完成所有传感器校准")
# 创建从每个坐标系到基准坐标系(这里选相机)的变换
transforms = {
'camera': np.eye(4),
'lidar': np.linalg.inv(self.camera_to_lidar),
'imu': np.linalg.inv(self.camera_to_imu),
'gps': np.linalg.inv(self.camera_to_imu) @ np.linalg.inv(self.imu_to_gps)
}
# 将点转换为齐次坐标
p_homogeneous = np.append(point, 1)
# 从源坐标系转换到基准坐标系(相机)
p_camera = transforms[source_frame] @ p_homogeneous
# 从基准坐标系转换到目标坐标系
p_target = np.linalg.inv(transforms[target_frame]) @ p_camera
# 返回非齐次坐标
return p_target[:3]
八、实际应用案例
1. 移动机器人导航系统
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Ellipse
import math
import time
class NavigationSystem:
def __init__(self):
# 状态向量 [x, y, theta, v, omega]
self.state = np.zeros(5)
# 状态协方差
self.P = np.diag([1.0, 1.0, 0.1, 0.1, 0.1])
# 地图(简化为障碍物列表)
self.obstacles = []
# 规划的路径
self.path = []
# 最后更新时间
self.last_update_time = None
# 传感器融合系统
self.sensor_fusion = SensorFusion()
# 路径规划器
self.path_planner = PathPlanner()
# 控制器
self.controller = Controller()
# 历史轨迹
self.trajectory = []
def update_map(self, obstacles):
"""更新地图"""
self.obstacles = obstacles
def set_goal(self, goal_position):
"""设置目标位置"""
# 使用路径规划器规划路径
self.path = self.path_planner.plan_path(
self.state[:2], # 当前位置
goal_position, # 目标位置
self.obstacles # 地图障碍物
)
def process_sensor_data(self, camera_data=None, imu_data=None, lidar_data=None, gps_data=None):
"""处理传感器数据"""
# 更新时间
current_time = time.time()
dt = 0.0
if self.last_update_time is not None:
dt = current_time - self.last_update_time
self.last_update_time = current_time
# 传感器融合
pose_update = self.sensor_fusion.update(
camera_data, imu_data, lidar_data, gps_data, dt)
# 更新状态
if pose_update is not None:
self.state[:3] = pose_update[:3] # 更新位置和朝向
self.state[3:] = pose_update[3:] # 更新速度和角速度
# 更新协方差
self.P = self.sensor_fusion.get_covariance()
# 记录轨迹
self.trajectory.append(self.state[:2].copy())
def update_control(self):
"""更新控制指令"""
if not self.path:
return 0.0, 0.0 # 没有路径，停止
# 获取当前位置和朝向
x, y, theta = self.state[:3]
# 找到当前路径上的目标点
target_point = self._get_current_target()
# 计算控制指令
v, omega = self.controller.compute_control(
np.array([x, y]),
theta,
target_point,
self.state[3], # 当前线速度
self.state[4] # 当前角速度
)
return v, omega
def _get_current_target(self):
"""获取当前跟踪的路径点"""
if not self.path:
return None
# 简单策略：选择路径上距离当前位置一定距离的点
# 更复杂的策略可以基于时间或曲率等因素
current_pos = self.state[:2]
# 找到最近的路径点
distances = [np.linalg.norm(current_pos - np.array(p)) for p in self.path]
min_idx = np.argmin(distances)
# 选择前方一定距离的点作为目标
lookahead_distance = 1.0 # 前视距离
for i in range(min_idx, len(self.path)):
dist = np.linalg.norm(current_pos - np.array(self.path[i]))
if dist >= lookahead_distance:
return np.array(self.path[i])
# 如果没有符合条件的点，返回终点
return np.array(self.path[-1])
def visualize(self):
"""可视化当前状态"""
plt.figure(figsize=(10, 8))
# 绘制障碍物
for obs in self.obstacles:
circle = plt.Circle((obs[0], obs[1]), obs[2], color='red', alpha=0.5)
plt.gca().add_artist(circle)
# 绘制路径
if self.path:
path_x = [p[0] for p in self.path]
path_y = [p[1] for p in self.path]
plt.plot(path_x, path_y, 'g--', label='Planned Path')
# 绘制历史轨迹
if self.trajectory:
traj_x = [p[0] for p in self.trajectory]
traj_y = [p[1] for p in self.trajectory]
plt.plot(traj_x, traj_y, 'b-', linewidth=2, label='Trajectory')
# 绘制当前位置
x, y, theta = self.state[:3]
plt.plot(x, y, 'ko', markersize=10, label='Robot')
# 画出方向箭头
arrow_length = 0.5
plt.arrow(x, y, arrow_length * math.cos(theta), arrow_length * math.sin(theta),
head_width=0.2, head_length=0.2, fc='k', ec='k')
# 绘制不确定性椭圆
cov = self.P[:2, :2] # 位置的协方差
eigenvals, eigenvecs = np.linalg.eig(cov)
# 椭圆参数
angle = np.degrees(np.arctan2(eigenvecs[1, 0], eigenvecs[0, 0]))
width, height = 2 * np.sqrt(eigenvals) # 95%置信区间
ellipse = Ellipse(xy=(x, y), width=width, height=height, angle=angle,
alpha=0.3, color='blue', label='Uncertainty')
plt.gca().add_artist(ellipse)
# 设置图表
plt.axis('equal')
plt.grid(True)
plt.legend()
plt.title('Robot Navigation Visualization')
plt.xlabel('X (m)')
plt.ylabel('Y (m)')
plt.show()
class SensorFusion:
def __init__(self):
# 状态向量 [x, y, theta, v, omega]
self.state = np.zeros(5)
# 状态协方差
self.P = np.diag([1.0, 1.0, 0.1, 0.1, 0.1])
# 过程噪声
self.Q = np.diag([0.01, 0.01, 0.01, 0.1, 0.1])
# 初始化滤波器(使用EKF)
self.initialize_ekf()
def initialize_ekf(self):
"""初始化EKF滤波器参数"""
pass # 此处简化，实际应包含雅可比矩阵计算等
def update(self, camera_data, imu_data, lidar_data, gps_data, dt):
"""更新状态估计"""
# 预测步骤
self._predict(dt)
# 更新步骤 - 处理各传感器数据
if camera_data is not None:
self._update_with_camera(camera_data)
if imu_data is not None:
self._update_with_imu(imu_data)
if lidar_data is not None:
self._update_with_lidar(lidar_data)
if gps_data is not None:
self._update_with_gps(gps_data)
return self.state
def _predict(self, dt):
"""EKF预测步骤"""
# 简化的运动模型
x, y, theta, v, omega = self.state
# 预测状态
x_new = x + v * math.cos(theta) * dt
y_new = y + v * math.sin(theta) * dt
theta_new = theta + omega * dt
# 简化：保持速度不变
v_new = v
omega_new = omega
self.state = np.array([x_new, y_new, theta_new, v_new, omega_new])
# 计算雅可比矩阵(简化)
F = np.eye(5)
F[0, 2] = -v * math.sin(theta) * dt
F[0, 3] = math.cos(theta) * dt
F[1, 2] = v * math.cos(theta) * dt
F[1, 3] = math.sin(theta) * dt
F[2, 4] = dt
# 更新协方差
self.P = F @ self.P @ F.T + self.Q * dt
def _update_with_camera(self, camera_data):
"""使用相机数据更新"""
# 实现相机观测模型和EKF更新
pass # 此处简化
def _update_with_imu(self, imu_data):
"""使用IMU数据更新"""
# 提取加速度和角速度
acc = np.array([imu_data['accelerometer']['x'],
imu_data['accelerometer']['y'],
imu_data['accelerometer']['z']])
gyro = np.array([imu_data['gyroscope']['x'],
imu_data['gyroscope']['y'],
imu_data['gyroscope']['z']])
# 构建观测矩阵(简化)
H = np.zeros((2, 5))
H[0, 3] = 1.0 # 速度
H[1, 4] = 1.0 # 角速度
# 观测值
z = np.array([np.linalg.norm(acc) - 9.81, gyro[2]]) # 简化
# 预测的观测值
z_pred = np.array([self.state[3], self.state[4]])
# 观测噪声
R = np.diag([0.5, 0.1])
# 计算卡尔曼增益
S = H @ self.P @ H.T + R
K = self.P @ H.T @ np.linalg.inv(S)
# 更新状态
innovation = z - z_pred
self.state = self.state + K @ innovation
# 更新协方差
self.P = (np.eye(5) - K @ H) @ self.P
def _update_with_lidar(self, lidar_data):
"""使用LiDAR数据更新"""
# 实现LiDAR观测模型和EKF更新
pass # 此处简化
def _update_with_gps(self, gps_data):
"""使用GPS数据更新"""
# 提取位置
gps_position = np.array([gps_data['x'], gps_data['y'], 0])
# 构建观测矩阵
H = np.zeros((2, 5))
H[0, 0] = 1.0 # x位置
H[1, 1] = 1.0 # y位置
# 观测值
z = gps_position[:2]
# 预测的观测值
z_pred = self.state[:2]
# 观测噪声(根据GPS精度)
R = np.eye(2) * (gps_data['accuracy'] ** 2)
# 计算卡尔曼增益
S = H @ self.P @ H.T + R
K = self.P @ H.T @ np.linalg.inv(S)
# 更新状态
innovation = z - z_pred
self.state = self.state + K @ innovation
# 更新协方差
self.P = (np.eye(5) - K @ H) @ self.P
def get_covariance(self):
"""获取当前状态协方差"""
return self.P
class PathPlanner:
def __init__(self):
pass
def plan_path(self, start, goal, obstacles):
"""规划从起点到终点的路径，避开障碍物
这里实现一个简化版的RRT(Rapidly-exploring Random Tree)算法
"""
# 初始化RRT树
tree = {tuple(start): None} # 节点: 父节点
# 最大迭代次数
max_iterations = 1000
# 步长
step_size = 0.5
# 搜索
for _ in range(max_iterations):
# 生成随机点(有概率直接取目标点)
if np.random.random() < 0.1:
random_point = goal
else:
random_point = np.random.uniform(
low=[start[0] - 10, start[1] - 10],
high=[start[0] + 10, start[1] + 10],
size=2
)
# 找到树中最近的节点
nearest_node = self._find_nearest(tree, random_point)
# 计算新节点
direction = random_point - np.array(nearest_node)
norm = np.linalg.norm(direction)
if norm > 0:
direction = direction / norm
new_node = np.array(nearest_node) + direction * step_size
# 检查新节点是否有效(无碰撞)
if self._is_valid(nearest_node, new_node, obstacles):
tree[tuple(new_node)] = nearest_node
# 检查是否可以连接到目标
if np.linalg.norm(new_node - goal) < step_size and \
self._is_valid(new_node, goal, obstacles):
tree[tuple(goal)] = tuple(new_node)
break
# 提取路径
path = self._extract_path(tree, start, goal)
# 平滑路径
smoothed_path = self._smooth_path(path, obstacles)
return smoothed_path
def _find_nearest(self, tree, point):
"""找到树中距离给定点最近的节点"""
return min(tree.keys(), key=lambda node: np.linalg.norm(np.array(node) - point))
def _is_valid(self, node1, node2, obstacles):
"""检查两节点之间的路径是否无碰撞"""
# 简化：检查线段是否与任何障碍物相交
for obs in obstacles:
obs_pos = np.array([obs[0], obs[1]])
obs_radius = obs[2]
# 使用点到线段的最小距离
p1 = np.array(node1)
p2 = np.array(node2)
# 计算线段长度
segment_length = np.linalg.norm(p2 - p1)
if segment_length == 0:
# 如果两点重合，直接计算点到障碍物的距离
distance = np.linalg.norm(p1 - obs_pos)
if distance < obs_radius:
return False
continue
# 计算投影点
t = max(0, min(1, np.dot(obs_pos - p1, p2 - p1) / (segment_length ** 2)))
projection = p1 + t * (p2 - p1)
# 计算距离
distance = np.linalg.norm(obs_pos - projection)
# 检查碰撞
if distance < obs_radius:
return False
return True
def _extract_path(self, tree, start, goal):
"""从树中提取路径"""
path = [goal]
current = tuple(goal)
while current != tuple(start):
current = tree[current]
path.append(current)
return path[::-1] # 反转路径，从起点到终点
def _smooth_path(self, path, obstacles):
"""平滑路径"""
# 简化：删除冗余点
i = 0
smoothed_path = [path[0]]
while i < len(path) - 1:
current = path[i]
# 尝试连接当前点与更远的点
for j in range(len(path) - 1, i, -1):
if self._is_valid(current, path[j], obstacles):
smoothed_path.append(path[j])
i = j
break
else:
i += 1
smoothed_path.append(path[i])
return smoothed_path
class Controller:
def __init__(self):
# 控制器参数
self.linear_gain = 0.5
self.angular_gain = 1.0
self.max_linear_speed = 1.0
self.max_angular_speed = 0.5
def compute_control(self, current_pos, current_theta, target_pos, current_v, current_omega):
"""计算控制指令"""
# 计算到目标的距离
distance = np.linalg.norm(target_pos - current_pos)
# 计算目标朝向
target_theta = math.atan2(target_pos[1] - current_pos[1], target_pos[0] - current_pos[0])
# 计算朝向误差(处理角度循环)
angle_error = target_theta - current_theta
angle_error = math.atan2(math.sin(angle_error), math.cos(angle_error))
# 简单的P控制器
v = self.linear_gain * distance
omega = self.angular_gain * angle_error
# 限制速度
v = max(0.0, min(v, self.max_linear_speed))
omega = max(-self.max_angular_speed, min(omega, self.max_angular_speed))
return v, omega
# 使用示例
def main():
# 创建导航系统
nav_system = NavigationSystem()
# 设置地图(简化为圆形障碍物:[x, y, radius])
obstacles = [
[5, 5, 1.0],
[8, 3, 1.5],
[2, 7, 0.8]
]
nav_system.update_map(obstacles)
# 设置目标
goal = [10, 10]
nav_system.set_goal(goal)
# 模拟传感器数据和导航过程
for t in range(100):
# 模拟传感器数据
camera_data = None # 简化示例不使用相机
imu_data = {
'accelerometer': {'x': 0.1, 'y': 0.05, 'z': 9.81},
'gyroscope': {'x': 0.01, 'y': 0.02, 'z': 0.03}
}
lidar_data = None # 简化示例不使用激光雷达
gps_data = {
'x': nav_system.state[0] + np.random.normal(0, 0.3),
'y': nav_system.state[1] + np.random.normal(0, 0.3),
'accuracy': 0.5
}
# 处理传感器数据
nav_system.process_sensor_data(camera_data, imu_data, lidar_data, gps_data)
# 计算控制指令
v, omega = nav_system.update_control()
# 模拟机器人运动(简化)
dt = 0.1 # 时间步长
nav_system.state[0] += v * math.cos(nav_system.state[2]) * dt
nav_system.state[1] += v * math.sin(nav_system.state[2]) * dt
nav_system.state[2] += omega * dt
nav_system.state[3] = v
nav_system.state[4] = omega
# 检查是否到达目标
if np.linalg.norm(nav_system.state[:2] - np.array(goal)) < 0.5:
print("到达目标!")
break
# 可视化结果
nav_system.visualize()
if __name__ == "__main__":
main()
2. AR/VR 头显定位系统
#include <iostream>
#include <vector>
#include <array>
#include <deque>
#include <chrono>
#include <cmath>
#include <Eigen/Dense>
// 简化版AR/VR头显定位系统
class HMDTrackingSystem {
private:
// 状态变量
Eigen::Vector3d position; // 位置
Eigen::Quaterniond orientation; // 朝向(四元数)
Eigen::Vector3d velocity; // 速度
Eigen::Vector3d angular_velocity; // 角速度
// 协方差矩阵
Eigen::MatrixXd P;
// 传感器误差
double camera_noise; // 相机定位噪声(米)
double imu_accel_noise; // IMU加速度噪声(m/s^2)
double imu_gyro_noise; // IMU角速度噪声(rad/s)
// IMU偏差
Eigen::Vector3d accel_bias; // 加速度计偏差
Eigen::Vector3d gyro_bias; // 陀螺仪偏差
// 历史数据缓冲
struct TimeStampedPose {
double timestamp;
Eigen::Vector3d position;
Eigen::Quaterniond orientation;
};
std::deque<TimeStampedPose> pose_history;
double pose_history_max_age = 0.5; // 最长保存0.5秒
// 上次更新时间
double last_update_time;
// 是否已初始化
bool initialized = false;
// 跟踪状态
enum class TrackingState {
TRACKING_GOOD, // 跟踪良好
TRACKING_LIMITED, // 跟踪受限(仅IMU)
TRACKING_LOST // 跟踪丢失
};
TrackingState tracking_state = TrackingState::TRACKING_LOST;
// 帧计数和统计
int frame_count = 0;
double total_processing_time = 0.0;
public:
HMDTrackingSystem()
: position(Eigen::Vector3d::Zero()),
orientation(Eigen::Quaterniond::Identity()),
velocity(Eigen::Vector3d::Zero()),
angular_velocity(Eigen::Vector3d::Zero()),
camera_noise(0.01),
imu_accel_noise(0.05),
imu_gyro_noise(0.01),
accel_bias(Eigen::Vector3d::Zero
北京人形旗下天工造物具身智能开源社区，聚焦具身天工与慧思开物两大平台
更多推荐
25
0- 0
已为社区贡献33条内容
所有评论(0)