UKF与高斯过程融合的机器人位姿估计技术

1. 项目概述

在机器人感知与控制领域，状态估计一直是个核心挑战。传统卡尔曼滤波虽然经典，但在处理非高斯噪声时往往力不从心。最近我在研究一种结合无迹卡尔曼滤波(UKF)和高斯过程(GP)的混合架构，用于解决生物启发多连杆机器人的位姿估计问题。这个项目源于对海洋樽海鞘运动方式的仿生研究，我们开发了一种名为LandSalp的三连杆陆地机器人平台。

这个机器人有几个关键特点：被动关节连接、每个连杆配备独立推进器、仅依靠陀螺仪进行感知。这种设计带来了两个主要挑战：一是系统欠驱动（关节无主动控制），二是感知极度受限（没有位置传感器和关节编码器）。我们通过UKF结合GP残差建模的方法，成功实现了仅用陀螺仪数据就能同时估计机器人整体位姿和各关节角度的目标。

2. 核心算法原理

2.1 无迹卡尔曼滤波(UKF)基础

UKF是传统卡尔曼滤波的改进版，特别适合处理非线性系统。与扩展卡尔曼滤波(EKF)不同，UKF不需要对非线性函数进行线性化，而是通过"无迹变换"来近似概率分布。

具体来说，UKF会选取一组称为sigma点的样本点，这些点能精确捕获系统状态的均值和协方差。将这些sigma点通过非线性函数传播后，再重新计算统计特性。这种方法对强非线性系统有更好的适应性，计算复杂度与EKF相当，但精度通常更高。

在我们的应用中，系统状态包括：

• 位置(x,y)
• 朝向θ
• 两个关节角度α₁,α₂
• 各连杆角速度ω₁,ω₂,ω₃

2.2 高斯过程(GP)增强

传统UKF假设噪声是零均值高斯分布，但实际系统中常存在非零均值、非高斯的噪声。我们采用GP来建模这些残差，具体实现方式是：

1. 用运动捕捉系统收集大量训练数据
2. 计算名义模型预测与真实测量之间的残差
3. 训练GP模型学习这些残差的分布特性

在滤波过程中，GP会实时提供残差修正：

x_k+1 = f(x_k,u_k) + GP_μ(x_k,u_k) // 状态预测修正 y_k = h(x_k,u_k) + GP_μ(x_k,u_k) // 测量预测修正 Q_k = GP_Σ(x_k,u_k) // 过程噪声协方差 R_k = GP_Σ(x_k,u_k) // 测量噪声协方差

这种"Enhanced-GP-UKF"架构在[17]中已被证明优于标准UKF和传统GP-UKF。

3. 机器人系统建模

3.1 动力学模型

LandSalp采用准静态假设，认为推力与阻力瞬间平衡（适用于低雷诺数环境）。系统配置q由位姿g和形态r组成：

r = [α₁,α₂]ᵀ q = [g,r]ᵀ ˙q = [∘g,˙r]ᵀ

其中g∈SE(2)表示基座标系的平面位姿，∘g∈se(2)是基座标系下的体速度。

广义力方程考虑了各连杆推力、阻力以及关节阻力：

f_q = ∑J*_unit_i(∘f_thrust_i + ∘f_drag_i) + ∑J*_joint_j τ_drag_j

阻力模型采用线性粘滞阻尼：

∘f_drag_i = -d_unit_i ∘g_i τ_drag_j = -d_joint_j ˙r_j

3.2 步态设计

由于系统欠驱动，必须设计周期性步态来实现可控运动。我们采用一阶傅里叶级数参数化步态，包含常数偏移和正余弦项。实验验证了五种基本步态：

• 前进/后退
• 左移/右移
• 转向

图2展示了这些步态的控制输入和形态轨迹。特别值得注意的是，转向步态需要最大的推力输入，而侧向移动步态所需的推力最小。

4. 实验验证与结果分析

4.1 实验设置

LandSalp实验平台(图4)包括：

• 三个连杆组成的链式结构
• 每个连杆配备HEBI系列弹性执行器驱动全向轮
• 内置IMU提供陀螺仪数据
• OptiTrack运动捕捉系统提供地面真值

实验时，机器人通过轻质电缆供电和通信，避免引入外部干扰。控制命令以200Hz频率发送，IMU数据以相同频率采集。

4.2 模型验证

通过8次前进步态实验的平均数据验证了模型准确性(图6)：

1. 位姿速度预测与实测的相关系数达0.92
2. 关节速度预测捕捉了主要趋势，但存在二阶效应
3. 陀螺仪测量模型预测误差在±0.1rad/s内

4.3 状态估计性能

我们比较了两种GP训练策略：

1. 仅使用前进步态数据
2. 使用多步态混合数据

测试结果表明(图7)：

• 两种方法对关节角的估计误差都小于5°
• 位姿估计存在预期中的积分漂移
• 多步态训练的滤波器表现出相当的泛化能力

特别有趣的是关节角估计协方差的周期性变化(图8)，这与我们的可观测性启发式Λ高度相关。Λ定义为测量雅可比矩阵关于关节角的范数：

Λ = ||∂h(r,u)/∂α₁||

实验显示当Λ超过阈值时，协方差会明显减小，验证了"控制输入影响可观测性"的假设。

5. 关键实现细节

5.1 GP训练技巧

1. 输入特征选择：仅使用关节角度和推进器指令，共5维
2. 数据降采样：从200Hz降到5Hz，减少计算量
3. 核函数：采用MATLAB默认的平方指数核
4. 独立训练：为每个状态和输出维度训练单独的GP

5.2 实时性优化

虽然GP-UKF计算量较大，但通过以下方法实现实时运行：

1. 将更新频率降至1-10Hz
2. 采用稀疏GP近似
3. 在C++中实现核心算法

5.3 协方差调参

初始协方差设置：

• 位置：1cm标准差
• 朝向：5°标准差
• 关节角：5°标准差

过程噪声协方差通过GP预测方差在线调整。

6. 应用前景与局限

6.1 优势特点

1. 仅需陀螺仪即可实现形态估计
2. 对非高斯噪声具有鲁棒性
3. 可迁移到其他欠驱动多连杆系统

6.2 当前局限

1. 位姿估计存在不可避免的积分漂移
2. GP训练计算成本较高
3. 对剧烈运动的适应性有限

6.3 未来方向

1. 融合其他传感器（如磁强计）抑制漂移
2. 开发参数化残差模型替代GP
3. 扩展到水下机器人FloatSalp平台

在实际部署中，我发现周期性重置位姿估计（如通过视觉或GPS）能显著改善长期性能。另外，适当约束关节角范围可以提升GP在状态空间边缘区域的预测准确性。