DRAFT
机器人学的状态估计
作者
Timothy D. Barfoot (tim.barfoot@utoronto.ca)
译者
⾼翔 (gao.xiang.thu@gmail.com)
颜沁睿 (castoryan1991@gmail.com)
刘富强 (liufuqiang_robot@hotmail.com)
郭⽟峰 (taylorguo@126.com)
秦超 (qinchaom4a1@163.com)
谢晓佳 (zerosmemories@gmail.com)
郑帆 (fzheng@link.cuhk.edu.hk)
吴博 (wubo.neu@gmail.com)
张明 (nkzimmon@gmail.com)
左星星 (zuojiaxing1994@foxmail.com)
范帝楷 (fandikai@outlook.com)
张明明 (robotsming@gmail.com)
最后更新 2018 年 1 ⽉ 15 ⽇
DRAFT
目 录
第一章 引 言 5
1.1 状态估计简史 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.2 传感器，测量和问题定义 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.3 本书组织结构 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
1.4 与其它教程的关系 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
第一部分 状态估计机理 10
第二章 概率论基础 11
2.1 概率密度函数 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.1.1 定 义 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.1.2 贝叶斯公式及推断 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.1.3 矩 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.1.4 样本均值和样本⽅差 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.1.5 统计独⽴性与不相关性 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.1.6 归⼀化积 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.1.7 ⾹农和互信息 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.1.8 克拉美罗下界和费歇尔信息量 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.2 ⾼斯概率密度函数 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
2.2.1 定 义 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
2.2.2 Isserlis 定理 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
2.2.3 联合⾼斯概率密度函数，分解与推断 . . . . . . . . . . . . . . . . . . . . . . . . . 20
2.2.4 统计独⽴性，不相关性 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
2.2.5 ⾼斯分布随机变量的线性变换 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
2.2.6 ⾼斯概率密度函数的归⼀化积 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
2.2.7 Sherman-Morrison-Woodbury 等式 . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
2.2.8 ⾼斯分布随机变量的⾮线性变换 . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
2.2.9 ⾼斯分布的⾹农信息 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
2.2.10 联合⾼斯概率密度函数的互信息 . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
2.2.11 ⾼斯概率密度函数的克拉美罗下界 . . . . . . . . . . . . . . . . . . . . . . . . . . 31
2.3 ⾼斯过程 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
2
DRAFT
2.4 总 结 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
2.5 习 题 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
第三章 线性高斯系统的状态估计 36
3.1 离散时间的批量估计问题 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
3.1.1 问题定义 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
3.1.2 最⼤后验估计 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
3.1.3 贝叶斯推断 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
3.1.4 存在性，唯⼀性与能观性 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
3.1.5 MAP 的协⽅差 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
3.2 离散时间的递归平滑算法 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
3.2.1 利⽤批量优化结论中的稀疏结构 . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
3.2.2 Cholesky 平滑算法 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
3.2.3 Rauch-Tung-Striebel 平滑算法 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
3.3 离散时间的递归滤波算法 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
3.3.1 批量优化结论的分解 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
3.3.2 通过 MAP 推导卡尔曼滤波 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
3.3.3 通过贝叶斯推断推导卡尔曼滤波 . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
3.3.4 从增益最优化的⾓度来看卡尔曼滤波 . . . . . . . . . . . . . . . . . . . . . . . . . 64
3.3.5 关于卡尔曼滤波的讨论 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
3.3.6 误差动态过程 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
3.3.7 存在性，唯⼀性以及能观性 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
3.4 连续时间的批量估计问题 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
3.4.1 ⾼斯过程回归 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
3.4.2 ⼀种稀疏的⾼斯过程先验⽅法 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
3.4.3 线性时不变情况 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
3.4.4 与批量离散时间情况的关系 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80
3.5 总 结 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80
3.6 习 题 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81
第四章 非线性非高斯系统的状态估计 83
4.1 引 ⾔ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83
4.1.1 全贝叶斯估计 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84
4.1.2 最⼤后验估计 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85
4.2 离散时间的递归估计问题 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 87
4.2.1 问题定义 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 87
4.2.2 贝叶斯滤波 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88
4.2.3 扩展卡尔曼滤波 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90
4.2.4 ⼴义⾼斯滤波 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93
4.2.5 迭代扩展卡尔曼滤波 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94
4.2.6 从 MAP ⾓度看 IEKF . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95
DRAFT
4.2.7 其它将 PDF 传⼊⾮线性函数的⽅法 . . . . . . . . . . . . . . . . . . . . . . . . . . 96
4.2.8 粒⼦滤波 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102
4.2.9 sigmapoint 卡尔曼滤波 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104
4.2.10 迭代 sigmapoint 卡尔曼滤波 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 109
4.2.11 ISPKF 与后验均值 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110
4.2.12 滤波器分类 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111
4.3 离散时间的批量估计问题 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112
4.3.1 最⼤后验估计 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112
4.3.2 贝叶斯推断 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118
4.3.3 最⼤似然估计 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 120
4.3.4 讨 论 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124
4.4 连续时间的批量估计问题 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125
4.4.1 运动模型 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126
4.4.2 观测模型 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 128
4.4.3 贝叶斯推断 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 128
4.4.4 算法总结 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 129
4.5 总 结 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 129
4.6
习 题
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 130
第五章 偏差，匹配和外点 132
5.1 处理输⼊和测量的偏差 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132
5.1.1 偏差对于卡尔曼滤波器的影响 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132
5.1.2 未知的输⼊偏差 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 135
5.1.3 未知的测量偏差 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137
5.2 数据关联 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 139
5.2.1 外部数据关联 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 139
5.2.2 内部数据关联 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 140
5.3 处理外点 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 140
5.3.1 随机采样⼀致性 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141
5.3.2 M-估计 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142
5.3.3 协⽅差估计 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 144
5.4 总 结 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 146
5.5 习 题 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 146
第二部分 三维空间运动机理 147
第六章 三维几何学基础 148
6.1 向量和参考系 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 148
6.1.1 参考系 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 148
6.1.2 点 积 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 149
DRAFT
6.1.3 叉 积 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 150
6.2 旋 转 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 151
6.2.1 旋转矩阵 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 151
6.2.2 基本旋转矩阵 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 152
6.2.3 其它旋转表⽰形式 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153
6.2.4 旋转运动学 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 159
6.2.5 加上扰动的旋转 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163
6.3 姿 态 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 166
6.3.1 转换矩阵 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 167
6.3.2 机器⼈学的符号惯例 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 168
6.3.3 弗莱纳参考系 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 170
6.4 传感器模型 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 172
6.4.1 透视相机 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 173
6.4.2 ⽴体相机 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 179
6.4.3 距离-⽅位⾓-俯仰⾓模型 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 181
6.4.4 惯性测量单元 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 182
6.5 总 结 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184
6.6 习 题 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184
第七章 矩阵李群 186
7.1 ⼏何学 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 186
7.1.1 特殊正交群和特殊欧⼏⾥得群 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 186
7.1.2 李代数 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 188
7.1.3 指数映射 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 190
7.1.4 伴 随 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 196
7.1.5 Baker-Campbell-Hausdorff . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 200
7.1.6 距离、体积与积分 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 207
7.1.7 插 值 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 209
7.1.8 齐次坐标点 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 214
7.1.9 微积分和优化 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 215
7.1.10 公式摘要 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 222
7.2 运动学 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 222
7.2.1 旋 转 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 222
7.2.2 位 姿 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 225
7.2.3 旋转线性化 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 227
7.2.4 线性化位姿 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 231
7.3 概率与统计 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 232
7.3.1 ⾼斯随机变量和概率分布函数 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 233
7.3.2 旋转向量的不确定性 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 236
7.3.3 位姿组合 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 238
评论1