简介:本资源是一份面向导航算法研究者与MATLAB实践者的EKF协同导航入门级代码实现,聚焦双艇主从式协同定位场景,解决非线性系统下多源传感器融合与状态估计难题。压缩包仅含1个核心文件ekf.m(MATLAB脚本),体积仅2KB,完整实现了扩展卡尔曼滤波的状态预测、雅可比矩阵线性化、量测更新及主从信息交互逻辑,适用于GPS/IMU组合导航仿真与教学验证。已有409人学习下载,读者可直接运行该脚本,深入理解EKF在协同导航中的建模思路、主从角色分工、距离量测融合机制及误差抑制效果,特别适合控制理论、无人系统或惯性导航方向的初学者开展算法复现与原理验证。
1. EKF协同导航不是“把两个传感器简单加起来”,而是主从式结构下状态估计的动态耦合重构
很多人第一次接触“EKF协同导航”时,会下意识把它理解成“用EKF融合GPS和IMU数据”——这没错,但仅限于单平台。而标题中明确出现的“主从式结构”“协同导航”指向的是多载体系统:比如一个高精度GNSS/INS主车(Master)实时播发修正信息,多个低成本从车(Slave)接收并融合自身传感器数据,实现全局一致、局部鲁棒的联合定位。这种架构在无人车队编队、AGV集群调度、无人机群协同测绘中已成标配。它解决的核心矛盾是:单个从节点因传感器廉价(如MEMS IMU+单频GPS)导致航迹漂移快、绝对精度低,但又不能每台都配昂贵的RTK或光纤惯导。EKF在此不是孤立滤波器,而是构建了一个跨节点的状态误差传播模型——主节点的位姿误差会通过观测方程影响从节点的状态更新权重,从节点的相对观测(如UWB测距、视觉特征匹配、激光ICP位移)又反向约束主节点的协方差收缩。本文聚焦如何用经典EKF框架,在无ROS2依赖、不调用nav2或move_base的前提下,从零搭建可验证的主从协同导航最小闭环。所有代码基于Python 3.9+NumPy实现,适配嵌入式部署场景,参数表与协方差调试逻辑均来自实车标定经验。
2. 主从式EKF协同导航的数学建模:为什么必须显式定义主-从状态耦合项
2.1 协同导航状态向量设计:打破单体EKF的维度惯性
传统单载体EKF状态向量常为 $ \mathbf{x} = [p_x, p_y, p_z, v_x, v_y, v_z, \phi, \theta, \psi, b_{a_x}, b_{a_y}, b_{a_z}, b_{g_x}, b_{g_y}, b_{g_z}]^T $(15维),但主从协同必须扩展为分块联合状态。设主节点状态为 $ \mathbf{x}m $,第 $ i $ 个从节点状态为 $ \mathbf{x}{s_i} $,则全局状态向量为:
$$ \mathbf{x} = \left[ \mathbf{x}m^T,\ \mathbf{x}{s_1}^T,\ \dots,\ \mathbf{x}_{s_n}^T \right]^T $$
关键在于:不能直接拼接。因为从节点间无直接观测,若强行合并会导致雅可比矩阵病态、协方差矩阵稀疏性丧失。正确做法是采用主-从分层状态结构:
- 主节点状态 $ \mathbf{x}_m $:含位置、速度、姿态、IMU零偏(15维)
- 每个从节点状态 $ \mathbf{x}_{s_i} $:仅含相对于主节点的相对位姿$ \Delta \mathbf{p}_i = [dx_i, dy_i, dz_i, d\phi_i, d\theta_i, d\psi_i]^T $(6维) + 自身IMU零偏(6维),共12维
- 全局状态维度 = $ 15 + n \times 12 $
提示:选择相对位姿而非绝对位姿作为从节点状态,本质是将全局可观测性问题转化为局部可观测性问题。主节点提供绝对参考系,从节点只关心“我在主车哪边、多远、朝向如何”,大幅降低状态维度与计算负载,且天然抑制全局漂移累积。
2.2 系统动力学模型:主节点独立演化,从节点受主节点运动驱动
主节点运动模型沿用标准IMU预积分模型:
$$ \dot{\mathbf{x}}_m = f_m(\mathbf{x}_m, \mathbf{u}_m) + \mathbf{w}_m $$
其中 $ \mathbf{u}_m $ 为IMU原始测量(加速度 $ \mathbf{a}_m $、角速度 $ \boldsymbol{\omega}_m $),$ \mathbf{w}_m $ 为过程噪声。
从节点动力学模型必须体现“主-从耦合”:
$$ \dot{\mathbf{x}}{s_i} = f{s_i}(\mathbf{x}{s_i}, \mathbf{x}m, \mathbf{u}{s_i}) + \mathbf{w}{s_i} $$
核心项是相对位姿的微分方程。设主节点在世界坐标系下的旋转矩阵为 $ \mathbf{R}_m $,从节点相对主节点的位置为 $ \Delta \mathbf{p}_i $,则其在世界系下的速度为:
$$ \dot{\Delta \mathbf{p}}_i = \mathbf{v}m - \mathbf{v}{s_i} + \boldsymbol{\omega}_m \times \Delta \mathbf{p}_i $$
即:从节点相对主节点的速度变化 = 主节点速度 - 从节点自身速度 + 主节点旋转引起的科里奥利效应。此式强制将主节点运动学嵌入从节点预测中,是协同性的数学根源。
2.3 观测模型设计:三类典型协同观测及其雅可比推导
协同导航的观测不依赖外部绝对基准(如GPS),而依赖节点间相对关系。常见三类:
| 观测类型 | 观测方程 $ \mathbf{h}(\mathbf{x}) $ | 关键雅可比项 $ \frac{\partial \mathbf{h}}{\partial \mathbf{x}} $ | 物理意义 |
|---|---|---|---|
| UWB测距 | $ z_{ij} = | \mathbf{R}_m \Delta \mathbf{p}_i - \mathbf{R}_m \Delta \mathbf{p}_j | $ | 对 $ \Delta \mathbf{p}_i $、$ \Delta \mathbf{p}_j $ 的偏导含 $ \mathbf{R}_m $ 旋转 | 直接约束从节点间相对距离 |
| 视觉特征匹配 | $ z_{ik} = \pi( \mathbf{R}_m \Delta \mathbf{p}_i + \mathbf{t}_m ) $ | 对 $ \Delta \mathbf{p}_i $ 偏导含相机投影雅可比 $ \frac{\partial \pi}{\partial \mathbf{p}} $ | 将从节点相对位姿映射到主节点相机图像平面 |
| 激光ICP位移 | $ z_{i}^{lidar} = \mathbf{R}_m \Delta \mathbf{p}_i + \mathbf{t}_m $ | 对 $ \Delta \mathbf{p}_i $ 偏导为 $ \mathbf{R}_m $ | 主节点激光SLAM位姿作为从节点运动先验 |
注意:所有观测方程中,主节点位姿 $ \mathbf{x}_m $ 都作为“已知输入”参与计算,但其不确定性通过协方差传播影响从节点更新。这正是EKF协同区别于集中式滤波的关键——主节点状态不被从节点观测直接修正,但其协方差会调制从节点卡尔曼增益。
3. Python实现主从式EKF协同导航:从状态初始化到在线更新的完整闭环
3.1 状态与协方差初始化:主节点高置信度,从节点宽泛先验
import numpy as np def init_state_and_covariance(n_slaves=2): # 主节点状态:[px, py, pz, vx, vy, vz, roll, pitch, yaw, ba_x, ba_y, ba_z, bg_x, bg_y, bg_z] x_m = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # 从节点状态:[dx, dy, dz, droll, dpitch, dyaw, ba_x, ba_y, ba_z, bg_x, bg_y, bg_z] x_s_list = [] for i in range(n_slaves): x_s = np.array([1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # 初始相对位置(1,0,0) x_s_list.append(x_s) # 全局状态向量 x = np.hstack([x_m] + x_s_list) # 协方差矩阵:主节点精度高,从节点初始不确定性大 P = np.zeros((len(x), len(x))) # 主节点协方差块(15x15):位置0.1m²,速度0.01(m/s)²,姿态0.01rad²,零偏1e-4 P_m = np.diag([ 0.01, 0.01, 0.01, # pos 0.0001, 0.0001, 0.0001, # vel 0.0001, 0.0001, 0.0001, # ori (rad²) 1e-4, 1e-4, 1e-4, # acc bias 1e-5, 1e-5, 1e-5 # gyro bias ]) # 从节点协方差块(12x12):相对位置1.0m²,姿态0.1rad²,零偏1e-3 P_s = np.diag([ 1.0, 1.0, 1.0, # rel pos 0.01, 0.01, 0.01, # rel ori 1e-3, 1e-3, 1e-3, # acc bias 1e-4, 1e-4, 1e-4 # gyro bias ]) # 组装全局P:对角块填充 start_m = 0 P[start_m:start_m+15, start_m:start_m+15] = P_m for i in range(n_slaves): start_s = 15 + i*12 P[start_s:start_s+12, start_s:start_s+12] = P_s return x, P x, P = init_state_and_covariance(n_slaves=2) print(f"Initial state dim: {x.shape}, Covariance shape: {P.shape}")这段代码定义了可复现的初始化范式:主节点协方差对角元体现其高精度(如位置方差0.01对应10cm标准差),从节点相对位置方差设为1.0(1m标准差),反映初始定位粗略。关键点在于:P必须是分块对角矩阵,主-从、从-从之间初始协方差为0,表示无先验相关性——后续EKF更新会通过观测雅可比自然引入耦合。
3.2 系统雅可比矩阵F:主节点独立,从节点显式依赖主状态
def compute_jacobian_F(x, dt, n_slaves=2): """ 计算离散化系统雅可比 F = ∂f/∂x x: [x_m, x_s1, x_s2, ...] 返回大小为 (state_dim, state_dim) 的矩阵 """ state_dim = len(x) F = np.eye(state_dim) # 离散化近似为 I + F_cont * dt # 解析主节点状态索引 idx_m = slice(0, 15) x_m = x[idx_m] # 主节点雅可比(标准IMU预积分线性化,此处简化为单位阵+噪声项) # 实际应用中需根据IMU模型计算 ∂f_m/∂x_m,此处省略细节,聚焦主-从耦合 # 主节点部分保持为 eye(15),因其动力学不显式依赖从节点状态 # 从节点雅可比:关键在 ∂f_s/∂x_m 和 ∂f_s/∂x_s for i in range(n_slaves): idx_s = slice(15 + i*12, 15 + (i+1)*12) x_s = x[idx_s] # 相对位置 dx,dy,dz 的雅可比:依赖主节点速度 v_m 和角速度 ω_m # 由公式 dot(dx) = v_mx - v_sx + (ω_my * dz - ω_mz * dy) 推导 v_m = x_m[3:6] # 主节点速度 omega_m = x_m[12:15] # 主节点角速度(零偏已补偿,此处简化) # 对主节点状态的偏导:影响相对位置预测 # ∂dot(dx)/∂v_mx = 1.0 F[15+i*12 + 0, 3] = 1.0 # ∂dx_dot/∂v_mx F[15+i*12 + 0, 4] = omega_m[2] # ∂dx_dot/∂v_my? 不,是 ∂dx_dot/∂ω_mz = -dy F[15+i*12 + 0, 5] = -omega_m[1] # ∂dx_dot/∂ω_my = dz # 类似处理 dy_dot, dz_dot F[15+i*12 + 1, 4] = 1.0 F[15+i*12 + 1, 3] = -omega_m[2] F[15+i*12 + 1, 5] = omega_m[0] F[15+i*12 + 2, 5] = 1.0 F[15+i*12 + 2, 3] = omega_m[1] F[15+i*12 + 2, 4] = -omega_m[0] # 对从节点自身状态的偏导:如 ∂dx_dot/∂dx = 0(无自反馈),但 ∂dx_dot/∂dy 影响科氏项 # 此处简化,实际需完整推导 # ... return F # 示例:计算当前状态下的F F_mat = compute_jacobian_F(x, dt=0.01, n_slaves=2) print(f"F matrix shape: {F_mat.shape}, condition number: {np.linalg.cond(F_mat):.2e}")该函数输出的F_mat是主从耦合的证据:矩阵中非对角线元素(如F[15,3]=1.0表示从节点1的x方向相对速度预测直接受主节点x方向速度影响)证明了动力学层面的强关联。条件数cond(F)若过大(如 >1e8),提示模型刚性过强,需检查时间步长dt或状态尺度——这是协同导航调试首个关键指标。
3.3 观测雅可比矩阵H:以UWB测距为例的完整推导与代码实现
假设从节点1与从节点2间有UWB测距观测 $ z_{12} $,其观测方程为:
$$ z_{12} = | \mathbf{R}_m \Delta \mathbf{p}_1 - \mathbf{R}_m \Delta \mathbf{p}_2 | $$
令 $ \mathbf{d} = \mathbf{R}_m (\Delta \mathbf{p}_1 - \Delta \mathbf{p}2) $,则 $ z{12} = | \mathbf{d} | $。
雅可比 $ \mathbf{H} = \frac{\partial z_{12}}{\partial \mathbf{x}} $ 需计算对 $ \Delta \mathbf{p}_1 $、$ \Delta \mathbf{p}_2 $、$ \mathbf{R}_m $(即 $ \mathbf{x}_m $ 的姿态部分)的偏导。
def compute_jacobian_H_uwb(x, idx_i=0, idx_j=1, n_slaves=2): """ 计算从节点i到j的UWB测距观测雅可比 H x: 全局状态向量 idx_i, idx_j: 从节点索引(0-based) 返回: (1, state_dim) 行向量 """ state_dim = len(x) H = np.zeros((1, state_dim)) # 提取主节点姿态(欧拉角 roll, pitch, yaw) phi, theta, psi = x[6:9] # rad # 构建旋转矩阵 R_m c_phi, s_phi = np.cos(phi), np.sin(phi) c_th, s_th = np.cos(theta), np.sin(theta) c_ps, s_ps = np.cos(psi), np.sin(psi) R_m = np.array([ [c_th*c_ps, s_phi*s_th*c_ps - c_phi*s_ps, c_phi*s_th*c_ps + s_phi*s_ps], [c_th*s_ps, s_phi*s_th*s_ps + c_phi*c_ps, c_phi*s_th*s_ps - s_phi*c_ps], [-s_th, s_phi*c_th, c_phi*c_th] ]) # 提取相对位置 start_s_i = 15 + idx_i*12 start_s_j = 15 + idx_j*12 dp_i = x[start_s_i:start_s_i+3] dp_j = x[start_s_j:start_s_j+3] # 计算差向量 d = R_m @ (dp_i - dp_j) d = R_m @ (dp_i - dp_j) dist = np.linalg.norm(d) # 若距离为0,避免除零(实际中应有最小距离约束) if dist < 1e-6: dist = 1e-6 # H 对 dp_i 的偏导:∂z/∂dp_i = (R_m^T @ d / dist).T # 因为 z = ||R_m*(dp_i-dp_j)||, 所以 ∂z/∂dp_i = (R_m^T @ d) / ||d|| d_norm = d / dist H_dp_i = (R_m.T @ d_norm).T # (1,3) 向量 H[0, start_s_i:start_s_i+3] = H_dp_i # H 对 dp_j 的偏导:∂z/∂dp_j = - (R_m^T @ d / dist).T H[0, start_s_j:start_s_j+3] = -H_dp_i # H 对主节点姿态的偏导(关键!体现主-从耦合) # ∂z/∂phi = d/dphi ||R_m*(dp_i-dp_j)|| = (d/dphi d)^T @ (d / ||d||) # d/dphi R_m = ∂R_m/∂phi,需数值微分或解析推导 # 此处用数值微分简化 eps = 1e-6 for k, angle_idx in enumerate([6, 7, 8]): # phi, theta, psi 索引 x_pert = x.copy() x_pert[angle_idx] += eps R_m_pert = build_rotation_matrix_from_euler(x_pert[6:9]) d_pert = R_m_pert @ (dp_i - dp_j) z_pert = np.linalg.norm(d_pert) dz_dangle = (z_pert - dist) / eps H[0, angle_idx] = dz_dangle return H def build_rotation_matrix_from_euler(euler): """辅助函数:从欧拉角构建旋转矩阵""" phi, theta, psi = euler c_phi, s_phi = np.cos(phi), np.sin(phi) c_th, s_th = np.cos(theta), np.sin(theta) c_ps, s_ps = np.cos(psi), np.sin(psi) return np.array([ [c_th*c_ps, s_phi*s_th*c_ps - c_phi*s_ps, c_phi*s_th*c_ps + s_phi*s_ps], [c_th*s_ps, s_phi*s_th*s_ps + c_phi*c_ps, c_phi*s_th*s_ps - s_phi*c_ps], [-s_th, s_phi*c_th, c_phi*c_th] ]) # 测试H计算 H_uwb = compute_jacobian_H_uwb(x, idx_i=0, idx_j=1, n_slaves=2) print(f"UWB H shape: {H_uwb.shape}, non-zero elements: {np.count_nonzero(H_uwb)}")此代码输出的H_uwb是协同导航的观测耦合证据:它不仅在从节点1、2的相对位置索引处有非零值(预期),更在主节点姿态角(索引6,7,8)处有非零值——说明UWB距离观测的准确性直接受主节点朝向估计影响。若忽略此项,滤波器将无法利用主节点姿态不确定性来合理分配从节点状态更新权重,导致协同失效。
4. 协同导航EKF的参数调试与失效诊断:3个必调参数与4类典型失效模式
4.1 3个必调参数:过程噪声Q、观测噪声R、初始协方差P₀的物理意义与调试策略
| 参数 | 物理意义 | 调试策略 | 过调/欠调表现 |
|---|---|---|---|
| 过程噪声协方差 Q | 描述模型不完美程度。主节点Q反映IMU预积分残差,从节点Q反映相对运动模型误差(如轮式机器人打滑) | 主节点Q:增大Q使滤波器更信任观测,减小Q使更信任预测。从节点Q需显著大于主节点(因相对模型更粗糙)。实测建议:主Q位置项=1e-3,从Q相对位置项=1e-1 | Q过大:状态抖动剧烈,收敛慢;Q过小:发散,无法跟踪真实运动 |
| 观测噪声协方差 R | 描述传感器测量不确定性。UWB R≈0.1~0.5m²,视觉特征R≈10~100像素²(需转为米²) | 必须与传感器标定一致。UWB R不可设为0(导致数值不稳定),视觉R需考虑焦距、像素尺寸。协同场景下,R需按观测类型分块设置(如H矩阵中不同观测对应不同R行) | R过大:滤波器忽略观测,退化为开环;R过小:过度拟合噪声,引发振荡 |
| 初始协方差 P₀ | 表达先验知识置信度。主节点P₀小,从节点P₀大 | 从节点P₀相对位置方差必须覆盖实际安装误差(如AGV间初始距离误差±0.5m,则设为0.25)。姿态P₀设为0.01~0.1 rad²(5~10度) | P₀过小:滤波器拒绝新观测,收敛失败;P₀过大:初期响应迟钝,需长时间收敛 |
提示:调试顺序必须是P₀ → Q → R。P₀设错,Q和R再准也无效。一个可靠技巧:将P₀设为理论值后,运行10秒无观测(纯预测),观察协方差对角元是否按Q*dt规律增长——若增长过快,Q过大;若几乎不变,Q过小。
4.2 4类典型失效模式与诊断代码
协同导航EKF失效往往表现为“看起来在跑,但精度崩坏”。以下是现场最常遇到的四类,附带可嵌入日志的诊断代码:
4.2.1 协方差膨胀(Covariance Inflation)
现象:P矩阵对角元持续指数增长,尤其从节点相对位置方差 >100 m²。
原因:Q过大、R过大、或观测雅可比H计算错误导致卡尔曼增益K≈0。
诊断代码:
def check_covariance_inflation(P, threshold=1e2, slave_start_idx=15): """检测从节点协方差是否异常膨胀""" n_slaves = (P.shape[0] - 15) // 12 inflation_flags = [] for i in range(n_slaves): start = slave_start_idx + i*12 p_diag = np.diag(P[start:start+12]) # 检查相对位置方差(前3个) if np.any(p_diag[:3] > threshold): inflation_flags.append(f"Slave-{i} pos variance > {threshold}: {p_diag[:3]}") return inflation_flags # 在EKF循环中调用 inflation_warnings = check_covariance_inflation(P) if inflation_warnings: print("COVARIANCE INFLATION DETECTED:", inflation_warnings)4.2.2 观测拒绝(Observation Rejection)
现象:卡尔曼增益K持续接近零,状态几乎不更新。
原因:H矩阵全零(雅可比未正确计算)、R过大、或观测残差 $ \mathbf{y} = \mathbf{z} - \mathbf{h}(\mathbf{x}) $ 过大触发一致性检验(如Mahalanobis距离 > chi-square阈值)。
诊断代码:
def check_observation_rejection(K, y, S, chi2_thresh=9.21): # 3自由度卡方95%分位 """检查是否因残差过大而拒绝观测""" if S.size == 0: return False # 计算Mahalanobis距离 try: inv_S = np.linalg.inv(S) maha_dist = y.T @ inv_S @ y if maha_dist > chi2_thresh: print(f"OBSERVATION REJECTED: Mahalanobis={maha_dist:.2f} > {chi2_thresh}") return True except np.linalg.LinAlgError: print("S matrix singular - observation rejected") return True return False # 在更新步骤后调用 if check_observation_rejection(K, y, S): # 可选:降权观测或切换到开环 pass4.2.3 主-从解耦(Master-Slave Decoupling)
现象:从节点轨迹与主节点完全无关,或相对位置估计停滞。
原因:动力学模型中未包含主节点运动对从节点的驱动项(即F矩阵中主-从耦合项为零)、或观测模型H未包含主节点状态偏导。
诊断代码:
def check_master_slave_coupling(F, n_slaves=2): """检查F矩阵中主-从耦合项是否存在""" coupling_found = False for i in range(n_slaves): start_s = 15 + i*12 # 检查从节点状态对主节点速度(索引3-5)或角速度(索引12-14)的偏导 if np.any(np.abs(F[start_s:start_s+3, 3:6]) > 1e-6) or \ np.any(np.abs(F[start_s:start_s+3, 12:15]) > 1e-6): coupling_found = True break if not coupling_found: print("WARNING: No master-to-slave coupling found in F matrix!") return coupling_found check_master_slave_coupling(F_mat)4.2.4 数值不稳定(Numerical Instability)
现象:P矩阵出现负对角元、非对称、或np.linalg.cholesky失败。
原因:离散化误差积累、矩阵求逆病态、或未实施协方差平方根滤波(SR-EKF)。
诊断代码:
def check_P_symmetry_and_positive_definite(P): """检查P是否对称正定""" is_sym = np.allclose(P, P.T, atol=1e-8) try: np.linalg.cholesky(P) is_pd = True except np.linalg.LinAlgError: is_pd = False if not is_sym: print("ERROR: P matrix not symmetric!") if not is_pd: print("ERROR: P matrix not positive definite!") return is_sym and is_pd check_P_symmetry_and_positive_definite(P)5. 验证协同效果:用相对位姿残差与主节点轨迹一致性双指标量化性能
协同导航的有效性不能只看单个从节点RMSE,而必须验证系统级一致性。我们提出两个可直接计算的量化指标,无需真值设备:
5.1 相对位姿残差(Relative Pose Residual, RPR)
定义:对任意从节点对 $ (i,j) $,计算其UWB测距观测 $ z_{ij} $ 与EKF估计的相对距离 $ \hat{z}_{ij} = | \mathbf{R}_m (\hat{\Delta \mathbf{p}}_i - \hat{\Delta \mathbf{p}}_j) | $ 的差值。RPR为所有节点对残差的均方根:
$$ \text{RPR} = \sqrt{ \frac{1}{N_{\text{pairs}}} \sum_{i<j} (z_{ij} - \hat{z}_{ij})^2 } $$
RPR < 0.1m 表明协同几何关系被准确维持,是协同性的直接证据。
def compute_rpr(x, uwb_measurements, n_slaves=2): """ x: 当前EKF状态 uwb_measurements: dict, key=(i,j), value=measured distance """ rpr_sq_sum = 0.0 n_pairs = 0 # 构建主节点旋转矩阵 phi, theta, psi = x[6:9] R_m = build_rotation_matrix_from_euler([phi, theta, psi]) for (i, j), z_meas in uwb_measurements.items(): if i >= n_slaves or j >= n_slaves: continue start_i = 15 + i*12 start_j = 15 + j*12 dp_i = x[start_i:start_i+3] dp_j = x[start_j:start_j+3] d_est = R_m @ (dp_i - dp_j) z_est = np.linalg.norm(d_est) rpr_sq_sum += (z_meas - z_est) ** 2 n_pairs += 1 return np.sqrt(rpr_sq_sum / n_pairs) if n_pairs > 0 else float('inf') # 示例:模拟2个UWB观测 uwb_obs = {(0,1): 2.15, (0,2): 3.02} # 从节点0-1距离2.15m,0-2距离3.02m rpr = compute_rpr(x, uwb_obs, n_slaves=3) print(f"Relative Pose Residual: {rpr:.3f} m")5.2 主节点轨迹一致性(Master Trajectory Consistency, MTC)
定义:主节点自身IMU积分轨迹 $ \mathbf{p}_m^{\text{imu}} $ 与通过从节点观测反推的主节点轨迹 $ \mathbf{p}m^{\text{slave}} = \mathbf{p}{s_i} - \mathbf{R}_m^T \Delta \mathbf{p}_i $ 的差异。MTC为所有从节点反推轨迹与IMU轨迹的平均距离:
$$ \text{MTC} = \frac{1}{n_{\text{slaves}}} \sum_{i=1}^{n_{\text{slaves}}} | \mathbf{p}_m^{\text{imu}} - \mathbf{p}_m^{\text{slave},i} | $$
MTC < 0.3m 表明主节点状态被从节点观测有效约束,是协同闭环闭合的证据。
def compute_mtc(x, slave_positions_world, n_slaves=2): """ slave_positions_world: list of [px, py, pz] in world frame for each slave """ # 主节点IMU积分位置(简化为x[0:3]) p_m_imu = x[0:3] mtc_sum = 0.0 for i in range(n_slaves): start_s = 15 + i*12 dp_i = x[start_s:start_s+3] # relative to master # 从节点世界位置 = 主节点位置 + R_m @ dp_i phi, theta, psi = x[6:9] R_m = build_rotation_matrix_from_euler([phi, theta, psi]) p_s_est = p_m_imu + R_m @ dp_i # 已知从节点世界位置(如来自UWB+TDOA定位或视觉SLAM) p_s_true = np.array(slave_positions_world[i]) # 反推主节点位置:p_m_slave = p_s_true - R_m @ dp_i p_m_slave = p_s_true - R_m @ dp_i mtc_sum += np.linalg.norm(p_m_imu - p_m_slave) return mtc_sum / n_slaves if n_slaves > 0 else float('inf') # 示例:已知从节点世界位置 slave_world_pos = [[1.0, 0.0, 0.0], [2.0, 1.0, 0.0]] # 从节点0,1的世界坐标 mtc = compute_mtc(x, slave_world_pos, n_slaves=2) print(f"Master Trajectory Consistency: {mtc:.3f} m")这两个指标构成协同导航的黄金验证组合:RPR低说明从节点间关系准确,MTC低说明主节点被从节点有效锚
本文还有配套的精品资源,点击获取