目标定位^[1]是仿人机器人研究的一个重要内容.在人机交互中, 机器人须将交互对象从环境中识别出来, 并获得两者之间的相对位置信息, 用以进行更深层次、更自然的交互.仿人机器人平台的定位系统多为被动式定位系统^[2-6], 即机器人通过若干传感器获得目标位置信息, 如仿人机器人Han、Sophia、SociBot、iCub、“可佳”、“西施”等, 都在头部或躯干部位布置了若干传感器, 以获得相关信息.被动式定位技术通过处理环境或交互对象发射或反射的信号进行目标定位, 定位方式单一且传感器的定位效果受外界因素的影响较大^[7], 如听觉定位须限制声音环境, 视觉定位须限制光照条件等.

为了减少机器人对定位环境的依赖性, 提高定位精度, 本文设计多传感器定位系统.该系统应用于上海大学研制的仿人机器人SHFR-Ⅲ人机交互系统上, 包括由热释电红外传感器阵列组成的红外定位系统、听觉传感器阵列组成的听觉定位系统和双目视觉定位系统.通过对系统中传感器获得的信息进行处理, 得到机器人与交互目标的相对位置关系；通过多个传感器的数据融合和功能互补, 减弱系统对定位环境的依赖性, 提高整体定位精度.

1 多传感器定位系统设计 1.1 基于传感器阵列的红外定位

红外传感器受环境的光照、噪声等因素影响小^[8], 可以在复杂环境下实现交互目标的识别与定位.针对粗略、快速的定位需求, 选用热释电红外(pyroelectric infrared, PIR)传感器, 型号为RobotBase RB-02S017A(工作温度为-20~80 ℃, 输出延时为2.3~3.0 s).检测区间为圆心角为120°、半径为6 m的扇形运动区域.

选择4个相同参数的热释电红外传感器, 构成图 1所示的传感器阵列^[9-10].4个传感器垂直等距分布, 相邻传感器的水平夹角为30°, 从上到下4个传感器的输出结果分别记为：S₁、S₂、S₃、S₄.该传感器阵列可以覆盖正面0~210°范围.

定义定位函数Fr(S₁, S₂, S₃, S₄)为传感器输出值S₁、S₂、S₃、S₄和目标方位信息θ之间的映射关系.S₁、S₂、S₃、S₄共7种组合方式, 每一种都对应于检测区域的一个扇形区间, 方位信息F(0°~210°)为传感器测得的相应方位信息范围, θ为该区间的中间角度, 这样可以保证定位精度相对最高, 为区间大小的一半, 即15°.输出值S₁、S₂、S₃、S₄为1或0, 对应于传感器输出高、低电平, 故可以用表 1所示的真值表描述定位函数Fr(S₁, S₂, S₃, S₄).

该方法借助多个热释电红外传感器交错布置, 使所有传感器的检测区间部分重叠.将整个定位区域划分为若干个单独区域和重叠区域, 通过传感器阵列输出结果, 判断目标所处位置, 实现目标定位.

1.2 基于听觉传感器阵列的声源定位

听觉传感器可以弥补视觉传感器视角有限和依赖光照的缺陷, 实现360°全方位的信息获取, 且不受环境中障碍物的影响.可以通过声音, 从环境中识别多个潜在交互对象.

基于听觉传感器阵列的声源定位, 将多个麦克风布置成三角形传感器阵列.每个传感器相对于声源的位置不同, 获得的由声源传来的声音信号不同.结合传感器间的相对位置关系, 计算出声源的位置.一组声达时间差所对应的一组渐近线的交点, 即声源位置^[11].对于水平面内的听觉定位, 2个渐近线的交点可以确定声源位置坐标, 因此3个听觉传感器构成的阵列可以满足定位需要.

设计的听觉传感器阵列呈三角形分布(型号为RobotBase RB-02S084), 3个传感器在垂直面内为等腰三角形分布, 传感器B、C水平布置, 间距为p.传感器A位于B、C中垂线上, 距离为q, 声源记为P(x, y, z), 分布模型如图 2所示.

定义传感器2和传感器1间的声达时间差为t₂₁, 传感器3和传感器1间的声达时间差为t₃₁.根据传感器采样频率50 kHz和声速340 m/s, 可得声达时间差t与距离差d的关系为

$ d = 6.8t. $

(1)

根据声达时间差的定义, 可得

$ PB - PA = {d_{21}}, PC - PA = {d_{31}}. $

(2)

式中：

$ \left. \begin{array}{l} PA = \sqrt {{x^2} + {z^2} + {q^2}} , \\ PB = \sqrt {{{\left( {x - p} \right)}^2} + {z^2}} , \\ PC = \sqrt {{{\left( {x + p} \right)}^2} + {z^2}} . \end{array} \right\} $

(3)

即

$ \left. \begin{array}{l} \sqrt {{{\left( {x - p} \right)}^2} + {z^2}} - \sqrt {{x^2} + {z^2} + {q^2}} = {d_{21}}, \\ \sqrt {{{\left( {x + p} \right)}^2} + {z^2}} - \sqrt {{x^2} + {z^2} + {q^2}} = {d_{31}}. \end{array} \right\} $

(4)

声源P到原点的距离$ \rho = \sqrt {{x^2} + {z^2}} $.记中间变量$ k = \sqrt {{\rho ^2} + {q^2}} $, 则式(4)为

$ \left. \begin{array}{l} \sqrt {{{\left( {x - p} \right)}^2} + {z^2}} = {d_{21}} + k, \\ \sqrt {{{\left( {x + p} \right)}^2} + {z^2}} = {d_{31}} + k. \end{array} \right\} $

(5)

可得

$ k = (2{p^2} - 2{q^2} - d_{21}^2 - d_{31}^2)/[2({d_{21}} + {d_{31}})], $

(6)

即

$ \left. \begin{array}{l} {k^2} - {q^2} + {p^2} - 2xp = d_{21}^2 + {k^2} + 2k{d_{21}}, \\ {k^2} - {q^2} + {p^2} + 2xp = d_{31}^2 + {k^2} + 2k{d_{31}}. \end{array} \right\} $

(7)

记声源P相对原点的极角为θ, 有

$ \theta = \arccos \left( {x/\rho } \right). $

(8)

则声源的极坐标(ρ, θ)为

$ \left. {\rho = \sqrt {{k^2} - {q^2}} , \theta = \arccos \left( {x/\rho } \right).} \right\} $

(9)

式中：

$ x = ({d_{31}} - {d_{21}})({d_{31}} + {d_{21}} + 2k)/\left( {4p} \right). $

(10)

1.3 基于双目立体视觉的定位

在良好的光照条件下, 机器人视觉与其他传感器相比定位精度更高^[12], 在人脸检测算法的支持下, 可以实现多交互对象的同时定位.

仿人机器人视觉定位系统基于立体视觉的双目定位法^[13], 工作流程分为以下3步:1)摄像头获取左、右视角下图像, 通过图像采集卡对两路模拟信号进行预处理, 并将之转换为数字图像数据传输给上位机；2)使用Haar-like特征^[14]的级联分类器进行人脸检测, 调用OpenCV库进行图像预处理和人脸检测, 获得人脸在左、右眼图像中的相对位置；3)根据光学成像原理, 建立投影几何模型, 获得目标在场景和图像中的位置关系；由人脸像素位置确定目标在实际场景中的坐标.

光学成像是三维空间信息转换为二维平面信息的过程, 要借助视觉传感器进行视觉定位, 需要逆向建立二维信息与三维信息间的关系.使用图像中的二维坐标推导出物体的三维坐标, 即建立视觉定位系统的几何模型.

仿人机器人双目视觉系统的实物图和几何模型如图 3所示.所用相机的分辨率为640×480, 8 MB像素, 型号为KBT000658.其中左、右两眼的相机坐标系分别以光心为原点, 世界坐标系位于两相机正中间, 3个坐标系互相平行.

记左、右眼焦距分别为(f_x1，f_y1)、(f_x2，f_y2), 光心坐标分别为(c_u1, c_v1)、(c_u2, c_v2), 两眼间距为2B.设计的双目视觉系统使用平行光轴定位法, 左、右眼摄像头型号相同, 焦距相同, 可以近似认为

$ {f_{y1}} = {f_{x1}} = {f_{y2}} = {f_{x2}} = f. $

(11)

图像平面内的像素坐标(u, v)与世界坐标系的坐标(x_w, y_w, z_w)之间的转换关系如下：

$ \begin{array}{l} s\left[ {\begin{array}{*{20}{c}} u\\ v\\ 1 \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{f_x}}&0&{{u_0}}&0\\ 0&{{f_y}}&{{v_0}}&0\\ 0&0&1&0 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} \mathit{\boldsymbol{R}}&t\\ {{\mathit{\boldsymbol{0}}^{\rm{T}}}}&1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{x_{\rm{w}}}}\\ {{y_{\rm{w}}}}\\ {{z_{\rm{w}}}}\\ 1 \end{array}} \right] = \\ \;\;\;\;\;\;\;\;\;\;{\mathit{\boldsymbol{M}}_1}{\mathit{\boldsymbol{M}}_2}\left[ {\begin{array}{*{20}{c}} {{x_{\rm{w}}}}\\ {{y_{\rm{w}}}}\\ {{z_{\rm{w}}}}\\ 1 \end{array}} \right] = \mathit{\boldsymbol{M}}\left[ {\begin{array}{*{20}{c}} {{x_{\rm{w}}}}\\ {{y_{\rm{w}}}}\\ {{z_{\rm{w}}}}\\ 1 \end{array}} \right]. \end{array} $

(12)

式中：R为3×3的旋转矩阵；t为3×1的平移矢量，即相机外参数；0^T=[0, 0, 0].分别将左、右眼内外参数代入式(12), 经过整理可得

$ \left. \begin{array}{l} {z_{\rm{w}}} = \frac{{2Bf}}{{{c_{{\rm{u}}1}} - {c_{{\rm{u}}2}} - {u_1} + {u_2}}}, \\ {x_{\rm{w}}} = {z_{\rm{w}}}\frac{{{u_1} - {c_{{\rm{u}}1}}}}{f} + B, \\ {y_{\rm{w}}} = {z_{\rm{w}}}\frac{{{v_1} - {c_{{\rm{v}}1}}}}{f}. \end{array} \right\} $

(13)

式(13)为空间一点在像平面内的像素坐标和在世界坐标系的物理坐标间的对应关系.

2 多传感器定位系统信息融合 2.1 多传感器定位系统的融合策略分析

设计的多传感器定位系统中, 红外定位系统的覆盖范围是以传感器中心为原点, 半径为6 m的半圆形区域；听觉定位系统的覆盖范围是整个定位区域；视觉定位系统的定位范围如图 4所示, 是2个圆心角为38°的扇形区域的交集.

该系统的工作环境为5 m×6 m的封闭房间, 定位系统在该场景下的覆盖区域如图 5所示.图中, O点为多传感器系统的定位中心, DEGF表示整个房间.视觉定位系统的覆盖范围是三角形区域ABC, 对目标极角的定位误差E_θ和距离定位误差E_ρ均随着目标距离的增加而不断增加.红外定位系统的覆盖范围是矩形区域MNFG, 极角定位误差随着目标距离的增加而不断减小.听觉定位系统范围是整个房间DEGF, 距离定位误差随着目标距离的增加而不断增加, 极角定位误差保持不变.在距离定位误差方面, 随着目标距离的增加, 视觉定位的误差总体小于听觉定位.

根据多传感器定位系统的工作特点, 提出图 6所示的多层次定位融合方式.在三角形ABC区域内, 3种定位系统均正常工作, 对得到的3个定位数据进行融合.在矩形区域MNGF中, 只有红外定位系统和听觉定位系统工作, 对得到的2个定位数据进行融合.在矩形区域DENM中, 只有听觉定位系统可以正常工作, 此时直接输出听觉定位数据.

2.2 权重可变的加权融合算法

对3种传感器定位结果进行决策层融合^[15-17].由于需要融合的数据为3个, 数量较少, 且3个数据之间具有较强的功能互补性，采用加权平均法, 将3个数据融合为一个平均精度更高、容错性更强、更稳定的最终定位结果.

多传感器定位系统的最终结果即交互目标在水平面内相对于机器人的坐标以极坐标(ρ, θ)表示.记红外定位系统的定位数据为θ_r, 听觉系统的定位数据为(ρ_s, θ_s), 视觉系统定位数据为(ρ_v, θ_v).构建加权融合算法为

$ \theta = {m_{\rm{r}}}{\theta _{\rm{r}}} + {m_{\rm{s}}}{\theta _{\rm{s}}} + {m_{\rm{v}}}{\theta _{\rm{v}}}, \rho = {n_{\rm{s}}}{\rho _{\rm{s}}} + {n_{\rm{v}}}{\rho _{\rm{v}}}. $

(14)

式中：m_r、m_s、m_v分别为红外定位系统、听觉定位系统、视觉定位系统的角度加权系数, n_s、n_v分别为听觉定位系统、视觉定位系统的距离加权系数.加权融合算法的关键是根据先验知识确定这些系数值.

分别对这3种定位系统开展单独的定位实验, 统计出相应的定位精度, 红外定位系统的角度定位平均误差为15°；听觉定位系统的角度定位平均误差为3°, 距离定位平均误差为340 mm；视觉定位系统的角度定位平均误差为1°, 距离平均误差为100 mm.根据每种定位系统的定位精度, 确定相应的加权系数.使加权系数与定位精度成反比, 根据系数之和为1, 计算得到加权系数, 即

$ \left. \begin{array}{l} {m_{\rm{r}}} : {m_{\rm{s}}} : {m_{\rm{v}}} = \frac{1}{{{d_{\theta {\rm{r}}}}}} : \frac{1}{{{d_{\theta {\rm{s}}}}}} : \frac{1}{{{d_{\theta {\rm{v}}}}}}, \\ {n_{\rm{s}}} : {n_{\rm{v}}} = \frac{1}{{{d_{\rho {\rm{s}}}}}} : \frac{1}{{{d_{\rho {\rm{v}}}}}}, \\ {m_{\rm{r}}} + {m_{\rm{s}}} + {m_{\rm{v}}} = 1, {n_{\rm{s}}} + {n_{\rm{v}}} = 1. \end{array} \right\} $

(15)

式中：d_θr、d_θs、d_θv分别为红外定位系统、听觉定位系统、视觉定位系统的角度定位误差, d_ρs、d_ρv分别为听觉定位系统和视觉定位系统的距离定位误差.

将实验数据代入式(15), 得到系统的加权融合算式：

$ \theta = 0.05{\theta _{\rm{r}}} + 0.24{\theta _{\rm{s}}} + 0.71{\theta _{\rm{v}}}, \rho = 0.22{\rho _{\rm{s}}} + 0.78{\rho _{\rm{v}}}. $

(16)

式中：加权系数是不随交互目标位置和外界环境变化的常量.实际上, 这只适用于3种定位系统皆正常工作的情况.由于不同传感器本身的使用条件限制, 随着交互目标位置和外界环境的变化, 某个或某些定位系统可能会失效^[11-12].

根据融合策略, 在式(15)的基础上, 根据实验确定的各定位系统定位精度, 提出权重可变的加权融合算法, 以适应360°的定位范围和更复杂的外界环境.

当交互对象脱离视觉定位系统定位范围或处于昏暗环境(红外和听觉定位系统正常工作)时, θ_r、θ_s有数据, θ_v无数据, 此时

$ \theta = 0.17{\theta _{\rm{r}}} + 0.83{\theta _{\rm{s}}}, \rho = {\rho _{\rm{s}}}. $

(17)

当只有听觉定位系统工作时, 表明交互对象处于昏暗环境且脱离红外定位系统定位范围, 此时系统直接输出听觉定位系统的结果.

当交互者处于噪声环境时, 听觉定位系统停止工作.此时, 当θ_r、θ_v有数据时, 说明此时交互对象处于三角形ABC区域内且光线充足, 则

$ \theta = 0.06{\theta _{\rm{r}}} + 0.94{\theta _{\rm{v}}}, \rho = {\rho _{\rm{v}}}. $

(18)

当只有红外定位系统工作时, 直接输出θ_s, 此时系统无距离信息输出.

3 多传感器融合系统的定位实验 3.1 不同场景下的多传感器定位实验

为了验证多传感器定位系统可以适用于各种工作环境, 分别针对正常光照环境(见图 7(a))、昏暗环境(见图 7(b))及噪声环境(见图 7(c))，开展目标定位实验.实验结果如表 2所示.

从多场景定位实验结果可知, 设计的多传感器定位系统在不同场景下, 虽然个别定位系统会失效, 但其他定位系统依然正常工作, 稳定性较好.

3.2 多传感器定位结果融合实验

为了验证融合结果相比于单一定位结果的优势, 使用多传感器定位系统对环境中的任意两点进行目标定位, 对结果进行融合, 统计结果如表 3所示.

从实验结果可知, 融合后的定位精度比单一定位效果最好的视觉定位精度更高.作为3个定位系统的融合结果, 即使某一自定位系统的误差较高, 如红外定位系统, 定位误差为0~15°, 但总体融合结果依然保持一定精度, 表明定位的稳定性得到了很大的提高.

4 结语

本文针对仿人机器人SHFR-Ⅲ人机交互系统的目标定位需求, 建立适用于多定位场景的多传感器定位系统.该系统由红外定位系统、听觉定位系统和视觉定位系统融合而成.其中红外定位系统能够快速获得交互对象的粗略方位信息；听觉定位系统可以实现大场景下较准确的全方位目标定位；视觉定位系统具有较高的定位精度, 并且基于人脸检测的目标定位, 可以为仿人机器人的表情交互和视线交流进一步提供人脸位置信息.与单一定位系统相比, 本文设计的多传感器系统可以适应更复杂的定位场景, 提高了机器人人机交互系统的整体定位精度和定位可靠性.