运动传感器与位姿估计

里程编码器(测量距离)

  • 基本原理：记录刻度数，测量转角(转动端相对于固定端的转角)，转换为弧长(≈行驶里程)

  • 实质测量的是转角

image.png

  • 里程编码器误差：里程编码器测量的里程≠真实里程。本质上是轮子(装有里程编码器)里程≠车的里程

  • e.g.：一个轮子气多，一个轮子气少；地面凹凸不平；光滑地面/沙地

  • 测量车轮行驶里程的里程编码器又称车轮编码器Wheel Encoder

image.png

编码器数据

  • 数据文件：COMPort_X_20130903_195003.txt

  • 格式（文本）：E Millisecond 1 Count

  • Count：累计计数，[1,30000]

  • 单位计数对应的里程数：1 Count ≈ 0.003846154 meter

image.png

  • 数据有跳跃是因为测量帧率不够高

惯性传感器

  • 陀螺仪、加速度计、惯性测量单元(Inertial Measurement Unit, IMU)

  • 见 04 惯性传感器

位姿估计

  • 利用运动传感器和运动模型，可以对机器人的位置姿态进行估计

  • 里程计法（Odometry）：

    1. 利用轮速编码器等传感器和运动学模型，估计帧间机器人位姿变化
    2. 此外，可用利用环境传感器，如视觉里程计Visual Odometry、激光里程计LiDAR Odometry

  • 航位推算法(Dead Reckoning)：

    1. 已知当前位姿，通过轮速编码器、惯性传感器等推算下一个时刻的估计机器人位姿

  • 两种方法是紧密相关的，里程计法积分实质上就是航位推算法

  • 机器人运动模型：运动模型 g 描述机器人在 t 时刻执行控制命令 ut 后从前一个状态 xt-1 转移到下一个状态 xt 的过程

image.png

里程计法（Odometry）

  • 利用轮速编码器等传感器和运动学模型，估计帧间机器人位姿变化

image.png

  • Odometry已知δSl、δSr，求δxt

  • 差速驱动机器人里程计模型：

image.png

航位推算法(Dead Reckoning)

  • 已知当前位姿，通过轮速编码器、惯性传感器等推算下一个时刻的估计机器人位姿

  • 通过轮速编码器得到δtrans，通过惯性传感器得到δrot。已知xt-1、δtrans、δrot，求xt

image.png

  • 简化：机器人直线运动

image.png

  • 简化：δrot1 = δrot2 = 0.5 * δrot

image.png

位姿估计的误差

  • 造成误差原因：
    1. 简化的运动模型与计算
      1. 机器人运动模式的多样性
      2. 数据采集帧率不足
    2. 传感器误差
    3. 状态估计中的误差传播（误差积累）

简化的航位推算法

image.png

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
import math
import random

def sample_motion_model(u, x, alpha):
    """
    u = (delta_rot, delta_trans)
    x = (x, y, theta)
    alpha = (alpha1, alpha2, alpha3, alpha4)
    """
    delta_rot, delta_trans = u
    x_pos, y_pos, theta = x
    alpha1, alpha2, alpha3, alpha4 = alpha

    # Step 1: 加噪声的旋转量
    delta_rot_hat = delta_rot + random.gauss(0, alpha1 * abs(delta_rot) + alpha2 * delta_trans)

    # Step 2: 加噪声的平移量
    delta_trans_hat = delta_trans + random.gauss(0, alpha3 * delta_trans + alpha4 * abs(delta_rot))

    # Step 3: 更新位置
    x_prime = x_pos + delta_trans_hat * math.cos(theta + delta_rot_hat / 2.0)

    # Step 4: 更新位置
    y_prime = y_pos + delta_trans_hat * math.sin(theta + delta_rot_hat / 2.0)

    # Step 5: 更新方向
    theta_prime = theta + delta_rot_hat

    # Step 6: 返回新的状态
    return (x_prime, y_prime, theta_prime)