\

3. 1 位姿传感器

如今, 已经存在不少的位姿传感器可以获取空间中物体的位置和姿态, 它们通过计算物体运动的连续图像或者跟踪物体的运动, 从而得到物体的运动变 化参数。在基于位姿传感器的在线误差测量模型中, 我们使用了一个位置传感器来跟踪机器人末端的位置, 惯性测量单元 IMU 被用来测量机器人末端的姿态变化。基于位姿传感器的在线误差测量实验场景如图 4-10 所示, 其在机器 人的末端绑定一个惯性测量单元, 因此它的姿态变化可以认为是机器人末端的姿态变化, 同时在机器人正前方固定了一个位姿传感器和一个摄像头, 摄像头只用来观察实验中的反馈, 位姿传感器结合视觉算法可以跟踪机器人末端的位置变化。 1. 位置传感器工作原理
位置传感器工作原理是根据其内置的软件和摄像头检测来捕捉物体的运动。一般地, 位置传感器中内置了三个摄像头, 其中一个为彩色摄像头, 用来获取物体运动的彩色图像信息, 另外是两个深度摄像头, 用于获取物体的深度图像信息。两个深度摄像头中有一个负责发射红外光, 另外一个负责接收红外光 。通过使用位置传感器可以结合视觉跟踪的方法, 获取机器人末端的位置。该方法需要对物体的特征进行提取。由于惯性测量单元 IMU 被绑定在机器人末端, 因此, IMU 的位置可以默认为机器人末端的位置, 通过结合视觉方法可以识别 IMU 的位置。基于视觉的目标跟踪方法主要有 Camshift 算法和 K-Means 算法, 它们通过对前后两帧图像的运算来检测物体运动的变化, 这两种方法在检测物体运动方面已经获得了较好的效果。

1)

Camshift 算法

Camshift算法是一种跟踪算法, 算法流程图如图 4-11 所示。该算法使用 目标物体的颜色为特征, 首先将采集的图像从 RGB 颜色空间变换到 HSV 颜色空间, 接着用直方图中的像素的统计量来代替相应原始图像, 然后重新量化所得到的结果, 使其落在[0,255]的范围之内, 从而获得颜色概率分布图。接着通过得到的颜色概率分布图来设置一个用于目标识别的搜索窗的位置和大小, 在对目标跟踪和识别的过程中, 对于搜索窗以外的像素, 其颜色概率为零, 而对 于确定搜索窗内的每一个像素，它的概率分布值可以通过搜索目标的跟踪直方图来获得。如果所有目标像素的概率分布被确定之后, 就能够计算出目标物体的质心。根据物体质心，重新确定搜索窗口的中心，直到确定目标的位置为止。下面是 Camshift 算法的工作过程:(1) 在颜色概率分布图中确定大小为  且包含被跟踪目标的搜索窗。(2) 根据上一步中所设定的搜索窗, 进行相应计算:  (4-66) 式(4-66) 表示零阶矩阵。  (4-67)式(4-67) 表示一阶矩阵。其中, 搜索窗的变化范围为  ; 图像坐标的像素值为  。(3)计算搜索窗质心:  (4-68)(4) 重新调整搜索窗的大小, 使其符合窗内的颜色概率分布函数。(5) 重复 (2)、(3)、(4) 步骤直到确定目标的位置为止。经过上述步骤可得二阶矩阵:  (4-69)而被跟踪目标的长轴、短轴和方向角, 也可以通过以下计算得出:目标物体的长轴方向角的计算公式为:  (4-70)目标物体的长轴和短轴的计算公式为:  (4-71)其中，  (4-72) 在目标跟踪的过程中, 上一帧计算得到的搜索窗口将作为下一帧的初始窗 口继续跟踪, 反复迭代下去, 从而通过对相邻两帧图像的分析可以计算出机器人末端的位置。 

2)

K-means 算法

K-means算法是一种基于距离迭代的聚类算法, 它往往使用欧式距离作为 相似性的评价指标。首先，它将采集到的  个观察实例分类到  个聚类当中，分类的标准是使每个观察实例距离它所在的聚类的中心点比到其他聚类中心点的 距离更小。K-means 算法的工作流程图如图 4-12 所示。K-means 算法的工作过程:(1) 从观测实例中随机选取出  个观测点作为聚类中心点, 然后依次遍历其 余的观测实例, 找到其到这  个聚类中心点距离Zui近的中心点, 并且将其加入到该聚类中。(2) 每个聚类中心至少有一个观测实例。这样, 可以求出每个聚类的中心点 作为新的聚类中心, 然后再遍历所有的观测实例, 找到距离其Zui近的中心点, 加人到该聚类中。(3) 不断重复上述过程, 直到标准测度函数开始收玫为止, 即前后两次迭代 得到的聚类中心点是相同的。 通常计算聚类中心点有以下三种方法：(1) 闵可夫斯基距离 (Minkowski distance) 公式:  （4-74）其中,   可以为任意值。当  取值为 2 或 1 时, 分别得到以下公式。(2) 欧几里得距离 (Euclidean distance) 公式:  （4-75）(3) 曼哈顿距离 (CityBlock distance) 公式:  （4-76）