第4期 孙自飞，等：多传感器的移动机器人可定位性估计与自定位 ·445. Fisher信息矩阵提出的，Fisher信息提供了观测信息 2.2利用RGB-D感知的未建模动态障碍物概率 中蕴含关于待估参数信息量大小的度量。针对服 估计 务机器人依靠激光数据和先验地图进行位姿估计， 由于激光传感器与Kinect传感器的数据已通 A.Censi[13]依据图2的机器人模型将Fisher信息矩 过坐标转换进行了对齐，本文采用Kinect传感器实 阵定义为式(2)所示的激光扫描到环境表面斜率的 现视野中的快速人体检测。在RGB-D数据中使用 函数。 HOG+SVM14]方法实现移动机器人作业场景中人体 的检测并得到关于人体的点云数据。 机器人 每一簇关于人体的点云经过点云投影和坐标 环境中的障碍物 机器人偏转角 变换到激光坐标系下后，利用其中的三维信息确定 机器人纵坐标 很 p.0+e,) 激光数据是由人体干扰引起的角度范围：对于每一 全局坐标系 传感器 簇对齐后的人体点云数据 机器人横坐标 {P:lp:=(:y:),i=1,2,…,n},计算每一个点云对 图2机器人在全局地图中的模型 应的角度6，即 Fig.2 Robot model in global map 0.arctan2(point[i].y,point[i].x)(5) (2) 并得到每一簇人体点云的统计信息，即角度6， i=1,2,…,n的最大和最小值0和0n。当使用 式中：p=(x,y,0)为机器人的位姿，r:为机器人在当 RGB-D传感器检测到场景中存在n个人时，用同样 前位姿下第i个激光束的长度，σ2为激光数据的噪 的方法确定n对0m.和0n,记做(0min,0mx）,k=1, 声方差。王炜o)根据概率栅格地图的特点对Fisher 2,…,n。对于一个特定范围本文用高斯模型确定 信息矩阵r(p)离散化，用△r/△p近似ars/ap,得 该范围内每一束激光上的未建模动态障碍物概率 到静态可定位性矩阵(P)。 并记为e,(i表示第i束激光)，如图3所示。 (p)仅考虑到已知静态概率栅格地图，未考虑 局部动态信息对机器人可定位性的影响。而在实 际环境中，未知障碍物会导致激光数据匹配度降低 甚至失效，无法反映其可定位性。因此在(p)的基 础上，考虑局部激光扫描数据，引入动态障碍物影 受人体干扰的 响因子s,以度量动态障碍物对激光扫描数据的 激光读数 影响。 如图2所示，机器人在参考位置P。= 受人体干扰的 激光读数 [xo,yo,]已知的前提下，设激光测距仪实际获取 π2 的第i个激光束的长度为，：，扫描到已知栅格为事 2 010 件A,扫描到未知障碍物为事件B,则根据贝叶斯公 图3未建模动态障碍物概率估计 式，该激光点扫描到已知栅格的概率为 Fig.3 Estimating the probability of unmodeled obstacles p(r:I A)p(A) p(AI r)=- (3) 则整个视场中激光束的未建模动态障碍物的 p(r:I A)p(A)+p(r:I B)p(B) 概率估计为E,可以用式(6)计算： 设第i个激光束的全局坐标位置(x:,y:)为 1 （9:-4)2 x:=x0+rC0s(9:+8。-T/2) -exp(- (4) 2TO 2u2）,9e(8m,6） yi=yo +r;sin(+0o-T/2) 0. 其他 则p(:A)=,04为原始地图中(，：)处的栅 (6) 格概率值，将1-p(A1r:)定义为未知障碍物影响因 式中：9：为第i束激光的角度，：和ok可用P 子s:,3:值越大该激光数据对机器人可定位性的提 Pan、9m、9amn唯一确定。若场景中出现提取出来的 高越小。 两簇点云有重叠的情形（图4）。Ｆｉｓｈｅｒ 信息矩阵提出的，Ｆｉｓｈｅｒ 信息提供了观测信息 中蕴含关于待估参数信息量大小的度量。 针对服 务机器人依靠激光数据和先验地图进行位姿估计， Ａ． Ｃｅｎｓｉ ［１３］依据图 ２ 的机器人模型将 Ｆｉｓｈｅｒ 信息矩 阵定义为式（２）所示的激光扫描到环境表面斜率的 函数。 图 ２ 机器人在全局地图中的模型 Ｆｉｇ．２ Ｒｏｂｏｔ ｍｏｄｅｌ ｉｎ ｇｌｏｂａｌ ｍａｐ τ（ｐ） ＝ ∑ ｎ ｉ １ σ ２ ∂ ｒ Ｔ ｉ ∂ｐ ∂ ｒｉ ∂ｐ é ë ê ê ù û ú ú （２） 式中：ｐ ＝ （ｘ，ｙ，θ）为机器人的位姿，ｒｉ 为机器人在当 前位姿下第 ｉ 个激光束的长度，σ ２ 为激光数据的噪 声方差。 王炜［６］根据概率栅格地图的特点对 Ｆｉｓｈｅｒ 信息矩阵 τ（ ｐ）离散化，用 ΔｒｉＥ ／ Δｐ 近似∂ｒｉＥ ／ ∂ｐ，得 到静态可定位性矩阵 τ ＾ （ｐ）。 τ ＾ （ｐ）仅考虑到已知静态概率栅格地图，未考虑 局部动态信息对机器人可定位性的影响。 而在实 际环境中，未知障碍物会导致激光数据匹配度降低 甚至失效，无法反映其可定位性。 因此在 τ ＾ （ｐ）的基 础上，考虑局部激光扫描数据，引入动态障碍物影 响因子 ｓｉ 以度量动态障碍物对激光扫描数据的 影响。 如 图 ２ 所 示， 机 器 人 在 参 考 位 置 Ｐ０ ＝ ｘ０ ，ｙ０ ，θ０ [ ] 已知的前提下，设激光测距仪实际获取 的第 ｉ 个激光束的长度为 ｒｉ，扫描到已知栅格为事 件 Ａ，扫描到未知障碍物为事件 Ｂ，则根据贝叶斯公 式，该激光点扫描到已知栅格的概率为 ｐ（Ａ ｜ ｒｉ） ＝ ｐ（ｒｉ ｜ Ａ）ｐ（Ａ） ｐ（ｒｉ ｜ Ａ）ｐ（Ａ） ＋ ｐ（ｒｉ ｜ Ｂ）ｐ（Ｂ） （３） 设第 ｉ 个激光束的全局坐标位置（ｘｉ，ｙｉ）为 ｘｉ ＝ ｘ０ ＋ ｒｉ ｃｏｓ（φｉ ＋ θ０ － π／ ２） ｙｉ ＝ ｙ０ ＋ ｒｉ ｓｉｎ（φｉ ＋ θ０ { － π／ ２） （４） 则 ｐ（ｒｉ ｜ Ａ）＝ μｘｉ ｙｉ 。 μｘｉ ｙｉ为原始地图中（ ｘｉ，ｙｉ）处的栅 格概率值，将 １－ｐ（ Ａ ｜ ｒｉ ）定义为未知障碍物影响因 子 ｓｉ，ｓｉ 值越大该激光数据对机器人可定位性的提 高越小。 ２．２ 利用 ＲＧＢ⁃Ｄ 感知的未建模动态障碍物概率 估计 由于激光传感器与 Ｋｉｎｅｃｔ 传感器的数据已通 过坐标转换进行了对齐，本文采用 Ｋｉｎｅｃｔ 传感器实 现视野中的快速人体检测。 在 ＲＧＢ⁃Ｄ 数据中使用 ＨＯＧ＋ＳＶＭ ［１４］方法实现移动机器人作业场景中人体 的检测并得到关于人体的点云数据。 每一簇关于人体的点云经过点云投影和坐标 变换到激光坐标系下后，利用其中的三维信息确定 激光数据是由人体干扰引起的角度范围：对于每一 簇 对 齐 后 的 人 体 点 云 数 据 ｐｉ ｜ ｐｉ ＝ （ｘｉ，ｙ { ｉ），ｉ ＝ １，２，…，ｎ} ，计算每一个点云对 应的角度 θｉ，即 θｉ ＝ ａｒｃｔａｎ２（ｐｏｉｎｔ［ｉ］．ｙ，ｐｏｉｎｔ［ｉ］．ｘ） （５） 并得到每一簇人体点云的统计信息，即角度 θｉ， ｉ ＝ １，２，…，ｎ 的最大和最小值 θｍａｘ 和 θｍｉｎ 。 当使用 ＲＧＢ⁃Ｄ 传感器检测到场景中存在 ｎ 个人时，用同样 的方法确定 ｎ 对 θｍａｘ和 θｍｉｎ ，记做（θｋｍｉｎ ，θｋｍａｘ），ｋ ＝ １， ２，…，ｎ。 对于一个特定范围本文用高斯模型确定 该范围内每一束激光上的未建模动态障碍物概率 并记为 εｉ（ｉ 表示第 ｉ 束激光），如图 ３ 所示。 图 ３ 未建模动态障碍物概率估计 Ｆｉｇ．３ Ｅｓｔｉｍａｔｉｎｇ ｔｈｅ ｐｒｏｂａｂｉｌｉｔｙ ｏｆ ｕｎｍｏｄｅｌｅｄ ｏｂｓｔａｃｌｅｓ 则整个视场中激光束的未建模动态障碍物的 概率估计为 εｉ，可以用式（６）计算： εｉ ＝ １ ２πσｋ ｅｘｐ（ － （φｉ － μｋ） ２ ２σｋ ２ ）， φｉ ∈ （θｋｍｉｎ ，θｋｍａｘ） ０， 其他 ì î í ï ï ï ï （６） 式中：φｉ 为第 ｉ 束激光的角度，μｋ 和 σｋ 可用 ｐｍａｘ、 ｐｍｉｎ 、θｋｍａｘ、θｋｍｉｎ唯一确定。 若场景中出现提取出来的 两簇点云有重叠的情形（图 ４）。 第 ４ 期 孙自飞，等：多传感器的移动机器人可定位性估计与自定位 ·４４５·