四腿机器人定位的理论与实现 课件

二、定位模块实现与发展
Monte Carlo 定位 Markov 定位中的分布因为是连续的，
所以在实际中无法在计算机上直接使用， 这需要将其离散化。而Monte Carlo 定 位实际上是Markov 定位的一种离散化。
二、定位模块实现与发展
Monte Carlo 定位 Monte Carlo 定位使用一个样本集来表示定位点的可能
一、定位模块功能及基本环境
四腿机器人系统分为五个功能模块：视觉模 块，动作模块，定位模块，通讯模块和行为 决策模块。
定位模块功能：一方面从视觉模块获得图像 分析结果，另一方面从动作模块获得当前的 身体偏移走向，综合两方面的内容，试图计 算出机器人身体在场地上面的位置，告知行 为决策模块。
模块间的层次定位和相互之间的信息传递如 下页图所示。
在计算了所有的点后，即可得到新的分布。
二、定位模块实现与发展
Markov 定位 根据传感器数值修正： 这种修正使用了Bayes 法则。用s 表示一个传 感器获取的数值，P { s | L }表示在定位点L 下获取s 的可能性。这样我们有
其中α为一个归一常量。这种修正有一个前提条件， 即过去的传感器信息读取与未来的传感器信息读 取是独立的，这种条件下的环境称为Markovian Environment。
二、定位模块实现与发展
样本集维护—— 行动更新
作的定义：三个偏 移量：dx，dy，d。二、定位模块实现与发展
样本集维护——行动更新 第一步就要把动作偏移量从机器人自身的坐标系
变换到场地坐标系中，这样就可以知道执行的动 作使得机器人在场地上到底走了多大的偏移量。 设δx ，δy ，δθ 分别为动作的三个偏移量，θ 为 原定位结果的身体朝向角度， Δx和Δy为场地坐 标系中的x，y 偏移量，有以下公式：
3．我如何才能到达那里？一旦机器人明确了前面的两个问题， 它就会决定如果才能达到那里。路径规划(path planning)就是帮 助机器人找到达到目标的方法。
自主移动式机器人的定位
机器人定位研究的就是第一个问题“我在哪里”。对这个问题， 现有的研究并没有找到一种完美的解决方案，已有的方法可分 为两种：相对位置测量方法（包括“里程计法”和惯性导航法 等等）和绝对位置测量方法（包括“主动的灯塔法”，人工/自 然地标识别法，模式匹配法和基于视觉的方法等等）。在实际 研究中，通常将两类方法结合使用。
二、定位模块实现与发展
定出单点——动作更新 根据机器人完成的动作修正定位。动作 信息有dx, dy, dθ。这些都是相对机器人 自身坐标系而言的。通过坐标系变换， 将这三个偏移量变换为场地坐标系下的 实际偏移量，然后加到原定位点上，产 生新的定位结果。
二、定位模块实现与发展
定出单点——视觉更新 视觉更新完全依赖于视觉模块提供的6 个地标 的信息。对每个看到的地标，视觉模块提供 以下两个信息：此地标与机器人中心（机器 狗的身体部分在场地上的投影的中心）的距 离，此地标相对于机器人朝向方向的夹角。 1 双地标定位 2 单地标调整
自主移动式机器人的定位
四腿机器人aibo上面的输入设备： 彩色CMOS摄像头；立体声麦克风；头部、
背部静电传感器；爪子、下巴开关式传感 器；头部、胸部三个距离传感器；加速度 传感器（三维）；震动传感器。 由于距离传感器的实际运行效果不好，定 位模块使用的关于外部环境的输入信息都 是由视觉模块处理摄像头所得到的图像以 后所得到的信息。
四腿机器人定位 的理论与实现
定位模块讲解内容
自主移动式机器人的定位 定位模块功能及基本环境 定位模块实现与发展
定出单点 蒙特卡罗定位(Monte Carlo) 卡尔曼滤波(Kalman Filter) 改进定位结果
的抖动性
自主移动式机器人的定位
移动性：顾名思义，移动性是指机器人可以在何种程 度上自由地在世界中移动。最初的机器人----所谓的 操纵器、机械手----只能反复重复地做一些动作，比 如它们含有一个固定在地面上、可以移动的机器手臂， 但是它们无法自由地移动，也就是说，它们还没有具 备足够的移动性。
一、定位模块功能及基本环境
比赛中的人为因素 根据规则的规定，机器人在犯规的时候 会被裁判拿起并放置到场地的其他位置 上。而这种人为的移动，机器人是无法 及时准确了解的。
二、定位模块实现与发展
定出单点
主要是根据视觉模块传来的地标信息和动 作模块传来的动作信息进行简单的几何方 法计算定位。 1 动作更新 2 视觉更新
球门信息主要包括两个门柱与机器人身体朝向的夹角 a1 和a2。我们根据球门的位置和这两个夹角也可以 在场地坐标系中确定一个圆，并得到机器人身体朝向 相对于球门中心的夹角b，用于更新样本（如下页图 右）。
性分布。这个集合是有限的（设N 个样本），所以实际 上只是表示了空间中的一部分定位点作为代表。每个样 本（sample）表示一个可能的定位点，包含x，y，θ 三 个量，同时还包含一个类似于Bel(L)作用的量p。即每个 样本为((x , y, θ) ,p) 。应该保证p 值是归一的，即∑p=1 。这样，样本最密集的地方就是最终定位结果。
取的信息能改变样本点的可信度的时候， 如视觉更新，才会进行样本集重采样。 样本集重采样是为了调整样本可信度分 布，使之更集中于可能的分布，排除不 可能分布（可信度p为零的样本点）。
二、定位模块实现与发展
样本集维护——视觉更新
定位系统利用视觉模块提供的地标和球门的信息更新 样本集。
地标信息主要包括地标的编号，与机器人的距离d 和 与机器人身体朝向的夹角b。由于地标的位置是固定 的，因此根据这些信息可在球场坐标内确定一个圆弧， 作为样本更新的依据（如下页图左）。
具体实现
基本设定
场地坐标系 机器人自身坐标系
样本集维护
产生高斯噪声 行动更新（没有样本集重采样） 视觉更新（需要样本集重采样）
定位结果计算
二、定位模块实现与发展
机器人自身坐标系 场地坐标系
二、定位模块实现与发展
样本集维护——产生高斯噪声 机器人在执行动作和获取图像的时候都不可
自主性：自主性(autonomy)取决于在何种程度上一 个机器人依赖于它可能获得的前验知识和关于环境的 信息。具体的，可以分为非自主、半自主和全自主。 全自主机器人可以在完全没有外界引导操作的情况下， 完成一些智能的行动和动作。
自主移动式机器人的定位
机器人导航的任务就是将自动移动式机器人从一个位置安全地 移动到另外一个位置。机器人导航的主要问题可以分为三个基 本问题：
二、定位模块实现与发展
双地标定位 （TwoMarkLoc） 如果在一个视觉帧中 （即同一画面中）看到 了多个不同的地标，或 者在机器人身体不动的 情况下，连续看到了多 个不同的地标，就可以 通过画圆求交点的方法 计算定位点。如图。
二、定位模块实现与发展
单地标调整 （OneMarkAdjustLoc） 如果只有一个地标的信 息可用，就在原有定位 结果上进行调整：以这 个地标的信息在场地中 画一个圆弧，然后以当 前定位点和地标作一直 线，它们的交点就是新 定位点。如图。
这个样本集同样也通过行动和传感器获取的数值进行修 正更新。
二、定位模块实现与发展
Monte Carlo 定位 根据行动更新：
根据样本的可信度p 值，从样本集合中 随机取出样本（p 值高的，被取出的可 能性就越大），然后根据P ( L | L’, a ) 计算出新的样本。这样从原来的样本集 中总共取N 次样本并计算即可产生新的 样本集合。
一、定位模块功能及基本环境
机器人传感器数据的不确定性 AIBO 机器人能用于定位的传感器信息主要来源于视觉。而 视觉信息不可避免的会出现一些误差或错误。 例如，如果机器人离地标距离比较远，看到的地标颜色 块的像数点数目很少，这种情况下视觉信息的抖动特别 大，前一帧看到四个像数点，可能下一帧就只有两个， 用像数点数目统计来计算距离的方法就给距离的计算带 来很大误差，表现在定位的不准确以及短时间内定位结 果的抖动。 机器人的头在快速运动的时候，视觉图像会产生运动模 糊，造成地标的边界模糊，对视觉处理造成严重影响。 另外，环境的干扰也会造成偶尔的误识别，由于对标示 物的识别是基于颜色的识别的，如果场外有别的物体或 人衣服的颜色相同或相近，就有可能把把他们误识别为 地标。
基于视觉的定位方法主要利用光学传感器（比如激光距离探测 器，CCD光学照相机等）来搜集信息，相对其它的传感器，光 学传感器能得到的信息最为庞大，而如何从中提取所需特征引 起了广泛的关注。地表识别法和模式匹配法中相关的方案都可 以应用在这里。在已经展开的研究中，以下途径被使用：地标、 物体模型、地图以及基于特征的地图重建。
能做到很精确，会有一些误差在里面。为了 在定位过程中将这种误差考虑进去，可以在 每次根据动作或者视觉对样本进行修正以后 人为的加入一定的噪声来模拟这种误差。一 般来说，观测值总是在真实值的左右跳动， 误差的均值一般为零。而且越靠近真值的观 测值出现的可能性越高。高斯噪声（就是正 态分布的噪声）比较好的符合这些条件。
二、定位模块实现与发展
优点： 速度快，系统消耗少。在看到两个地标 的时候能够迅速修正定位结果到相对准 确的位置上。
缺点： 主要缺点是抗信息抖动能力差。
二、定位模块实现与发展
蒙特卡罗定位(Monte Carlo) Monte Carlo 定位的基础是Markov 定位。Markov 定位的
二、定位模块实现与发展
Monte Carlo 定位 根据传感器获得的数值更新： 对于每个样本，根据P { s | L }计算出它的p 值。
然后根据各样本新生成的p 值进行重采样 （resample）。即从原样本集中根据新计算的p 值随机的取出样本，取N 次，即可生成新的样本 集合。
二、定位模块实现与发展
角度偏移量不需变换。
二、定位模块实现与发展
样本集维护——行动更新 对于每个样本的x，y，θ 三个量，根据以下公式
更新。其中x，y，θ 为原样本数据，x’，y’， θ’为新样本数据
gx，gy，gθ 为高斯噪声。
二、定位模块实现与发展
样本集维护——行动更新 行动更新没有样本集重采样。只有在获
这个分布随着机器人的行动和传感器获得的数值而进行 修正。
二、定位模块实现与发展
Markov 定位 根据行动修正： 根据行动的修正模型为条件概率P ( L |L’,a )， 表示机器人在L’位置执行了行动a 后达到L 位置的可能性。实际上这是对机器人的行 动机械性质作了一个描述。这样，对于定 位点的Bel(L)可以根据以下公式求得
1．我在哪里？机器人必须知道自己当前的位置，这样才能做出 正确的决定。机器人定位(robot localization)的目的就是找到机 器人的行踪。
2．我将要去哪里？为了完成一些任务，机器人必须知道它将要 去何处。因此，它需要识别一个目的地（目标），那么这类问 题就是目标识别(goal recognition)。
一、定位模块功能及基本环境
一、定位模块功能及基本环境
RoboCup 四腿机器人足球赛是一个典型 的动态不确定性多主体环境。这种不确 定性对定位的影响很大，主要表现为：
机器人行走的不确定性 机器人传感器数据的不确定性 比赛中的人为因素
一、定位模块功能及基本环境
机器人行走的不确定性 在比赛之中，双方的机器人会因为争抢 足球而互相拥挤碰撞，或者机器人行走 中触碰球场壁，这些都会导致机器人行 走动作被阻塞。而由于AIBO机器人缺 乏必要的传感器，导致这种阻塞无法有 效的检测出来。
基本思想是：机器人维持一个对自己当前可能的定位点 的一个信念分布。
我们将每个定位点表示为一个向量L={x, y, θ}，x 和y 分 别表示坐标，θ 表示朝向。对于场地中所有的定位点， 机器人有一个自己是否处于该点的可能性信念值，记为 Bel(L)。这样我们就有了一个在空间( x, y, θ )中的Bel(L) 分布，注意这是一个连续的分布。这个分布中，可能性 最高的点可以视为最终的定位点。