CN101750060A - 利用感测元件的定位与检测的系统与方法 - Google Patents

Abstract

本发明涉及一种利用感测元件的定位与检测的系统与方法，利用彼此互补的多种传感器，进而达到定位与建地图。此外，在动态物件检测与追踪时，将多重传感器对物体的感测结果进行交叉比对，以检测出移动中的物体的位置并追踪此物体。

Description

利用感测元件的定位与检测的系统与方法

技术领域

本发明涉及一种利用感测元件的定位与检测的系统与方法，且特别是涉及一种利用可互补的多种感测元件的系统与方法，其定位载体、预测环境特征物件的位置、检测与追踪动态物件。

背景技术

室外定位系统，比如GPS(全球定位系统)，已广泛使用于车用导航系统，以在室外定出车辆或人的位置。但室内定位系统至今仍有其问题无法突破，室内定位系统的困难度在于(1)在室内时，电磁讯号易被遮蔽，无法接收到卫星讯号；(2)室内环境的变动性较室外环境大。

目前来说，室内定位的技术有两种类型，一种为外部定位系统，另一种为内部定位系统。外部定位系统例如，利用外部的传感器与机器人的接收器之间的相对关系去估测机器人在空间中的位置。由内部定位系统例如，在机器人上放传感器，将所扫描到的数据比较于其内建地图，进而估测机器人在空间中的位置。

外部定位系统的定位速度快，但需事先建好外部传感器，一但这些外部传感器被移动或被遮蔽，此系统将无法定位。外部定位系统如要用于大范围，所需的传感器数目与成本将增加。

内部定位系统的定位速度较慢但具扩充性，即使环境变动性大，只要仍有特征点可供定位，内部定位系统仍能定位。但其必需先内建室内环境地图，才能进行定位。考虑实时性的话，可以同时建地图与定位。然而此技术所建出的地图是静态的，但真实世界是动态的，所以，能达成动态环境下的定位与建地图的技术是必要的。

对于动态物件的估测通称为追踪。多个雷达可检测空中的移动物体，以判断是不是有敌机或飞弹来袭。目前此种检测与追踪技术已可应用在日常生活中，例如动态人物监控或其它安全监控应用。

发明内容

为达有效室内空间定位，和改善视觉传感器易受光线干扰而造成定位误差的问题，本发明目的是采用多重传感器间的互补性，以达成估测空间中物体状态的系统与方法，本发明利用电磁波感测元件(electro-magneticwave sensor)、机械波感测元件(mechanical wave sensor)或惯性感测元件(inertial sensor)，计算器率模型的感测融合(fusion)算法，定位出载体位置并估测环境特征物件在空间中的相对位置，以达到定位、建地图、动态物件检测与追踪。

本发明提出一种应用感测元件的定位与建地图的系统与方法，其妥善结合各种传感器的特性进而达到三度空间定位与建地图的功能。

本发明提出一种应用感测元件的动态物件检测与追踪的系统与方法，将多重传感器对物体感测的结果进行同构型(homogeneous)交叉比对或异质性(non-homogeneous)交叉比对，以检测出移动中的物体并追踪此物体。

本发明的一例提出一种感测系统，该系统包括：一载体；一多重传感器模块，配置于该载体上，该多重传感器模块感测多种彼此互补的特性，该多重传感器模块感测该载体以得到一载体信息，该多重传感器模块亦可感测一特征物件以得到一特征物件信息；一控制器，接收该多重传感器模块所传来的该载体信息与该特征物件信息；以及一显示单元，受控于该控制器，以提供一反应信号。该控制器执行下列的至少一者：该控制器将该载体定位于一地图内，且该控制器还将该特征物件加入于该地图中及更新在该地图中的该特征物件；以及根据该特征物件信息，该控制器预测该特征物件的一移动量，以决定该特征物件是否为已知的，并据以修正该地图及将该特征物件加入于该地图中。

本发明的另一例提供一种载体定位与建地图的感测方法，该方法包括：执行一第一感测步骤，以感测一载体以得到一载体信息；执行一第二感测步骤，以感测一特征物件以得到一特征物件信息，其中该第二感测步骤感测多种彼此互补的特性；分析该载体信息，以得到该载体的一位置与一状态，并将该载体定位于一地图内；分析该特征物件信息，以得到该特征物件的一位置与一状态；以及比较一地图与该特征物件的该位置与该状态，以将该特征物件的该位置与该状态加入于该地图中及更新在该地图中的该特征物件的该位置与该状态。

本发明的又另一例提供一种动态物件检测与追踪的感测方法，该方法包括：执行一第一感测步骤，以感测一动态物件以得到其第一移动量；执行一第二感测步骤，以感测该动态物件以得到其第二移动量，其中该第一感测步骤与该第二感测步骤彼此互补；分析该第一移动量与该第二移动量，以预估一载体与该动态物件间的一相对距离；决定该动态物件是否为已知的；若为已知，修正在一地图中的该动态物件的一状态，并检测与追踪；以及若为未知，将该动态物件及其状态加入于该地图，并检测与追踪。

附图说明

图1显示根据本发明实施例的利用感测元件的定位与建地图的系统。

图2显示利用视觉传感器来计算物件在空间中的位置的示意图。

图3显示双眼图像投影示意图。

图4A及图4B是依照本发明实施例的利用机械波传感器来检测载体与环境特征物件间的距离，以推测载体位置的示意图。

图5显示本发明实施例的定位与建静态地图的流程图。

图6显示定位与建静态地图的应用情境。

图7显示本发明实施例的对动态特征物件检测与追踪的流程图。

图8是对动态特征物件检测与追踪的应用情境。

图9显示本发明实施例的定位、建地图、移动物件的检测与追踪的应用情境。

【主要元件符号说明】

100：系统

110：多重传感器模块

120：载体

130：控制器

140：显示单元

110a、110b、110c：传感器

210、220：预处理

211、221：噪声移除

212、222：光线校正

213、223：图像矫正

IN1、IN2：图像信息

CM：相机参数矩阵

230、240：特征提取

250、260：图像描述

270：相似度比对

280：3D重建

510～570、710～770：步骤

610A～610C、810A～810C、910A～910D：特征物件

920：手

具体实施方式

为让本发明的上述内容能更明显易懂，下文特举实施例，并配合附图，作详细说明如下：

在本发明实施例中，能妥善结合各种传感器的特性进而达到三度空间定位与建地图的功能。此外，在动态物件检测与追踪时，利用多重传感器对物体的同构型交叉比对或异质性交叉比对，以检测出移动中的物体并追踪此物体。

图1显示根据本发明实施例的利用感测元件的定位与建地图的系统。如图1所示，此系统100包括：多重传感器模块110、载体120、控制器130与显示单元140。

多重传感器模块110可测量：外界环境或特征物件的电磁波信息(如图像或其它不可见的电磁波)、外界环境或特征物件的机械波信息(如声纳等经由机械震动所产生的震波)与载体120的力学信息(如位置、速度、加速度、角速度、角加速度)。多重传感器模块110会将所感测到的数据传输给控制器130。

在图1中，多重传感器模块110包括至少三种传感器110a、110b与110c，其中，这三种传感器所感测的特性不同，且可彼此互补。当然，多重传感器模块110可包括更多种的传感器，此皆在本发明的精神与范围内。

比如，传感器110a用以感测外界环境的电磁波信息，其可为：可见光视觉传感器、不可见光视觉传感器、电磁波传感器、红外线热传感器、红外线距离传感器等。传感器110b用以感测外界环境的机械波信息，其可为超音波传感器、超音波数组传感器或声纳传感器等。亦即，传感器110a与110b可感测外界环境中的环境特征物件与载体120间的距离。传感器110c用以感测载体120的力学信息，其可为加速规、陀螺仪、转速计数组或其它可测量物体力学的传感器等。传感器110a易受昏暗光线或无光源干扰，但较不易受物件的形状影响到测量结果。另一方面，传感器110b虽不会因为昏暗光线或无光源而影响测量，但会受物件的形状影响测量结果。亦即，传感器110a与110b彼此互补。

多重传感器模块110安装于载体120上。载体120可为车辆、机车、自行车、机器人、眼镜、手表、安全帽、其它可移动的物体等。

控制器130接收多重传感器模块110所感测到的载体力学信息与环境感测信息(至少包括外界环境中的环境特征物件与载体120间的距离)，以计算(预估测)出载体状态信息、预估出外界环境中的环境特征物件的特性(如其移动距离、移动方向等)与建立地图。更甚者，根据几何表达式，控制器130换算多重传感器模块110所传来的载体力学信息，以得知载体120的状态信息(比如，载体的惯性信息、姿态等)。此外，根据几何表达式，控制器130换算多重传感器模块110所传来的环境感测信息，可得知载体的移动信息或是环境特征物件的特性(如其位置等)。

控制器130以数字滤波器，如卡尔曼滤波器(Kalman Fiter)、粒子滤波器(Particle filter)、Rao-Blackwellised Particle filtetr或其它类贝氏滤波器推算出载体状态，以输出到显示单元140。

显示单元140连接于控制器130。显示单元140在控制器的命令下，产生对外界的互动反应。比如但不受限于，显示单元140所产生的互动反应可为语音信号、图像信号与提示信号的至少一者或其组合。语音信号包括：语音、音乐、预录的声音等。图像信号包括：图像、文字等。提示信号包括：颜色、色彩、明暗、闪烁、图形等。比如，当检测到别台车辆快要撞到应用本发明实施例的车辆时，显示单元可发出警示信息(声音等)，以警告驾驶。

在本发明实施例中，控制器130的状态估测可用数字滤波器实现，如下式所示，其中x_t为此刻的载体信息(包含位置(x，y，z)，载体姿态(θ，φ，ψ)与地标状态(x_n，y_n))，其中x_t-1为上一刻的载体信息，u_t为此时刻的载体移动感测信息(如加速度(a_x，a_y，a_z)、角速度(ω_x，ω_y，ω_z)等等信息)，z_t为此刻传感器所感测到的环境信息(如z_x，z_y，z_z))。

x_t＝f(x_t-1，u_t)+ε_t

z_t＝h(x_t)+δ_t

利用数字滤波器，可经由迭代的方式算出x_t，根据x_t，控制器130输出信息到其它装置(如显示单元140)。

以下解释传感器在测量空间中物件几何距离的概念与方法。

电磁波(可见光)：

利用视觉传感器，可通过图像来建立在空间中的物件位置与环境信息等。以图像感测为基础，定位真实世界的物件如图2与图3所示。图2显示利用视觉传感器来计算物件在空间中的位置的示意图。图3显示双眼图像投影示意图。

如图2所示，假设已知相机的内参数矩阵与外参数矩阵，而由内外参数矩阵可得相机参数矩阵CM(camera matrix)。对所提取的两张图像信息IN1与IN2(可由两个相机装置分别提取得，或利用一台相机在两个时间点取得)可选择性分别施以预处理210与220。预处理210与220分别包括：噪声移除(noise removal)211与221、光线校正(illumination correction)212与222；以及与图像矫正(image rectification)213与223。若欲施以图像矫正，则必须知道基础矩阵(fundamental matrix)。基础矩阵的计算方式如下所述。

在图像平面上的以相机坐标系表示的成像点可经由相机内参数矩阵转换，以得到此点以2维(2-D)图像平面成像点的表示方式，意即

$p_l={M_l}^{-1}{\stackrel{\‾}{p}}_l$

$p_r={M_r}^{-1}{\stackrel{\‾}{p}}_r$

其中，p_l与p_r分别为真实世界的物件点P于第一张与第二张图像的成像点，其以相机坐标系统表示；而与

分别为物件点P于第一张与第二张图像的成像点，其以2-D图像平面的坐标系统表示；M_l与Mr分别为第一台与第二台相机的内参数矩阵。

如图3所示，p_l的坐标为(x1，y1，z1)，而p_r的坐标为(xt，yt，zt)。在图3中，O₁与O_t为原点。

又，p_l与p_r可经由必要矩阵(essential matrix，E)转换，必要矩阵E为两个相机坐标系统之间的旋转与平移矩阵相乘后的结果，因此，

$p_r^T{\mathrm{Ep}}_l=0$

上式可改写成：

${\left({M_r}^{-1}{\stackrel{\‾}{p}}_r\right)}^{T}E\left({M_l}^{-1}{\stackrel{\‾}{p}}_l\right)=0$

再将M_l、Mr与E合并后可得

${{\stackrel{\‾}{p}}_r}^T\left({M_r}^{-T}{{\mathrm{EM}}_l}^{-1}\right){\stackrel{\‾}{p}}_l=0$

因此，令

F＝M_r ^-TRSM_l ^-1

可获得

与

之间的关系式如下

${{\stackrel{\‾}{p}}_r}^T=F{\stackrel{\‾}{p}}_l=0$

因此，通过输入两图像内的已知数组对应点，即可由上式求得基础矩阵。矫正后的两张图像会具有平行对应的核线(epipolar line)。

其后，对两张矫正后图像施以特征提取(feature extraction)230与240，以抽取出有意义的待比对特征点或区域。接着，利用图像描述(imagedescription)250与260来简化特征，使其成为特征描述子(feature descriptor)，的后，对两张图像特征施行相似度比对(stereo matching)270，找出两张图像中对应的特征描述子。

令p_l与p_r的坐标分别为

与

由于图像中存在噪声，因此可通过解开在3D重建(3D reconstruction)280中的最佳化问题

$\underset{P}{\min }\underset{j=l,r}{\mathrm{\Σ}}[{(\frac{m_1^{\mathrm{jT}}P}{m_3^{\mathrm{jT}}P}-u^j)}^2+{(\frac{m_2^{\mathrm{jT}}P}{m_3^{\mathrm{jT}}}-v^j)}^2]$

以估测空间中特征点P的世界坐标位置，其中，m₁ ^jT，m₂₁ ^jT，m₃ ^jT分别表示相机参数矩阵CM的第一，二，三列。如此，可得到载体与环境特征物件间的距离。

电磁波(能量)：

通常，在室内环境中有多种电器，而每种电器都会产生不同的电磁波。故而，可以利用电磁波能量而计算出物件(其会发出电磁波)与载体间的距离，进而得知物件的位置。首先，可以先用电磁波传感器来测量各类电磁波的讯号波形、频率与能量，可建出以下函数

$E\left(r\right)=K\frac{1}{r^2}\∝\frac{1}{r^2}$

其中E(r)为能量函数，K为常数或变数，r为载体与物件间的距离。经由电磁波的能量大小估算出物件与载体的距离，如此即可算出发出电磁波物件的位置。其细节请参考底下关于如何利用机械波来估计出载体与物件间的距离。

机械波(声纳)：

超音波属于距离信息(Range-only)传感器，也就是说，其仅能感测到物件在某个距离内，而无法得知物件的确切方位。通过分析机械波的能量大小，或分析机械波的发收时间差，即可估算出特征物件与载体的距离，而再利用载体移动前后的两笔距离信息以及载体的位置信息，即可得知特征物件或载体的所在位置。

图4A及图4B是依照本发明实施例的利用机械波传感器来检测载体与环境特征物件间的距离，以推测载体位置的示意图。

请先参照图4A，假设物件在k时刻的位置为(X1，Y1)，k+1时刻的位置为(X 2，Y2)，其中k时刻与k+1时刻相距Δt，而Δt为固定取样时间。假设机械波传感器在k时刻的位置为(a1，b1)，在k+1时刻的位置为(a2，b2)。根据机械波传感器在这两个位置所检测到机械波能量的大小，或发收时间差，即可推测出发出此机械波的环境特征物件与载体之间的距离r1及r2。

接着，以机械波传感器的位置(a1，b1)及(a2，b2)为中心，距离r1及r2为半径画两个圆，即可得到如图4A所示的圆A及圆B，其中圆A及圆B的方程式如下：

圆A： ${(X-a_1)}^2+{(Y-b_1)}^2=r_1^2---\left(1\right)$

圆B： ${(X-a_2)}^2+{(Y-b_2)}^2=r_2^2---\left(2\right)$

其中，圆A及圆B交点的联机为其根轴，而利用上述圆A及圆B的方程式即可求得此根轴的方程式：

$Y=\frac{-({2a}_2-{2a}_1)}{({2b}_2-{2b}_1)}X-\frac{(a_1^2+b_1^2+r_2^2-a_2^2-b_2^2-r_1^2)}{({2b}_2-{2b}_1)}---\left(3\right)$

接着，令圆A及圆B的交点(X_T，Y_T)的关系为：

Y_T＝mX_T+n                                   (4)

将式(4)代入圆A的方程式(1)：

${(X_T-a_1)}^2+{({\mathrm{mX}}_T+n-b_1)}^2=r_1^2$

$\⇒(m^2+1)X_T^2+(2\mathrm{mn}-2{\mathrm{mb}}_1-2a_1)X_T+{(n-b_1)}^2+a_1^2-r_1^2=0$

再令P＝m²+1，Q＝2mn-2mb₁-2a₁，以及

即可得：

$X_T=\frac{-Q\±\sqrt{Q^2-4\mathrm{PR}}}{2P}$

$Y_T=\frac{m(-Q\±\sqrt{Q^2-4\mathrm{PR}})}{2P}+n---\left(5\right)$

通过上述推导可得两组解(X_T，Y_T)。参考所测得电磁波的幅角，即可决定哪一组解才是特征物件所在的位置。

机械波收发元件属于距离信息(Range-only)的传感器，也就是说，机械波收发元件仅能用以感测载体在某个距离内，而无法得知载体的确切方位。机械波收发元件以机械震动来产生震波，其比如为超音波、超音波数组或声纳等。为了用机械波测量载体的位置，在本发明实施例中，亦利用载体移动前后的两笔机械波距离信息与载体的位置信息，将特征物件的位置简化为两个圆解共点，其求解的方式与前述电磁波传感器相类似，故在此不再赘述。

力学感测元件(IMU)：

力学感测元件用于测量动态(直线运动、旋转运动等)的物体的状态。通过演算机制，对测量到的动态信号加以解析，就可实时得到移动物体在三维空间中的各项数据，包含位置，速度、加速度、角度、角速度、以及角加速度。

现在说明力学感测元件的感测原理。在初始状态后，可根据陀螺仪量得载体的三轴角速度信息，经由四元素算法(integration of quaternion)算出载体的三轴姿态角，再经由坐标转换矩阵的转换，以得到载体在世界坐标下的三轴速度。在转换的过程中，可引入加速度传感器的信息，通过对时间的一次积分以及重力分量的消除，以得到载体的速度信息。再通过滤波器得到载体在三维空间下的三轴移动量信息的预测值。

如果仅利用此种感测信息，由数学运算积分所造成的累积误差，以及由传感器取样造成的误差，将导致估测值与实际值越差越远，并且随时间增加而发散。所以必须搭配其它种类的传感器，消除飘移累积误差。之后，再以机率型滤波器估测物体的位置。

或者说，力学感测元件在感测时，所用的运算包括：四元素(integrationof quaternion)运算、方向余弦运算(direction cosine convert to Euler angle)、重力分量抽离(separating gravity)运算、加速度积分(integration ofacceleration)运算、速度积分(integration of velocity)运算、坐标转换(coordinate transformation)运算、数据关联(data association)运算、延伸式卡尔曼滤波器校正(extended-Kalman filter correction)运算等。

现请参考图5，以说明在本发明实施例中如何进行定位与建静态地图。图6显示定位与建静态地图的应用情境。在图6中，假设载体120处于动态(移动及/或转动等)，而在外界环境中有多个静态特征物件610A～610C。在此，定位是指对载体的定位。

如图5所示，在步骤510中，提取第一传感器信息，此第一传感器信息用于感测载体120的状态。比如，提取传感器110c所检测到的载体加速度与速度信息u_t＝[a_x，t a_y，t a_z，t ω_x，t ω_y，t ω_z，t]^T。

接着，在步骤520中，根据第一传感器信息来预测载体状态。其详细方式如下。假设欲估测载体在空间中的位置为[x_t，y_t，z_t，θ_t，φ_t，ψ_t]，其中，

x_t＝g(x_t-1，u_t)+ε_t

z_t＝h(x_t)+δ_t

假设运动模型(Motion Model)如下：

X_t＝g(X_t-1，U_t)+ε_t

其中载体状态为

X_t＝[X_G，t V_x，t A_x，t Y_G，t V_y，t A_y，t Z_G，t V_z，t A_z，t e_0，t e_1，t e_2，t e_3，t]^T，[X_G，t Y_G，t Z_G，t]^T为载体在世界坐标中的绝对位置，[V_x，t V_y，t V_z，t]^T为载体在载体坐标中的速度，[A_x，t A_y，t A_z，t]^T为载体在载体坐标中的加速度，[e_0，t e_1，t e_2，t e_3，t]^T为载体在载体坐标中的四元素(quaternion)，U_t＝[a_x，t a_y，t a_z，t ω_x，t ω_y，t ω_z，t]^T为载体在载体坐标中的加速度与角速度。

要算出载体于t时在世界坐标中的绝对位置B_t，需要利用载体于t-1时在世界坐标中的绝对位置、利用载体上的加速规与陀螺仪所得到加速度和角速度的积分信息，且利用四元素把载体坐标信息经由载体坐标转换成世界坐标，而且，以上步骤在运动模型中完成。矩阵运算如下：

载体状态的运动模型

$\left[\begin{array}{c}{X}_{G,t}\\ V_{x,t}\\ A_{x,t}\\ Y_{G,t}\\ V_{y,t}\\ A_{y,t}\\ Z_{G,t}\\ V_{z,t}\\ A_{z,t}\\ e_{0,t}\\ e_{1,t}\\ e_{2,t}\\ e_{3,t}\end{array}\right]=$

$\left[\begin{array}{ccccccccccccc}1& R_{11}t& 0.5R_{11}{t}^2& 0& R_{12}t& 0.5R_{12}{t}^2& 0& R_{13}t& 0.5R_{13}{t}^2& 0& 0& 0& 0\\ 0& 1& 0& 0& {\mathrm{\ω}}_{z,t}t& 0& 0& -{\mathrm{\ω}}_{y,t}t& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& R_{21}t& 0.5R_{21}{t}^2& 1& R_{22}t& 0.5R_{22}{t}^2& 0& R_{23}t& 0.5R_{23}{t}^2& 0& 0& 0& 0\\ 0& -{\mathrm{\ω}}_{z,t}t& 0& 0& 1& 0& 0& {\mathrm{\ω}}_{x,t}t& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& R_{31}t& 0.5R_{31}{t}^2& 0& R_{32}t& 0.5R_{32}{t}^2& 1& R_{33}t& 0.5R_{33}{t}^2& 0& 0& 0& 0\\ 0& {\mathrm{\ω}}_{y,t}t& 0& 0& -{\mathrm{\ω}}_{x,t}t& 0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& -0.5{\mathrm{\ω}}_{z,t}t& -0.5{\mathrm{\ω}}_{y,t}t& -0.5{\mathrm{\ω}}_{z,t}\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0.5{\mathrm{\ω}}_{x,t}t& 1& 0.5{\mathrm{\ω}}_{y,t}t& -0.5{\mathrm{\ω}}_{z,t}t\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0.5{\mathrm{\ω}}_{y,t}t& -0.5{\mathrm{\ω}}_{z,t}t& 1& 0.5{\mathrm{\ω}}_{x,t}t\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& -0.5{\mathrm{\ω}}_{z,t}t& 0.5{\mathrm{\ω}}_{y,t}t& 0.5{\mathrm{\ω}}_{x,t}t& 1\end{array}\right]\left[\begin{array}{c}{X}_{G,t-1}\\ V_{x,t-1}\\ A_{x,t-1}\\ Y_{G,t-1}\\ V_{y,t-1}\\ A_{y,t-1}\\ Z_{G,t-1}\\ V_{z,t-1}\\ A_{z,t-1}\\ e_{0,t-1}\\ e_{1,t-1}\\ e_{2,t-1}\\ e_{3,t-1}\end{array}\right]$

$+\left[\begin{array}{c}0\\ (a_{x,t}-g_{x,t})t\\ (a_{x,t}-g_{x,t})\\ 0\\ (a_{y,t}-g_{y,t})t\\ (a_{y,t}-g_{y,t})\\ 0\\ (a_{z,t}-g_{z,t})t\\ (a_{z,t}-g_{z,t})\\ 0\\ 0\\ 0\\ 0\end{array}\right]+{\mathrm{\ϵ}}_t$

地图状态的运动模型

${\left[\begin{array}{c}{m}_{x,t}^i\\ m_{y,t}^i\\ m_{z,t}^i\end{array}\right]}_t=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]{\left[\begin{array}{c}{m}_{x,t}^i\\ m_{y,t}^i\\ m_{z,t}^i\end{array}\right]}_{t-1}$

其中，g_x，t为重力加速度在载体坐标轴x轴的分量，g_y，t为重力加速度在载体坐标轴y轴的分量，g_z，t为重力加速度在载体坐标轴z轴的分量，ε_t为传感器所产生的噪声，R₁₁～R₃₃为方向余弦矩阵(Direction Cosine Matrix)内的参数。

$\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\\ z^{\′}\end{array}\right]=\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\left[\begin{array}{ccc}{e}_0^2+e_1^2-e_2^2-e_3^2& 2(e_1{e}_2+e_0{e}_3)& 2(e_1{e}_3-e_0{e}_2)\\ 2(e_1{e}_2-e_0{e}_3)& e_0^2-e_1^2+e_2^2-e_3^2& 2(e_2{e}_3+e_0{e}_1)\\ 2(e_1{e}_3-e_0{e}_2)& 2(e_2{e}_3-e_0{e}_1)& e_0^2-e_1^2-e_2^2+e_3^2\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]$

经由以上的运动模型可算出载体在空间中的位置[X_G，t Y_G，t Z_G，t]^T、载体在载体坐标中的加速度[A_x，t A_y，t A_z，t]^T、载体在载体坐标中的速度[V_x，t V_y，t V_z，t]^T与载体的四元素[x_0，t e_1，t e_2，t e_3，t]^T。所算出的载体状态会包含加速规与陀螺仪传感器的噪声，故需要修正其误差。所以采用另外一个传感器当做感测模型(Sensor Model)，目的在于修正加速规与陀螺仪估测出来的物体状态。

感测模型如下：

Z_t＝h(X_t)+δ_t

若传感器为视觉传感器，感测模型如下：

$\left[\begin{array}{c}{z}_{x,t}\\ z_{y,t}\\ z_{z,t}\end{array}\right]=h_{c,t}\left(x_t\right)+{\mathrm{\δ}}_{c,t}$

$=\left[\begin{array}{c}{m}_{x,t}^i-X_{G,t}\\ m_{y,t}^i-Y_{G,t}\\ m_{z,t}^i-Z_{G,t}\end{array}\right]+{\mathrm{\δ}}_{c,t}$

其中，[m_x，t ⁱ m_y，t ⁱ m_z，t ⁱ]^T为第i个内建地图的空间坐标，δ_c，t为视觉传感器的噪声。

若传感器为声纳或电磁波传感器，模型如下：

$z_{r,t}=h_{s,t}\left(x_t\right)+{\mathrm{\δ}}_{s,t}$

$=\sqrt{{(m_{x,t}^i-X_{G,t})}^2+{(m_{y,t}^i-Y_{G,t})}^2+{(m_{z,t}^i-Z_{G,t})}^2}+{\mathrm{\δ}}_{s,t}$

其中δ_s，t为声纳传感器或电磁波的噪声。

接着，如步骤530所示，提取第二传感器信息，此第二传感器信息用于感测外在(室内)环境中的静态特征物件。第二传感器信息比如可利用传感器110a与110b的至少一者，或其二者所感测到的信息。也就是说，在步骤530中，利用电磁波类型传感器及/或机械波类型传感器来感测出静态特征物件610A～610C与载体间的距离。

接着，如步骤540所示，比较第二传感器信息与内建地图中现有的特征物件的信息，以决定所感测到的静态特征物件是否已出现在现有的内建地图中。如果是的话，则根据第二传感器信息来修正载体的位置、修正载体的状态、与修正已内建的地图，如步骤550所示。

步骤550的详细说明如下。由以上感测模型可算出载体在空间中的位置，进而修正运动模型所估测的载体状态，以估测载体状态，其中可估测的载体状态包含载体在空间中的位置[X_G，t Y_G，t Z_G，t]^T与四元素[e_0，t e_1，t e_2，t e_3，t]^T，若需要算载体相对于X轴的角度θ、载体相对于Y轴的角度ψ与载体相对于Z轴的角度φ可由四元素换算而得，其公式如下：

$\left\{\begin{array}{c}\sin \mathrm{\θ}=2(e_0{e}_2-e_3{e}_1)\\ \tan \mathrm{\ψ}=\frac{2(e_0{e}_3+e_1{e}_2)}{e_0^2+e_1^2-e_2^2-e_3^2}\\ \tan \mathrm{\φ}=\frac{2(e_0{e}_1+e_2{e}_3)}{e_0^2-e_1^2-e_2^2+e_3^2}\end{array}\right.$

以上的运动模型与感测模型可代入数字滤波器，即可估测载体位置。

如果载体完全不转动仅移动时，则所估测的载体状态仅为x_t＝[X_G，t Y_G，t Z_G，t]^T；若当载体完全不移动仅转动时，所估测的载体状态仅为x_t＝[e_0，t e_1，t e_2，t e_3，t]^T或经由转换后的x_t＝[θ ψ φ]^T，以上两种例子皆在本实施例范围。

如果步骤540的判断结果为否，则根据第二传感器信息，以增加新的地图特征于内建地图，如步骤560所示。也就是说，在步骤560中，将所感测到的静态特征物件当成新的地图特征，以加入于现有的内建地图中。比如，比较后结果发现特征物件610B并未出现于现有的内建地图中，则可将特征物件610B的位置、状态等加入于内建地图。

接下来，则说明本发明实施例如何应用于的动态特征物件的检测与追踪。图7显示本发明实施例的对动态特征物件检测与追踪的流程图，图8是对动态特征物件检测与追踪的应用情境。在此，假设载体是不动的，而在环境内(比如室内)有多个移动的特征物件810A～810C。

如图7所示，在步骤710中，根据第一传感器信息来预测动态特征物件的移动量。在此，可利用传感器110a及/或110b来感测至少一个动态特征物件的移动量。其方式如下。

追踪动态特征物件的运动模型如下：

O_t＝g(O_t，V_t)+ε_T，t

其中

[o_x，t ^l o_y，t ¹ o_x，t ¹ v_x，t ¹ v_y，t ¹ v_z，t ¹]^T为第一个动态特征物件在空间中的位置与速度，[o_x，t ^N o_y，t ^N o_x，t ^N v_x，t ^N v_y，t ^N v_z，t ^N]^T为第N个动态特征物件(N为正整数)在空间中的位置与速度，而

为物体在空间中的加速度，ε_T，t为动态特征物件的移动量估测误差。第n个(n＝1～N，n为正整数)运动模型矩阵如下：

$\left[\begin{array}{c}{o}_{x,t}^n\\ o_{y,t}^n\\ o_{z,t}^n\\ v_{x,t}^n\\ v_{y,t}^n\\ v_{z,t}^n\end{array}\right]=\left[\begin{array}{cccccc}1& 0& 0& t& 0& 0\\ 0& 1& 0& 0& t& 0\\ 0& 0& 1& 0& 0& t\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{o}_{y,t-1}^n\\ o_{y,t-1}^n\\ o_{y,t-1}^n\\ v_{x,t-1}^n\\ v_{y,t-1}^n\\ v_{z,t-1}^n\end{array}\right]+\left[\begin{array}{ccc}{0.5t}^2& 0& 0\\ 0& 0.5t^2& 0\\ 0& 0& 0.5t^2\\ t& 0& 0\\ 0& t& 0\\ 0& 0& t\end{array}\right]\left[\begin{array}{c}{a}_{x,t}^n\\ a_{y,t}^n\\ a_{z,t}^n\end{array}\right]+{\mathrm{\ϵ}}_{T,t}$

经由此运动模型可估测出动态特征物件在空间中的位置。需注意的是在此把动态特征物件的移动视为加速度为常数且有误差的信息，依可粗略估出物件的移动位置。此外，还可利用感测模型以修正动态特征物件的真正位置。

接着，如步骤720所示，提取第二传感器信息，其亦用于测量环境特征物件，比如感测其移动量。接着，如步骤730所示，提取第三传感器信息，其亦用于测量环境特征物件，比如感测其移动量。

接着，如步骤740所示，比较目前所接收到的第二～第三传感器信息，以决定被感测的动态特征物件是否为已知的。如果是，则根据目前所接收到的第二至第三传感器信息来修正环境特征物件的状态与位置，并检测与追踪，如步骤750所示。如果在步骤740的决定为否，代表被感测的动态特征物件乃是新的动态特征物件，如此，则加入新的动态特征物件的位置与其状态于地图中，并检测与追踪，如步骤760所示。

在步骤740中，进行比较的方法有两种，一种为异质比对(homogeneous)，一种为同质比对(non-homogeneous)。异质比对方式为，当物件仅具有一种特性时，利用电磁波传感器与红外线热传感器比较出传感器信息的差异性，进而追踪仅具有一种特性的物件。同质比对方式则为，当物件具有两种特性时，利用视觉传感器与超音波传感器比较传感器信息间的异同，进而追踪此物件。

在图7中所用的感测模型如下：

Z_t＝T(X_t)+δ_T，t

其中δ_T，t为传感器的噪声。

若传感器为视觉传感器或其它可量得物体在空间中位置的传感器，则感测模型如下：

$\left[\begin{array}{c}{z}_{x,t}\\ z_{y,t}\\ z_{z,t}\end{array}\right]=T_c\left(X_t\right)+{\mathrm{\δ}}_{T,c,t}$

$=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{o}_{x,t}^n\\ o_{y,t}^n\\ o_{z,t}^n\\ v_{x,t}^n\\ v_{y,t}^n\\ v_{z,t}^n\end{array}\right]+{\mathrm{\δ}}_{T,c,t}$

若传感器为超音波传感器或电磁波传感器或其它距离信息(range-only)传感器，则感测模型如下：

$z_{r,t}=T_s\left(X_t\right)+{\mathrm{\δ}}_{T,s,t}$

$=\sqrt{{\left(o_{x,t}^n\right)}^2+{\left(o_{y,t}^n\right)}^2+{\left(o_{z,t}^n\right)}^2}+{\mathrm{\δ}}_{T,s,t}$

此外，在步骤750与760中，感测模型可估出物件在空间中的位置，而由运动模型所估测出的物件位置可经由感测模型修正，以得到物件在空间中的较精确位置与较精确速度，达到检测与追踪物体的目的。

此外，本发明实施例还可结合图5的定位与建地图，及图7的移动物件检测与追踪，以达到定位、建地图、移动物件检测与追踪，其应用情境如图9所示。在图9中，假设手920令载体120为动态(只移动不转动、不移动只转动、或又移动又转动)，特征物件910A～910C为静态，而特征物件910D为动态。由上述说明可知，如何对载体120检测其状态(比如，预估其姿态(posture))、如何建地图、如何检测与追踪动态的特征物件910D，故其细节不再重述。在此，若载体120为动态，则检测与追踪的算法是基于移动中的载体，所以需考虑载体的位置与不确定性、并预测载体的位置(类似图5的做法)。

综上所述可知，在本发明实施例中，利用可互补的多种传感器，来精准定位、追踪、检测、预估载体状态(姿态)。故而，本发明可应用于比如但不受限于，飞机的惯性导航系统、数字相机的防手震系统、车辆速度检测系统、车辆避撞系统、电视游乐器(如Wii)的把手的三维姿态检测等、手机定位、室内地图产生器。此外，本发明更可应用于室内伴侣机器人，其可监控环境里的老人、小孩等。本发明还可应用于车辆，其可监控环境里的车辆，避免车祸。本发明亦可应用于可移动的机器人，其可检测移动中的人类，进而追踪此人并提供服务。

综上所述，虽然本发明已以实施例公开如上，然其并非用以限定本发明。本发明所属技术领域中具有通常知识者，在不脱离本发明的精神和范围内，当可作各种的更动与润饰。因此，本发明的保护范围当视所附的权利要求所界定的为准。

Claims (15)

1.一种感测系统，其特征在于，该系统包括：

一载体；

一多重传感器模块，配置于该载体上，该多重传感器模块感测多种彼此互补的特性，该多重传感器模块感测该载体以得到一载体信息，该多重传感器模块还感测一特征物件以得到一特征物件信息；

一控制器，接收该多重传感器模块所传来的该载体信息与该特征物件信息；以及

一显示单元，受控于该控制器，以提供一反应信号；

其中，该控制器执行下列的至少一者：

该控制器将该载体定位于一地图内，且该控制器还将该特征物件加入于该地图中及更新在该地图中的该特征物件；以及

根据该特征物件信息，该控制器预测该特征物件的一移动量，以决定该特征物件是否为已知的，并据以修正该地图及将该特征物件加入于该地图中。

2.如权利要求1所述的系统，其特征在于，该多重传感器模块包括：可见光视觉传感器、不可见光视觉传感器、电磁波传感器、红外线热感应器与红外线距离传感器的至少一者或其组合。

3.如权利要求1所述的系统，其特征在于，该多重传感器模块包括：超音波感应器、超音波数组感应器与声纳感应器的至少一者或其组合。

4.如权利要求1所述的系统，其特征在于，该多重传感器模块包括：加速规、陀螺仪与转速计数组的至少一者或其组合。

5.如权利要求1所述的系统，其特征在于，该显示单元所提供的该反应信号包括：一语音信号、一图像信号与一提示信号的至少一者或其组合。

6.如权利要求1所述的系统，其特征在于，该载体包括：车辆、机车、自行车、机器人、眼镜、手表、安全帽或其它可移动物体的至少一者或其组合。

7.如权利要求1所述的系统，其特征在于，该控制器：

根据该载体信息，预测该载体的一状态；

比较被视为静态的该特征物件的该特征物件信息与该地图，以决定该特征物件是否在该地图内；

如果该特征物件未显示在该地图内，则将该特征物件的一状态与一位置加入于该地图中；以及

如果该特征物件显示在该地图内，则修正该地图、修正该载体的一位置、修正该载体的该状态。

8.如权利要求1所述的系统，其特征在于，该控制器：

比较被视为动态的该特征物件的该特征物件信息与该地图，以决定该特征物件是否为已知的；

如果该特征物件为已知的，则修正该特征物件在该地图中的一位置与一状态；以及

如果该特征物件不是已知的，则将该特征物件的该位置与该状态加入于该地图中。

9.一种载体定位与建地图的感测方法，其特征在于，该方法包括：

执行一第一感测步骤，以感测一载体以得到一载体信息；

执行一第二感测步骤，以感测一特征物件以得到一特征物件信息，其中该第二感测步骤感测多种彼此互补的特性；

分析该载体信息，以得到该载体的一位置与一状态，并将该载体定位于一地图内；

分析该特征物件信息，以得到该特征物件的一位置与一状态；以及

比较一地图与该特征物件的该位置与该状态，以将该特征物件的该位置与该状态加入于该地图中及更新在该地图中的该特征物件的该位置与该状态。

10.如权利要求9所述的方法，其特征在于，该第一感测步骤包括：

感测该载体，以得到该载体的一速度、一加速度、一角速度、一角加速度的至少一者。

11.如权利要求10所述的方法，其特征在于，该第二感测步骤包括：

感测该特征物件，以得到该特征物件与该载体间的一相对距离关系。

12.如权利要求10所述的方法，其特征在于，还包括：

比较该载体的该位置与该特征物件的该位置，以产生一情境反应。

13.一种动态物件检测与追踪的感测方法，其特征在于，该方法包括：

执行一第一感测步骤，以感测一动态物件以得到其第一移动量；

执行一第二感测步骤，以感测该动态物件以得到其第二移动量，其中该第一感测步骤与该第二感测步骤彼此互补；

分析该第一移动量与该第二移动量，以预估一载体与该动态物件间的一相对距离；

决定该动态物件是否为已知的；

若为已知，修正在一地图中的该动态物件的一状态，并检测与追踪；以及

若为未知，将该动态物件及其状态加入于该地图，并检测与追踪。

14.如权利要求13所述的方法，其特征在于，还包括：

分析该载体与该动态物件间的该相对距离，以产生一情境反应。

15.如权利要求13所述的方法，其特征在于，如果该载体为动态，该方法还包括：

感测该载体，以得到该载体的一位置、一速度、一加速度、一角速度、一角加速度的至少一者。

Images