PythonRobotics 图解 GraphSLAM从信息矩阵 Ω 到 H⁻¹b 的图优化全流程【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRoboticsGraphSLAM 是 PythonRoboticsSLAM/GraphBasedSLAM 模块中把 SLAM 问题建模为图上的最小二乘优化的代表性算法。本文以 graphSLAM_doc.rst 为骨架从一维最小示例出发逐步推导信息矩阵与信息向量的构造、非线性残差的线性化与雅可比、锚定约束的数学必要性并结合仓库源码 graph_based_slam.py 与graphslam求解器包graph.py、se2.py给出可运行代码。读完你将掌握 GraphSLAM 的完整实现链路构图 → 线性化 → 组装 H/b → 迭代求解 → 回环校正。一、为什么用图从概率滤波到图优化与 EKF、UKF、粒子滤波这类概率递推式SLAM 方法不同GraphSLAM 把 SLAM 表述为一个优化问题机器人走过的整条路径上所有位姿被建模为图的节点位姿之间的约束里程计、观测、回环被建模为图的边最终目标是找到一组位姿使所有约束误差的加权平方和最小。从源码结构看这一思想体现在仓库的两套实现中教学版graph_based_slam.py用Edge类显式存放残差e、信息矩阵omega与观测数据逐边组装全局信息矩阵H与信息向量b适合理解算法本质工程版graphslam包graph.py、vertex.py、edge/edge_odometry.py提供Graph类、稀疏 Hessian 与spsolve求解、.g2o格式导入导出支持大规模真实数据集。GraphSLAM 通常以离线批处理方式解决完整 SLAM 问题——即在路径走完之后一次性优化所有位姿但图中也提到已有变体利用图方法进行在线估计或只求解部分位姿子集。其核心数据流如下运动信息 环境观测 ──► 构建图节点位姿边约束 ──► 组装信息矩阵 Ω (H) 与信息向量 ξ (b) ──► 解线性系统 x* H⁻¹b ──► 更新位姿 ──► 重复至收敛二、一维最小示例GraphSLAM 的核心思想2.1 场景设定考虑一个只能沿一维方向移动的机器人N3个时刻控制输入u_t 1但运动不完美里程计读数偏离真实轨迹环境中只有一个地标位于x 3观测值为机器人到地标的距离同样含噪声一维问题不需要方位角信息。仿真数据如下代码取自文档与 graph_based_slam.py 的随机种子设置一致import copy import math import itertools import numpy as np import matplotlib.pyplot as plt from graph_based_slam import calc_rotational_matrix, calc_jacobian, cal_observation_sigma, \ calc_input, observation, motion_model, Edge, pi_2_pi np.set_printoptions(precision3, suppressTrue) np.random.seed(0) R 0.2 # 运动噪声协方差本示例中未直接用于 H/b仅用于仿真 Q 0.2 # 观测噪声协方差信息权重 ω 1/(2Q) N 3 graphics_radius 0.1 odom np.empty((N, 1)) obs np.empty((N, 1)) x_true np.empty((N, 1)) landmark 3 # 模拟里程计与观测读数 x_true[0], odom[0], obs[0] 0.0, 0.0, 2.9 x_true[1], odom[1], obs[1] 1.0, 1.5, 2.0 x_true[2], odom[2], obs[2] 2.0, 2.4, 1.0将原始里程计与观测绘制出来可以看到真实轨迹是[0, 1, 2]而里程计已经漂移到[0, 1.5, 2.4]偏差随时间累积。2.2 虚拟测量把地标观测转成节点间约束GraphSLAM 的关键技巧是虚拟测量virtual measurement不把地标作为待优化变量显式加入状态向量而是把两个节点观测到同一地标这一事实转化为这两个节点之间的相对约束。文档中的get_H_b函数即按此思想实现def get_H_b(odom, obs): 构造信息矩阵与信息向量。本实现基于虚拟测量概念 地标观测被转换为观测到该地标的节点之间的约束边。 measure_constraints {} omegas {} zids list(itertools.combinations(range(N), 2)) H np.zeros((N, N)) b np.zeros((N, 1)) for (t1, t2) in zids: x1, x2 odom[t1], odom[t2] z1, z2 obs[t1], obs[t2] # 虚拟测量约束x_j - x_i 应等于 z_i - z_j同一地标 measure_constraints[(t1, t2)] (x2 - x1 - z1 z2) omegas[(t1, t2)] (1 / (2 * Q)) # 累加进入系统的信息矩阵与信息向量 H[t1, t1] omegas[(t1, t2)] H[t2, t2] omegas[(t1, t2)] H[t2, t1] - omegas[(t1, t2)] H[t1, t2] - omegas[(t1, t2)] b[t1] omegas[(t1, t2)] * measure_constraints[(t1, t2)] b[t2] - omegas[(t1, t2)] * measure_constraints[(t1, t2)] return H, b这里有三个必须记住的要点文档明确强调累加而非覆盖每条边对H和b的贡献都加到先前的值上这对应多约束的叠加信息权重决定约束强度局部信息矩阵越大即Q或R越小该边对系统贡献的权重越大。本例中omega 1/(2Q) 2.5锚定约束必不可少仅由相对约束构成的H是奇异矩阵行列式为 0必须固定一个参考位姿锚定才能求解。2.3 锚定与迭代求解H, b get_H_b(odom, obs) print(The determinant of H: , np.linalg.det(H)) # 输出: The determinant of H: 0.0 H[0, 0] 1 # 锚定约束固定 x0 print(The determinant of H after anchoring constraint: , np.linalg.det(H)) # 输出: 18.75... # 迭代求解Gauss-Newton 式更新 for i in range(5): H, b get_H_b(odom, obs) H[(0, 0)] 1 dx np.linalg.inv(H) b odom dx # 更新位姿重复直至收敛 print(Odometry values after optimization: \n, odom) # 输出: # [[-0. ] # [ 0.9] # [ 1.9]]优化后里程计从[0, 1.5, 2.4]被校正到[0, 0.9, 1.9]明显逼近真实轨迹[0, 1, 2]。值得注意的是锚定处H[0,0] 1文档注释提示也可用np.inf其作用是打破秩亏保证H可逆由于只有相对约束平移自由度是固定的但系统的绝对坐标取决于锚定位置。2.4 一维示例的两个阶段文档将 GraphSLAM 明确划分为两个阶段图构建Graph Construction节点x x_{1:n}每个节点是机器人t_i时刻的位姿边约束按两种条件构造机器人从x_i移动到x_j→里程计边相对运动约束一维最小示例中未包含测量约束有两种做法把地标也纳入信息矩阵即地标也作为节点在节点x_i与地标m_k之间直接建立约束虚拟测量对所有观测到同一地标的节点对(x_i, x_j)建立相对测量约束。虚拟测量z_ij表示从节点 i 看去节点 j 的估计位姿之后像运动约束一样写入信息矩阵与信息向量。图优化Graph Optimization求解超定方程组目标是x* argmin Σ_ij f(e_ij)其中f是依赖边误差的误差函数文献推导得到闭式解x* H⁻¹b。三、二维平面示例3 自由度机器人的完整流程3.1 仿真参数与数据生成更接近现实的场景是二维 3 自由度[x, y, θ]ᵀ机器人。文档给出的仿真参数如下Qsim np.diag([0.01, np.deg2rad(0.010)])**2 # 加入距离和方位的观测噪声 Rsim np.diag([0.1, np.deg2rad(1.0)])**2 # 加入 [v, w] 的控制噪声 DT 2.0 # 时间步长 [s] SIM_TIME 100.0 # 仿真时间 [s] MAX_RANGE 30.0 # 最大观测范围 STATE_SIZE 3 # 状态维度 [x, y, yaw] # Graph SLAM 的协方差参数文档注释TODO 为何不使用 Qsim C_SIGMA1 0.1 C_SIGMA2 0.1 C_SIGMA3 np.deg2rad(1.0) MAX_ITR 20 # 优化最大迭代次数 timesteps 1 # 简化示例仅 2 个节点对应源码 graph_based_slam.py 中Q_sim np.diag([0.2, np.deg2rad(1.0)]) ** 2、R_sim np.diag([0.1, np.deg2rad(10.0)]) ** 2仿真时通过observation()在距离与方位上叠加高斯噪声np.random.randn() * Q_sim[0, 0]等。文档为了便于讲解只保留了一个地标RFID np.array([[10.0, -2.0, 0.0]]) # 地标 [x, y, yaw]状态与数据历史初始化随后用calc_input()v1.0 m/s、yaw_rate0.1 rad/s见源码驱动observation()推进运动学xTrue np.zeros((STATE_SIZE, 1)); xDR np.zeros((STATE_SIZE, 1)) xTrue[2] np.deg2rad(45); xDR[2] np.deg2rad(45) hxTrue, hxDR xTrue, xTrue _, z, _, _ observation(xTrue, xDR, np.array([[0, 0]]).T, RFID) hz [z] for i in range(timesteps): u calc_input() xTrue, z, xDR, ud observation(xTrue, xDR, u, RFID) hxDR np.hstack((hxDR, xDR)) hxTrue np.hstack((hxTrue, xTrue)) hz.append(z)源码中的运动模型motion_model()为def motion_model(x, u): F np.array([[1.0, 0, 0], [0, 1.0, 0], [0, 0, 1.0]]) B np.array([[DT * math.cos(x[2, 0]), 0], [DT * math.sin(x[2, 0]), 0], [0.0, DT]]) return F x B u即匀速线速度 恒定角速度模型x DT·cos(θ)·v、y DT·sin(θ)·v、θ DT·ω。3.2 构图节点组合与虚拟测量误差观测数据格式为z [d, angle, phi, landmark_id]其中d是距离、angle是相对机器人朝向的方位、phi是全局方位、最后一列是地标 ID见observation()中zi np.array([dn, angle, phi, i])。构图时枚举所有节点对zids list(itertools.combinations(range(len(zlist)), 2)) print(Node combinations: , zids) # 输出: [(0, 1)] for i in range(xlist.shape[1]): print(Node {} observed landmark with ID {}.format(i, zlist[i][0, 3])) # 输出: Node 0 observed landmark with ID 0.0 # Node 1 observed landmark with ID 0.0只有当两个节点在不同时刻观测到同一地标时才创建虚拟测量边对应源码calc_edges()中if z_list[t1][iz1, 3] z_list[t2][iz2, 3]的数据关联判断。虚拟测量误差公式为文档给出e_ij^x x_j d_j·cos(ψ_j θ_j) − x_i − d_i·cos(ψ_i θ_i) e_ij^y y_j d_j·sin(ψ_j θ_j) − y_i − d_i·sin(ψ_i θ_i) e_ij^ψ ψ_j θ_j − ψ_i − θ_i其中[x_i, y_i, ψ_i]是节点 i 的位姿d、θ分别是节点处的距离与方位测量。在运动与测量均完美的情况下x_j d_j·cos(ψ_j θ_j)应等于x_i d_i·cos(ψ_i θ_i)误差应为零——误差正是两者不一致的量度。示例中单条边的误差为[[-0.02], [-0.084], [0.]]。构造边的代码文档逐行讲解与源码calc_edge()对应edges [] for (t1, t2) in zids: x1, y1, yaw1 xlist[0, t1], xlist[1, t1], xlist[2, t1] x2, y2, yaw2 xlist[0, t2], xlist[1, t2], xlist[2, t2] iz1 iz2 0 # 本例所有节点观测同一地标 ID0无需数据关联分支 d1 zlist[t1][iz1, 0] angle1, phi1 zlist[t1][iz1, 1], zlist[t1][iz1, 2] d2 zlist[t2][iz2, 0] angle2, phi2 zlist[t2][iz2, 1], zlist[t2][iz2, 2] tangle1 pi_2_pi(yaw1 angle1) tangle2 pi_2_pi(yaw2 angle2) tmp1, tmp2 d1 * math.cos(tangle1), d2 * math.cos(tangle2) tmp3, tmp4 d1 * math.sin(tangle1), d2 * math.sin(tangle2) edge Edge() # 计算虚拟测量误差从节点 1 的观测看节点 2 的位置 edge.e[0, 0] x2 - x1 - tmp1 tmp2 edge.e[1, 0] y2 - y1 - tmp3 tmp4 edge.e[2, 0] pi_2_pi(yaw2 - yaw1 - tangle1 tangle2) edge.d1, edge.d2 d1, d2 edge.yaw1, edge.yaw2 yaw1, yaw2 edge.angle1, edge.angle2 angle1, angle2 edge.id1, edge.id2 t1, t2 edges.append(edge)3.3 线性化雅可比 A 与 B由于约束方程是非线性的在写入信息矩阵前必须先线性化。需要两个雅可比A ∂e_ij / ∂x_i残差对节点 i 位姿x、y、θ 三变量的偏导B ∂e_ij / ∂x_j残差对节点 j 位姿的偏导。H np.zeros((n, n)); b np.zeros((n, 1)) # n 节点数 × STATE_SIZE x_opt copy.deepcopy(hxDR) for edge in edges: id1 edge.id1 * STATE_SIZE id2 edge.id2 * STATE_SIZE t1 edge.yaw1 edge.angle1 A np.array([[-1.0, 0, edge.d1 * math.sin(t1)], [0, -1.0, -edge.d1 * math.cos(t1)], [0, 0, -1.0]]) t2 edge.yaw2 edge.angle2 B np.array([[1.0, 0, -edge.d2 * math.sin(t2)], [0, 1.0, edge.d2 * math.cos(t2)], [0, 0, 1.0]]) # 信息矩阵将观测协方差旋转到全局系后求逆 sigma np.diag([C_SIGMA1, C_SIGMA2, C_SIGMA3]) Rt1 calc_rotational_matrix(tangle1) Rt2 calc_rotational_matrix(tangle2) edge.omega np.linalg.inv(Rt1 sigma Rt1.T Rt2 sigma Rt2.T) # 组装 H 与 b对应源码 fill_H_and_b() H[id1:id1STATE_SIZE, id1:id1STATE_SIZE] A.T edge.omega A H[id1:id1STATE_SIZE, id2:id2STATE_SIZE] A.T edge.omega B H[id2:id2STATE_SIZE, id1:id1STATE_SIZE] B.T edge.omega A H[id2:id2STATE_SIZE, id2:id2STATE_SIZE] B.T edge.omega B b[id1:id1STATE_SIZE] A.T edge.omega edge.e b[id2:id2STATE_SIZE] B.T edge.omega edge.e这正是高斯牛顿法中对每一条边执行H JᵀΩJ、b JᵀΩe的经典累加模式与 graph.py 中_calc_chi2_gradient_hessian()的思路一致工程版用稀疏lil_matrix组装 Hessian 并以spsolve求解。文档源码中calc_jacobian()的第三行对 ψ 方向取 0[0, 0, 0]而文档示例用-1差异源于是否把方位误差计入雅可比读者可对照两处理解。3.4 锚定原点与迭代求解print(The determinant of H: , np.linalg.det(H)) # 输出: 0.0 H[0:STATE_SIZE, 0:STATE_SIZE] np.identity(STATE_SIZE) # 固定原点 print(The determinant of H after origin constraint: , np.linalg.det(H)) # 输出: 716.197... dx -np.linalg.inv(H) b for i in range(number_of_nodes): x_opt[0:3, i] dx[i*3:i*33, 0]锚定前后H与b的可视化如下左信息矩阵热力图右信息向量一次迭代后的对比文档输出ground truth: [[0. 1.414] [0. 1.414] [0.785 0.985]] Odom: [[0. 1.428] [0. 1.428] [0.785 0.976]] SLAM: [[-0. 1.448] [-0. 1.512] [ 0.785 0.976]] graphSLAM localization error: 0.010729072751057656 Odom localization error: 0.0004460978857535104注意本例仅 2 个节点、1 个地标单次迭代后 SLAM 误差反而略大于纯里程计——文档明确说明性能会随着更多迭代、更多节点和更多地标而提升。在完整仿真中算法以MAX_ITR 20迭代直到增量diff 1.0e-5收敛见 graph_based_slam.py 的graph_based_slam()并在show_graph_d_time 20.0秒间隔内周期性对历史轨迹重优化。运行完整仿真cd SLAM/GraphBasedSLAM python graph_based_slam.py图中蓝线为真值、黑线为航位推算、红线为 GraphSLAM 估计轨迹黑色星号为用于生成图边的地标参见 graph_slam_main.rst。对应测试见 tests/test_graph_based_slam.pyshow_animation False、SIM_TIME 20.0下执行主流程。四、一维与二维的关键异同维度约束是否非线性是否需要雅可比锚定方式信息矩阵1D线性不需要H[0,0] 1标量权重1/(2Q)2DSE(2)非线性需要 A、B 两个雅可比H[0:3,0:3] I₃(RᵀΣR)⁻¹旋转后的 3×3 矩阵两条通用原则文档Important Notes对H和b的贡献都是累加到已有值上2D 约束非线性更新H与b时必须引入残差对状态的雅可比锚定约束是必须的否则信息矩阵奇异、无法求逆两个示例中锚定前行列式均为 0.0。五、延伸阅读SE(2) 工程实现与真实数据集若想了解完整工程实现仓库提供了配套的数学推导文档graphSLAM_formulation.rst含最大似然推导、残差线性化、Δx −H⁻¹b的完整推导和真实数据集示例graphSLAM_SE2_example.rst使用graphslam.load.load_g2o_se2(data/input_INTEL.g2o)载入 Intel 数据集data/input_INTEL.g2o数据版权归 Luca Carlone见 data/README.rst该图含1483 条边、1228 个顶点边分两类里程计边约束相邻顶点与扫描匹配边回环约束约束非相邻顶点。优化前扫描匹配边的 χ² 高达 7191686经g.optimize()数次迭代后降至 73.652直观体现了回环校正对累计漂移的消除工程版采用数值微分替代解析雅可比EdgeOdometry._calc_jacobian()步长EPSILON 1e-6Hessian 使用稀疏矩阵以支持大规模问题Vertex与EdgeOdometry均实现to_g2o()可将结果导出为标准的.g2o格式vertex.py、edge/edge_odometry.py。文档还提示了 GraphSLAM 的最后一个必要步骤在得到优化后的路径估计后还需利用该路径更新关于地图的信念即地标位置的估计完整的 SLAM 才闭合。六、参考资源本文对应仓库内文档与代码主文档graphSLAM_doc.rst本文主体数学推导graphSLAM_formulation.rstSE(2) 真实数据集示例graphSLAM_SE2_example.rst教学版源码SLAM/GraphBasedSLAM/graph_based_slam.py工程版求解器graphslam/graph.py、graphslam/pose/se2.py、graphslam/edge/edge_odometry.py测试tests/test_graph_based_slam.py算法本身源自经典文献Thrun 等人的《The GraphSLAM Algorithm with Applications to Large-Scale Mapping of Urban Structures》以及 Grisetti 等人的《A Tutorial on Graph-Based SLAM》后者也正是仓库源码头部注释所引用的实现依据。【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考
PythonRobotics 图解 GraphSLAM：从信息矩阵 Ω 到 H⁻¹b 的图优化全流程
PythonRobotics 图解 GraphSLAM从信息矩阵 Ω 到 H⁻¹b 的图优化全流程【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRoboticsGraphSLAM 是 PythonRoboticsSLAM/GraphBasedSLAM 模块中把 SLAM 问题建模为图上的最小二乘优化的代表性算法。本文以 graphSLAM_doc.rst 为骨架从一维最小示例出发逐步推导信息矩阵与信息向量的构造、非线性残差的线性化与雅可比、锚定约束的数学必要性并结合仓库源码 graph_based_slam.py 与graphslam求解器包graph.py、se2.py给出可运行代码。读完你将掌握 GraphSLAM 的完整实现链路构图 → 线性化 → 组装 H/b → 迭代求解 → 回环校正。一、为什么用图从概率滤波到图优化与 EKF、UKF、粒子滤波这类概率递推式SLAM 方法不同GraphSLAM 把 SLAM 表述为一个优化问题机器人走过的整条路径上所有位姿被建模为图的节点位姿之间的约束里程计、观测、回环被建模为图的边最终目标是找到一组位姿使所有约束误差的加权平方和最小。从源码结构看这一思想体现在仓库的两套实现中教学版graph_based_slam.py用Edge类显式存放残差e、信息矩阵omega与观测数据逐边组装全局信息矩阵H与信息向量b适合理解算法本质工程版graphslam包graph.py、vertex.py、edge/edge_odometry.py提供Graph类、稀疏 Hessian 与spsolve求解、.g2o格式导入导出支持大规模真实数据集。GraphSLAM 通常以离线批处理方式解决完整 SLAM 问题——即在路径走完之后一次性优化所有位姿但图中也提到已有变体利用图方法进行在线估计或只求解部分位姿子集。其核心数据流如下运动信息 环境观测 ──► 构建图节点位姿边约束 ──► 组装信息矩阵 Ω (H) 与信息向量 ξ (b) ──► 解线性系统 x* H⁻¹b ──► 更新位姿 ──► 重复至收敛二、一维最小示例GraphSLAM 的核心思想2.1 场景设定考虑一个只能沿一维方向移动的机器人N3个时刻控制输入u_t 1但运动不完美里程计读数偏离真实轨迹环境中只有一个地标位于x 3观测值为机器人到地标的距离同样含噪声一维问题不需要方位角信息。仿真数据如下代码取自文档与 graph_based_slam.py 的随机种子设置一致import copy import math import itertools import numpy as np import matplotlib.pyplot as plt from graph_based_slam import calc_rotational_matrix, calc_jacobian, cal_observation_sigma, \ calc_input, observation, motion_model, Edge, pi_2_pi np.set_printoptions(precision3, suppressTrue) np.random.seed(0) R 0.2 # 运动噪声协方差本示例中未直接用于 H/b仅用于仿真 Q 0.2 # 观测噪声协方差信息权重 ω 1/(2Q) N 3 graphics_radius 0.1 odom np.empty((N, 1)) obs np.empty((N, 1)) x_true np.empty((N, 1)) landmark 3 # 模拟里程计与观测读数 x_true[0], odom[0], obs[0] 0.0, 0.0, 2.9 x_true[1], odom[1], obs[1] 1.0, 1.5, 2.0 x_true[2], odom[2], obs[2] 2.0, 2.4, 1.0将原始里程计与观测绘制出来可以看到真实轨迹是[0, 1, 2]而里程计已经漂移到[0, 1.5, 2.4]偏差随时间累积。2.2 虚拟测量把地标观测转成节点间约束GraphSLAM 的关键技巧是虚拟测量virtual measurement不把地标作为待优化变量显式加入状态向量而是把两个节点观测到同一地标这一事实转化为这两个节点之间的相对约束。文档中的get_H_b函数即按此思想实现def get_H_b(odom, obs): 构造信息矩阵与信息向量。本实现基于虚拟测量概念 地标观测被转换为观测到该地标的节点之间的约束边。 measure_constraints {} omegas {} zids list(itertools.combinations(range(N), 2)) H np.zeros((N, N)) b np.zeros((N, 1)) for (t1, t2) in zids: x1, x2 odom[t1], odom[t2] z1, z2 obs[t1], obs[t2] # 虚拟测量约束x_j - x_i 应等于 z_i - z_j同一地标 measure_constraints[(t1, t2)] (x2 - x1 - z1 z2) omegas[(t1, t2)] (1 / (2 * Q)) # 累加进入系统的信息矩阵与信息向量 H[t1, t1] omegas[(t1, t2)] H[t2, t2] omegas[(t1, t2)] H[t2, t1] - omegas[(t1, t2)] H[t1, t2] - omegas[(t1, t2)] b[t1] omegas[(t1, t2)] * measure_constraints[(t1, t2)] b[t2] - omegas[(t1, t2)] * measure_constraints[(t1, t2)] return H, b这里有三个必须记住的要点文档明确强调累加而非覆盖每条边对H和b的贡献都加到先前的值上这对应多约束的叠加信息权重决定约束强度局部信息矩阵越大即Q或R越小该边对系统贡献的权重越大。本例中omega 1/(2Q) 2.5锚定约束必不可少仅由相对约束构成的H是奇异矩阵行列式为 0必须固定一个参考位姿锚定才能求解。2.3 锚定与迭代求解H, b get_H_b(odom, obs) print(The determinant of H: , np.linalg.det(H)) # 输出: The determinant of H: 0.0 H[0, 0] 1 # 锚定约束固定 x0 print(The determinant of H after anchoring constraint: , np.linalg.det(H)) # 输出: 18.75... # 迭代求解Gauss-Newton 式更新 for i in range(5): H, b get_H_b(odom, obs) H[(0, 0)] 1 dx np.linalg.inv(H) b odom dx # 更新位姿重复直至收敛 print(Odometry values after optimization: \n, odom) # 输出: # [[-0. ] # [ 0.9] # [ 1.9]]优化后里程计从[0, 1.5, 2.4]被校正到[0, 0.9, 1.9]明显逼近真实轨迹[0, 1, 2]。值得注意的是锚定处H[0,0] 1文档注释提示也可用np.inf其作用是打破秩亏保证H可逆由于只有相对约束平移自由度是固定的但系统的绝对坐标取决于锚定位置。2.4 一维示例的两个阶段文档将 GraphSLAM 明确划分为两个阶段图构建Graph Construction节点x x_{1:n}每个节点是机器人t_i时刻的位姿边约束按两种条件构造机器人从x_i移动到x_j→里程计边相对运动约束一维最小示例中未包含测量约束有两种做法把地标也纳入信息矩阵即地标也作为节点在节点x_i与地标m_k之间直接建立约束虚拟测量对所有观测到同一地标的节点对(x_i, x_j)建立相对测量约束。虚拟测量z_ij表示从节点 i 看去节点 j 的估计位姿之后像运动约束一样写入信息矩阵与信息向量。图优化Graph Optimization求解超定方程组目标是x* argmin Σ_ij f(e_ij)其中f是依赖边误差的误差函数文献推导得到闭式解x* H⁻¹b。三、二维平面示例3 自由度机器人的完整流程3.1 仿真参数与数据生成更接近现实的场景是二维 3 自由度[x, y, θ]ᵀ机器人。文档给出的仿真参数如下Qsim np.diag([0.01, np.deg2rad(0.010)])**2 # 加入距离和方位的观测噪声 Rsim np.diag([0.1, np.deg2rad(1.0)])**2 # 加入 [v, w] 的控制噪声 DT 2.0 # 时间步长 [s] SIM_TIME 100.0 # 仿真时间 [s] MAX_RANGE 30.0 # 最大观测范围 STATE_SIZE 3 # 状态维度 [x, y, yaw] # Graph SLAM 的协方差参数文档注释TODO 为何不使用 Qsim C_SIGMA1 0.1 C_SIGMA2 0.1 C_SIGMA3 np.deg2rad(1.0) MAX_ITR 20 # 优化最大迭代次数 timesteps 1 # 简化示例仅 2 个节点对应源码 graph_based_slam.py 中Q_sim np.diag([0.2, np.deg2rad(1.0)]) ** 2、R_sim np.diag([0.1, np.deg2rad(10.0)]) ** 2仿真时通过observation()在距离与方位上叠加高斯噪声np.random.randn() * Q_sim[0, 0]等。文档为了便于讲解只保留了一个地标RFID np.array([[10.0, -2.0, 0.0]]) # 地标 [x, y, yaw]状态与数据历史初始化随后用calc_input()v1.0 m/s、yaw_rate0.1 rad/s见源码驱动observation()推进运动学xTrue np.zeros((STATE_SIZE, 1)); xDR np.zeros((STATE_SIZE, 1)) xTrue[2] np.deg2rad(45); xDR[2] np.deg2rad(45) hxTrue, hxDR xTrue, xTrue _, z, _, _ observation(xTrue, xDR, np.array([[0, 0]]).T, RFID) hz [z] for i in range(timesteps): u calc_input() xTrue, z, xDR, ud observation(xTrue, xDR, u, RFID) hxDR np.hstack((hxDR, xDR)) hxTrue np.hstack((hxTrue, xTrue)) hz.append(z)源码中的运动模型motion_model()为def motion_model(x, u): F np.array([[1.0, 0, 0], [0, 1.0, 0], [0, 0, 1.0]]) B np.array([[DT * math.cos(x[2, 0]), 0], [DT * math.sin(x[2, 0]), 0], [0.0, DT]]) return F x B u即匀速线速度 恒定角速度模型x DT·cos(θ)·v、y DT·sin(θ)·v、θ DT·ω。3.2 构图节点组合与虚拟测量误差观测数据格式为z [d, angle, phi, landmark_id]其中d是距离、angle是相对机器人朝向的方位、phi是全局方位、最后一列是地标 ID见observation()中zi np.array([dn, angle, phi, i])。构图时枚举所有节点对zids list(itertools.combinations(range(len(zlist)), 2)) print(Node combinations: , zids) # 输出: [(0, 1)] for i in range(xlist.shape[1]): print(Node {} observed landmark with ID {}.format(i, zlist[i][0, 3])) # 输出: Node 0 observed landmark with ID 0.0 # Node 1 observed landmark with ID 0.0只有当两个节点在不同时刻观测到同一地标时才创建虚拟测量边对应源码calc_edges()中if z_list[t1][iz1, 3] z_list[t2][iz2, 3]的数据关联判断。虚拟测量误差公式为文档给出e_ij^x x_j d_j·cos(ψ_j θ_j) − x_i − d_i·cos(ψ_i θ_i) e_ij^y y_j d_j·sin(ψ_j θ_j) − y_i − d_i·sin(ψ_i θ_i) e_ij^ψ ψ_j θ_j − ψ_i − θ_i其中[x_i, y_i, ψ_i]是节点 i 的位姿d、θ分别是节点处的距离与方位测量。在运动与测量均完美的情况下x_j d_j·cos(ψ_j θ_j)应等于x_i d_i·cos(ψ_i θ_i)误差应为零——误差正是两者不一致的量度。示例中单条边的误差为[[-0.02], [-0.084], [0.]]。构造边的代码文档逐行讲解与源码calc_edge()对应edges [] for (t1, t2) in zids: x1, y1, yaw1 xlist[0, t1], xlist[1, t1], xlist[2, t1] x2, y2, yaw2 xlist[0, t2], xlist[1, t2], xlist[2, t2] iz1 iz2 0 # 本例所有节点观测同一地标 ID0无需数据关联分支 d1 zlist[t1][iz1, 0] angle1, phi1 zlist[t1][iz1, 1], zlist[t1][iz1, 2] d2 zlist[t2][iz2, 0] angle2, phi2 zlist[t2][iz2, 1], zlist[t2][iz2, 2] tangle1 pi_2_pi(yaw1 angle1) tangle2 pi_2_pi(yaw2 angle2) tmp1, tmp2 d1 * math.cos(tangle1), d2 * math.cos(tangle2) tmp3, tmp4 d1 * math.sin(tangle1), d2 * math.sin(tangle2) edge Edge() # 计算虚拟测量误差从节点 1 的观测看节点 2 的位置 edge.e[0, 0] x2 - x1 - tmp1 tmp2 edge.e[1, 0] y2 - y1 - tmp3 tmp4 edge.e[2, 0] pi_2_pi(yaw2 - yaw1 - tangle1 tangle2) edge.d1, edge.d2 d1, d2 edge.yaw1, edge.yaw2 yaw1, yaw2 edge.angle1, edge.angle2 angle1, angle2 edge.id1, edge.id2 t1, t2 edges.append(edge)3.3 线性化雅可比 A 与 B由于约束方程是非线性的在写入信息矩阵前必须先线性化。需要两个雅可比A ∂e_ij / ∂x_i残差对节点 i 位姿x、y、θ 三变量的偏导B ∂e_ij / ∂x_j残差对节点 j 位姿的偏导。H np.zeros((n, n)); b np.zeros((n, 1)) # n 节点数 × STATE_SIZE x_opt copy.deepcopy(hxDR) for edge in edges: id1 edge.id1 * STATE_SIZE id2 edge.id2 * STATE_SIZE t1 edge.yaw1 edge.angle1 A np.array([[-1.0, 0, edge.d1 * math.sin(t1)], [0, -1.0, -edge.d1 * math.cos(t1)], [0, 0, -1.0]]) t2 edge.yaw2 edge.angle2 B np.array([[1.0, 0, -edge.d2 * math.sin(t2)], [0, 1.0, edge.d2 * math.cos(t2)], [0, 0, 1.0]]) # 信息矩阵将观测协方差旋转到全局系后求逆 sigma np.diag([C_SIGMA1, C_SIGMA2, C_SIGMA3]) Rt1 calc_rotational_matrix(tangle1) Rt2 calc_rotational_matrix(tangle2) edge.omega np.linalg.inv(Rt1 sigma Rt1.T Rt2 sigma Rt2.T) # 组装 H 与 b对应源码 fill_H_and_b() H[id1:id1STATE_SIZE, id1:id1STATE_SIZE] A.T edge.omega A H[id1:id1STATE_SIZE, id2:id2STATE_SIZE] A.T edge.omega B H[id2:id2STATE_SIZE, id1:id1STATE_SIZE] B.T edge.omega A H[id2:id2STATE_SIZE, id2:id2STATE_SIZE] B.T edge.omega B b[id1:id1STATE_SIZE] A.T edge.omega edge.e b[id2:id2STATE_SIZE] B.T edge.omega edge.e这正是高斯牛顿法中对每一条边执行H JᵀΩJ、b JᵀΩe的经典累加模式与 graph.py 中_calc_chi2_gradient_hessian()的思路一致工程版用稀疏lil_matrix组装 Hessian 并以spsolve求解。文档源码中calc_jacobian()的第三行对 ψ 方向取 0[0, 0, 0]而文档示例用-1差异源于是否把方位误差计入雅可比读者可对照两处理解。3.4 锚定原点与迭代求解print(The determinant of H: , np.linalg.det(H)) # 输出: 0.0 H[0:STATE_SIZE, 0:STATE_SIZE] np.identity(STATE_SIZE) # 固定原点 print(The determinant of H after origin constraint: , np.linalg.det(H)) # 输出: 716.197... dx -np.linalg.inv(H) b for i in range(number_of_nodes): x_opt[0:3, i] dx[i*3:i*33, 0]锚定前后H与b的可视化如下左信息矩阵热力图右信息向量一次迭代后的对比文档输出ground truth: [[0. 1.414] [0. 1.414] [0.785 0.985]] Odom: [[0. 1.428] [0. 1.428] [0.785 0.976]] SLAM: [[-0. 1.448] [-0. 1.512] [ 0.785 0.976]] graphSLAM localization error: 0.010729072751057656 Odom localization error: 0.0004460978857535104注意本例仅 2 个节点、1 个地标单次迭代后 SLAM 误差反而略大于纯里程计——文档明确说明性能会随着更多迭代、更多节点和更多地标而提升。在完整仿真中算法以MAX_ITR 20迭代直到增量diff 1.0e-5收敛见 graph_based_slam.py 的graph_based_slam()并在show_graph_d_time 20.0秒间隔内周期性对历史轨迹重优化。运行完整仿真cd SLAM/GraphBasedSLAM python graph_based_slam.py图中蓝线为真值、黑线为航位推算、红线为 GraphSLAM 估计轨迹黑色星号为用于生成图边的地标参见 graph_slam_main.rst。对应测试见 tests/test_graph_based_slam.pyshow_animation False、SIM_TIME 20.0下执行主流程。四、一维与二维的关键异同维度约束是否非线性是否需要雅可比锚定方式信息矩阵1D线性不需要H[0,0] 1标量权重1/(2Q)2DSE(2)非线性需要 A、B 两个雅可比H[0:3,0:3] I₃(RᵀΣR)⁻¹旋转后的 3×3 矩阵两条通用原则文档Important Notes对H和b的贡献都是累加到已有值上2D 约束非线性更新H与b时必须引入残差对状态的雅可比锚定约束是必须的否则信息矩阵奇异、无法求逆两个示例中锚定前行列式均为 0.0。五、延伸阅读SE(2) 工程实现与真实数据集若想了解完整工程实现仓库提供了配套的数学推导文档graphSLAM_formulation.rst含最大似然推导、残差线性化、Δx −H⁻¹b的完整推导和真实数据集示例graphSLAM_SE2_example.rst使用graphslam.load.load_g2o_se2(data/input_INTEL.g2o)载入 Intel 数据集data/input_INTEL.g2o数据版权归 Luca Carlone见 data/README.rst该图含1483 条边、1228 个顶点边分两类里程计边约束相邻顶点与扫描匹配边回环约束约束非相邻顶点。优化前扫描匹配边的 χ² 高达 7191686经g.optimize()数次迭代后降至 73.652直观体现了回环校正对累计漂移的消除工程版采用数值微分替代解析雅可比EdgeOdometry._calc_jacobian()步长EPSILON 1e-6Hessian 使用稀疏矩阵以支持大规模问题Vertex与EdgeOdometry均实现to_g2o()可将结果导出为标准的.g2o格式vertex.py、edge/edge_odometry.py。文档还提示了 GraphSLAM 的最后一个必要步骤在得到优化后的路径估计后还需利用该路径更新关于地图的信念即地标位置的估计完整的 SLAM 才闭合。六、参考资源本文对应仓库内文档与代码主文档graphSLAM_doc.rst本文主体数学推导graphSLAM_formulation.rstSE(2) 真实数据集示例graphSLAM_SE2_example.rst教学版源码SLAM/GraphBasedSLAM/graph_based_slam.py工程版求解器graphslam/graph.py、graphslam/pose/se2.py、graphslam/edge/edge_odometry.py测试tests/test_graph_based_slam.py算法本身源自经典文献Thrun 等人的《The GraphSLAM Algorithm with Applications to Large-Scale Mapping of Urban Structures》以及 Grisetti 等人的《A Tutorial on Graph-Based SLAM》后者也正是仓库源码头部注释所引用的实现依据。【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考
