CN102109348B - 定位载体、估测载体姿态与建地图的系统与方法 - Google Patents

Abstract

一种定位载体、估测载体姿态与建地图的系统，包括：一惯性感测元件，测量该载体的一移动状态与一旋转状态；一视觉感测元件，配置在该载体的一表面上以拍摄该载体所在的一室内环境内的至少一环境特征点；以及一控制器，控制该惯性感测元件与该视觉感测元件，接收该惯性感测元件的一感测结果与该视觉感测元件的一感测结果以估测该载体的一姿态信息、该载体的一位置信息与一速度信息，并建构含有该环境特征点的一地图。该控制器根据该惯性感测元件与该视觉感测元件之一的一修正后感测结果而估测，该控制器令该惯性感测元件与该视觉感测元件中的另一个感测并且据以修正该载体的该姿态信息、该载体的该位置信息、该速度信息与该地图。

Description

定位载体、估测载体姿态与建地图的系统与方法

技术领域

本发明是有关于一种应用感测元件以定位载体、估测载体姿态与建地图的系统与方法，且特别是有关于一种应用可互补的多种感测元件的系统与方法，其能定位载体、估测环境特征点的位置并建构地图。

背景技术

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

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

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

内部定位系统的定位速度较慢但具扩充性，即使环境变动性大，只要特征点可供定位，内部定位系统仍能定位。但其必需先内建室内环境地图，才能进行定位。

发明内容

本发明实施例提出一种结合惯性感测元件(inertial measurement unit，IMU)与视觉感测元件，以定位移动载体并计算出环境特征对象在三度空间的相对位置(相对于移动载体)，达到同步定位、姿态估测、与建构环境地图。

本发明的一实施例提出一种定位载体、估测载体姿态与建地图的系统，包括：一惯性感测元件，测量该载体的一移动状态与一旋转状态；一视觉感测元件，配置在该载体的一表面上以拍摄该载体所在的一室内环境内的至少一环境特征点；以及一控制器，控制该惯性感测元件与该视觉感测元件，接收该惯性感测元件的一感测结果与该视觉感测元件的一感测结果以估测该载体的一姿态信息、该载体的一位置信息与一速度信息，并建构含有该环境特征点的一地图。该控制器根据该惯性感测元件与该视觉感测元件之一的一修正后感测结果而估测，该控制器令该惯性感测元件与该视觉感测元件中的另一个感测并且据以修正该载体的该姿态信息、该载体的该位置信息、该速度信息与该地图。

本发明的另一实施例提出一种定位载体、估测载体姿态与建地图的方法，包括：利用一惯性感测元件以测量该载体的一移动状态与一旋转状态；利用一视觉感测元件以拍摄该载体所在的一室内环境内的至少一环境特征点；以及根据该惯性感测元件与该视觉感测元件之一的一修正后感测结果而估测，令该惯性感测元件与该视觉感测元件中的另一个感测并且据以修正该载体的一姿态信息、该载体的一位置信息、一速度信息与一地图。

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

附图说明

图1显示根据本发明第一实施例的系统示意图。

图2显示本第一实施例如何结合惯性感测元件与视觉感测元件来估测载体的状态。

图3显示根据本发明第一实施例的流程图。

附图标记说明

100：载体            110：惯性感测元件

120：视觉感测元件    130：控制器

140：显示单元        210～230：环境特征点

241～243：载体的姿态与位置

251：实际画面

210A～230A：环境特征点的实际位置

252：估测画面

210B～230B：环境特征点的估测位置

253：实际画面

210C～230C：环境特征点的实际位置

E1～E3：估测误差

310～370：步骤

具体实施方式

第一实施例：惯性感测元件+视觉感测元件(单眼相机)

图1显示根据本发明第一实施例的系统示意图。如图1所示，根据本发明第一实施例的系统包括：载体100、惯性感测元件110、视觉感测元件120、控制器130与显示单元140。

载体100是可以移动的，其比如但不受限为可进行室内定位的机器人、个人携带型导航装置(Personal Navigation System)、游戏娱乐机、车辆、机车、自行车、眼镜、手表、安全帽或其它可移动物体。

惯性感测元件110可为下列之一或其任意组合：加速度计(accelerometer)、陀螺仪(gyroscope)、转速计数组或其它可量测物体力学参数的传感器。加速度计可测量载体的移动状态(如加速度信息)，而陀螺仪则可测量载体的旋转状态(如角速度信息)。根据惯性感测元件110的感测结果，控制器130可以估测出移动载体的载体姿态、速度、位置等。控制器130可将惯性感测元件110所感测出的加速度、角速度等参数进行积分，以计算出载体100的速度信息、姿态信息与位置信息。然而，经过长时间积分后，由惯性感测元件110所计算出的速度信息与位置信息的累积误差(如速度误差、位移误差、姿态误差等)无法单靠惯性感测元件110的感测结果来消除。此外，惯性感测元件110不会因为环境的光线昏暗或无光源而影响测量结果的准确性。惯性感测元件110的感测速度快。在控制器130的控制下，惯性感测元件110进行感测并将其回传给控制器。

在第一实施例中，视觉感测元件120为单眼相机，且配置在载体100的表面上，以拍摄室内的环境特征点。根据视觉感测元件120的感测结果，控制器130可测量出环境特征点的移动，估测载体的移动/旋转状态。视觉感测元件易受光线昏暗或无光源的干扰，而且，在环境特征点的成像情况不明显下(例如，位于白墙前面的白色物体)，单靠视觉感测元件的感测结果，控制器不易/无法定位出载体与环境特征的相对位置。视觉感测元件的感测操作较慢且控制器130的相对应运算较复杂。如果没有惯性感测元件的感测结果的辅助，则单靠视觉感测元件的感测结果，控制器会花耗大量计算于计算出载体的3D状态。如果载体处在高动态状态，则此计算容易发散，使得系统可能会出错。故而，如果有惯性感测元件的辅助，控制器在计算载体的3D状态之时，不会只靠视觉感测元件的感测结果，所以控制器的计算量减少；即使载体处在高动态状态，计算仍不易发散，系统出错的可能性大为降低。

故而，在本实施例中，结合惯性感测元件与视觉感测元件，以计算出机率模型的感测融合，来定位出载体在三度空间的位置，及环境特征点在空间中的相对位置(相对于载体)，达到空间定位、姿态估测、建置地图的目的。在此，地图是由环境特征点所组成，而且，在本实施例中，可在载体移动/旋转的过程中，实时建构出地图。

控制器130连接至惯性感测元件110、视觉感测元件120与显示单元。虽然在图1中，控制器130似乎位于载体100之外，但在其它实施例中，控制器130也可位于载体内部。控制器会控制惯性感测元件110与视觉感测元件120的操作，接收这些感测元件的感测结果并进行计算，以估测载体姿态与位置，并建构含有环境特征点的地图。

显示单元140连接在控制器130。在控制器130的控制下，显示单元140产生可与外界互动的反应。比如，显示单元140可提醒使用者的反应为：影音表达语音、音乐、预录声音。此外，显示单元也可显示：图像、文字、颜色、明暗、闪烁、图形等。显示单元140所提供的影音表达语音包括：语音信号、影像信号与提示信号的至少一者或其组合。

现请参考图2，其显示本第一实施例如何结合惯性感测元件与视觉感测元件来估测载体的状态。图2所示为视觉同时定位与建地图(VSLAM，VisionSimultaneous Localization and Mapping)。在图2中，210～230代表在环境中的三个环境特征点。请注意，虽然在此以3个环境特征点为例，但本实施例并不受限于此。

在时刻t-1时，载体的姿态与位置(t-1|t-1)如参考附图标记241所示；而且，时刻t-1时，视觉感测元件会拍摄实际画面251，其中，实际画面251显示出环境特征点的实际位置210A～230A。

因为载体的移动/旋转，在时刻t时，根据惯性感测元件的感测结果，控制器估测载体的未修正姿态与位置(t|t-1)如参考附图标记242所示。

根据载体的未修正姿态与位置(t|t-1)242，控制器估测出画面252，其包括对环境特征点的估测位置210B～230B。

在时刻t时，在控制器的控制下，视觉感测元件拍摄实际画面253并回传给控制器，实际画面253包括环境特征点的实际位置210C～230C。控制器对比环境特征点的估测位置210B～230B与实际位置210C～230C，以计算出估测误差E1～E3，此误差可能包括控制器在量测环境特征点时的误差及控制器估测载体的姿态与位置的误差。

在本实施例中，控制器可逐一修正所有环境特征点的位置误差，或是同时修正所有环境特征点的位置误差。在本实施例中，修正误差的方式可有多种，比如可根据误差E1～E3的平均值进行修正。

根据误差E1～E3，控制器可以推算出对载体的姿态与位置的修正量，并据以修正载体的姿态与位置的状态，如参考附图标记243所示。当控制器欲命令惯性感测元件进行量测时，控制器会先根据修正后姿态与位置信息(t|t)243来进行估测，如此可以修正因惯性感测元件的积分结果所导致的累积误差。

如上述，在本实施例中，当要估测载体的姿态、位置与建构地图时，要进行估测(由控制器进行)-量测(由其中一种感测元件进行)-修正(由控制器进行)。当控制器在进行估测时，根据控制器对另一种感测元件的修正后感测结果而进行估测，之后，控制器会令其中一种感测元件进行数据感测并且依据其中一种感测元件的感测数据来进行修正。

现请参考图3，其显示根据本发明第一实施例的流程图。如步骤310所示，在命令惯性感测元件进行量测之前，控制器会先估测载体的3D姿态、速度、位置。

接着，如步骤320所示，在控制器的控制下，惯性感测元件测量载体的移动状态(如加速度信息)与旋转状态(如角速度信息)，并将感测结果回传给控制器。

之后，如步骤330所示，根据惯性感测元件的感测结果，控制器会修正载体的3D姿态、速度、位置等。

另一方面，如步骤340所示，在命令视觉感测元件进行量测之前，控制器会先进行估测(如估测载体的3D姿态、速度、位置)。本实施例中，当控制器已得到惯性感测元件的修正后结果，则控制器会依据惯性感测元件的修正后结果来进行步骤340中的估测。

接着，如步骤350所示，在控制器的控制下，视觉感测元件进行测量(即，视觉感测元件会拍摄画面)，并将感测结果回传给控制器。

之后，如步骤360所示，根据视觉感测元件的感测结果，控制器会修正载体的3D姿态、速度、位置等。详细地说，如步骤361所示，根据视觉感测元件所拍摄的实际画面，控制器比较环境特征点的估测位置与实际位置间的差异，以计算出误差。

接着，如步骤362所示，依据所算出的误差，控制器修正对所有环境特征点的估测位置。在步骤362中，控制器可逐一修正所有环境特征点的位置误差，或是同时修正所有环境特征点的位置误差。

如步骤363所示，根据上述步骤362所计算出的误差，控制器修正对载体预测位置，比如，修正所估测的载体位置、载体姿态等信息。

如步骤370所示，经由估测-测量-修正结果，可以对载体定位，估测载体的3D姿态，并建构环境地图(含有环境特征点)。

如果在控制器欲进行步骤310的估测时，控制器已得到视觉感测元件的修正后结果(即步骤370已完成)，则控制器会依据视觉感测元件的修正后结果来进行步骤310的估测。相似地，如果在控制器欲进行步骤340的估测时，控制器已得到惯性感测元件的修正后结果(即步骤330已完成)，则控制器会依据惯觉感测元件的修正后结果来进行步骤340的估测。

在本实施例中，如果惯性感测元件与视觉感测元件的操作速度不同步的话(通常，惯性感测元件的操作速度较快而视觉感测元件的操作速度较慢)，则在图3中，步骤310～330的操作不同于步骤340～370。

底下将详述本实施例的详细做法。利用惯性感测元件来定位的状态定义如下：

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}{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与载体的四元数[e_0，t  e_1，t  e_2，t  e_3，t]^T。

所算出的载体状态会包含加速规与陀螺仪传感器的噪声，故需要修正其误差。所以，在本实施例，以视觉感测元件当做感测模型(Sensor Model)，以修正加速规与陀螺仪估测出来的物体状态。单眼相机(mono camera)的成本较低，但需要透过时间差来计算出相机与特征点间的相对距离。

以下说明惯性感测元件(IMU)搭配单眼相机的实施方式。结合惯性感测元件(IMU)搭配单眼相机的系统状态(System State)表示如下。

$\hat{X}={\left[\begin{array}{ccccc}{{\hat{x}}_v}^T& {{\hat{y}}_0}^T& {{\hat{y}}_1}^T& ...& {{\hat{y}}_{n-1}}^T\end{array}\right]}^T$

系统状态包含相机状态和所有环境特征点的状态。其数学表示为一维向量，向量的大小几乎和环境特征点的个数成正比。

相机状态(Camera State)

相机状态包含相机的位置向量、旋转指向向量、直线移动速度向量、旋转角速度向量。相机状态的数学形式为13×1的一维向量。

${\hat{x}}_v=\left[\begin{array}{cccc}\hat{r}& \hat{q}& \hat{v}& \widehat{\mathrm{\ω}}\end{array}\right]{=\left[\begin{array}{cccc}\left[\begin{array}{ccc}x& y& z\end{array}\right]& \left[\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right]& \left[\begin{array}{ccc}{v}_x& v_y& v_z\end{array}\right]& \left[\begin{array}{ccc}{\mathrm{\ω}}_x& {\mathrm{\ω}}_y& {\mathrm{\ω}}_z\end{array}\right]\end{array}\right]}^T$

为相机的位置向量；

为相机的旋转朝相四元数向量；

为相机的直线移动速度向量；以及

为相机的旋转角速度向量。

特征状态(Feature State)

特征状态纪录环境特征点在空间中的坐标，一般以3×1的一维向量表示。

${\hat{y}}_i=\left[\begin{array}{ccc}{x}_i& y_i& z_i\end{array}\right]$

运动模型(Motion Model)

运动模型定义每张画面之间的系统状态改变，其为二阶运动方程式，表示如下。

${\hat{x}}_{v,t|t-1}={\left[\begin{array}{cccc}\hat{r}& \hat{q}& \hat{v}& \widehat{\mathrm{\ω}}\end{array}\right]}_{t|t-1}^T$

$={\left[\begin{array}{cccc}{\hat{r}}_{t-1|t-1}+({\hat{v}}_{t-1|t-1}+0.5{\hat{a}}_{t-1|t-1}\mathrm{\Δt})\mathrm{\Δt}& {\hat{q}}_{t-1|t-1}+Q({\widehat{\mathrm{\ω}}}_{t-1|t-1}+0.5{\widehat{\mathrm{\α}}}_{t-1|t-1}\mathrm{\Δt})\mathrm{\Δt}& {\hat{v}}_{t-1|t-1}+0.5{\hat{a}}_{t-1|t-1}\mathrm{\Δt}& {\widehat{\mathrm{\ω}}}_{t-1|t-1}+0.5{\widehat{\mathrm{\α}}}_{t-1|t-1}\mathrm{\Δt}\end{array}\right]}^T$ 其中，

为直线加速度；

为旋转加速度；Δt为状态更新时间间隔；以及Q(x)为尤拉角转四元数的转换函式。

观测模型(Observation Model)

如果采用不同的观测模型且观测向量的维度越高，则卡尔曼增益(KalmanGain)的维度也随之线性增加。

在第一实施例中，观测模型是定义如何从相机的姿态位置与空间特征点的坐标推算出特征点在相机中的成像位置。

$\left[\begin{array}{c}{u}_i\\ v_i\end{array}\right]=\left[\begin{array}{c}{u}_0-\frac{k_{u}f\frac{x_i^r}{z_i^r}}{\sqrt{1+2K({\left(k_{u}f\frac{x_i^r}{z_i^r}\right)}^2+{\left(k_{v}f\frac{y_i^r}{z_i^r}\right)}^2)}}\\ v_0-\frac{k_{v}f\frac{y_i^r}{z_i^r}}{\sqrt{1+2K({\left(k_{u}f\frac{x_i^r}{z_i^r}\right)}^2+{\left(k_{v}f\frac{y_i^r}{z_i^r}\right)}^2)}}\end{array}\right]$

其中，(u₀，v₀)为画面的中心坐标(以左上角坐标为(0，0))；(k_u，k_v)分别为像素的长度倒数与宽度倒数；K为相机广角扭曲系数；以及(x，y，z)_i ^r为环境特征点

相对于相机的坐标。

系统共变量矩阵(System Covariance Matrix)

系统共变量矩阵定义出，在系统状态向量中，每个元素彼此之间的变异关系，也包含每个元素的自我不确定性。虽然系统共变量矩阵的大小和环境特征点的个数成平方比，但在对一个环境特征点进行估测-测量-修正(Predict-Measure-Update)时，只针对相机和目前正被估测-测量-修正的单一特征点的共变量次矩阵(Covariance Submatrix)进行运算。系统共变量矩阵表示如下：

$P={\left[\begin{array}{ccccc}{P}_{\mathrm{xx}}& P_{{\mathrm{xy}}_0}& P_{{\mathrm{xy}}_1}& ...& P_{{\mathrm{xy}}_{n-1}}\\ P_{y_{0}x}& P_{y_0{y}_0}& P_{y_0{y}_1}& ...& ...\\ P_{y_{1}x}& P_{y_1{y}_0}& P_{y_1{y}_1}& ...& ...\\ ...& ...& ...& ...& ...\\ P_{y_{n-1}x}& P_{y_{n-1}{y}_0}& ...& ...& P_{y_{n-1}{y}_{n-1}}\end{array}\right]}_{(n+1)\×(n+1)}$

其中，各参数代表共存的n个特征点，n可以为任意正整数。

第二实施例：惯性感测元件+视觉感测元件(立体视觉相机)

在第二实施例中，视觉感测元件120为立体视觉相机(stereo visioncamera)，且其同样配置在载体100的表面上，以拍摄室内的环境特征点。立体视觉相机的成本较高，但在同一时刻就可算出相机与环境间的景深信息。

立体视觉相机的观测模型

原则上，第二实施例的模型类似/相同于第一实施例，不过，第二实施例的立体视觉相机的观测模型不同于第一实施例的单眼相机的观测模型。

在第二实施例中，立体视觉相机的观测模型定义出如何从相机的姿态位置与环境特征点的坐标推算出环境特征点在左右相机中的成像位置。在此假设左右相机内部参数相同，且其内部参数彼此为平移。

$\left[\begin{array}{c}{u}_i^L\\ v_i^L\\ u_i^R\\ v_i^R\end{array}\right]=\left[\begin{array}{c}{u}_0^L-\frac{k_{u}f\frac{x_i^{\mathrm{rL}}}{z_i^{\mathrm{rL}}}}{\sqrt{1+2K({\left(k_{u}f\frac{x_i^{\mathrm{rL}}}{z_i^{\mathrm{rL}}}\right)}^2+{\left(k_{v}f\frac{y_i^{\mathrm{rL}}}{z_i^{\mathrm{rL}}}\right)}^2)}}\\ v_0^L-\frac{k_{v}f\frac{y_i^{\mathrm{rL}}}{z_i^{\mathrm{rL}}}}{\sqrt{1+2K\left({\left(k_{u}f\frac{x_i^{\mathrm{rL}}}{z_i^{\mathrm{rL}}}\right)}^2{\left(k_{v}f\frac{y_i^{\mathrm{rL}}}{z_i^{\mathrm{rL}}}\right)}^2\right)}}\\ u_0^R-\frac{k_{u}f\frac{x_i^{\mathrm{rR}}}{z_i^{\mathrm{rR}}}}{\sqrt{1+2K({\left(k_{u}f\frac{x_i^{\mathrm{rR}}}{z_i^{\mathrm{rR}}}\right)}^2+{\left(k_{v}f\frac{y_i^{\mathrm{rR}}}{z_i^{\mathrm{rR}}}\right)}^2)}}\\ v_0^R-\frac{k_{v}f\frac{y_i^{\mathrm{rR}}}{z_i^{\mathrm{rR}}}}{\sqrt{1+2K({\left(k_{u}f\frac{x_i^{\mathrm{rR}}}{z_i^{\mathrm{rR}}}\right)}^2+{\left(k_{v}f\frac{y_i^{\mathrm{rR}}}{z_i^{\mathrm{rR}}}\right)}^2)}}\end{array}\right]$

其中，(u₀ ^L，v₀ ^L，u₀ ^R，v₀ ^R)分别为左右相机的画面中心坐标(以左上角坐标为(0，0))；(k_u，k_v)分别为像素的长度倒数与宽度倒数，在立体视觉中假设左右相机的这两参数相同；K为相机广角扭曲系数，在立体视觉中假设左右相机的此参数相同；以及(x，y，z)_i ^rL与(x，y，z)_i ^rL分别为环境特征点

相对于左右相机的坐标，其关系如下：

${\left[\begin{array}{c}x\\ y\\ z\end{array}\right]}_i^{\mathrm{rL}}={\left[\begin{array}{c}x\\ y\\ z\end{array}\right]}_i^{\mathrm{rR}}+T_{R2L}$

其中，T_R2L为右相机坐标系平移至左相机坐标系的平移向量。

综上，在第一实施例中，视觉感测元件更可量测出环境特征点在相机中投影成像的画面坐标(u，v)。在第二实施例中，视觉感测元件的量测状态有两种可能定义方式：将第一实施例中的单一单眼相机扩张成两个单眼相机，因此可得到左右两组(u，v)；或者将立体视觉相机当成单一传感器，量测环境特征点在空间中的(x，y，z)坐标。

在本发明其它实施例中，视觉感测元件可为一个或复数个单眼像机或一个或多个全像镜头相机等。

综上所述，在本发明上述两个实施例与其它可能实施例中，可融合多重感测元件的感测结果的估测(载体姿态、速度、角速度、位置、环境特征点的位置与相对距离)。利用惯性感测元件(陀螺仪与加速规)与视觉感测元件(单眼相机或是立体视觉相机)，以定位出移动载体在空间中的相对位置与环境特征点相对于移动载体的位置，达到状态估测的目的。可利用两种传感器的互补性，大幅减少运算复杂度，以扩增对载体的动态侦测范围，达成3D移动载体的定位(甚至可达到高速立即寻址)。

此外，本发明实施例也可结合在室外定位系统，以补偿室外定位系统的不足。

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

Claims (9)

1.一种定位载体、估测载体姿态与建地图的系统，包括：

一惯性感测元件，测量该载体的一移动状态与一旋转状态；

一视觉感测元件，配置在该载体的一表面上以拍摄该载体所在的一室内环境内的至少一环境特征点；以及

一控制器，控制该惯性感测元件与该视觉感测元件，接收该惯性感测元件的一感测结果与该视觉感测元件的一感测结果以估测该载体的一姿态信息、该载体的一位置信息与一速度信息，并建构含有该环境特征点的一地图；

该控制器根据该惯性感测元件与该视觉感测元件之一的一修正后感测结果而估测，之后，该控制器令该惯性感测元件与该视觉感测元件中的另一个感测并且据以修正该载体的该姿态信息、该载体的该位置信息、该速度信息与该地图；

其中，该控制器在命令该惯性感测元件进行量测之前，估测该载体的该姿态信息、该位置信息与该速度信息；该控制器如果在该控制器估测之前，已计算出该视觉感测元件的该修正后感测结果，则该控制器依据该视觉感测元件的该修正后感测结果进行估测；该惯性感测元件在该控制器的控制下，测量该载体的该移动状态与该旋转状态，并将该感测结果回传给该控制器；该控制器根据该惯性感测元件的该感测结果，修正该姿态信息、该位置信息与该速度信息；

该控制器在命令该视觉感测元件进行测量之前，先估测该载体的该姿态信息、该位置信息与该速度信息；该控制器如果在该控制器估测之前，已计算出该惯性感测元件的该修正后感测结果，则该控制器依据该惯性感测元件的该修正后结果进行估测；该视觉感测元件在该控制器的控制下，拍摄该载体所在室内的该环境特征点画面，并将该感测结果回传给该控制器；该控制器根据该视觉感测元件的该感测结果，修正该姿态信息、该位置信息与该速度信息；

其中，该控制器根据该视觉感测元件所拍摄的一实际画面，比较该环境特征点的一估测位置与一实际位置间的差异，以计算出一误差；该控制器依据所算出的该误差，修正对该环境特征点的该估测位置，并更新该地图；该控制器根据该误差，修正该姿态信息、该位置信息与该速度信息。

2.根据权利要求1所述的系统，其中，该移动状态至少包括一加速度信息，与该旋转状态至少包括一角速度信息。

3.根据权利要求1所述的系统，其中，如果该室内环境具有复数环境特征点，则该控制器逐一修正该些环境特征点的各别估测位置，或是该控制器同时修正该些环境特征点的各别估测位置。

4.根据权利要求1所述的系统，其中，该视觉感测元件包括下列的至少一者或其任意组合：一个或复数个单眼相机，一个或复数个全像镜头相机。

5.根据权利要求1所述的系统，其中，该惯性感测元件包括可为下列的至少一者或其任意组合：一加速规、一陀螺仪、一转速计数组。

6.根据权利要求1所述的系统，更包括：

一显示单元，连接在该控制器，在该控制器的控制下，该显示单元产生一外界互动反应。

7.根据权利要求6所述的系统，其中，该显示单元显示一影音表达语音、一音乐、一预录声音。

8.一种定位载体、估测载体姿态与建地图的方法，包括：

利用一惯性感测元件以测量该载体的一移动状态与一旋转状态；

利用一视觉感测元件以拍摄该载体所在的一室内环境内的至少一环境特征点；以及

根据该惯性感测元件与该视觉感测元件之一的一修正后感测结果而估测，令该惯性感测元件与该视觉感测元件中的另一个感测并且据以修正该载体的一姿态信息、该载体的一位置信息、一速度信息与一地图；

其中，在该惯性感测元件量测之前，估测该载体的该姿态信息、该位置信息与该速度信息；如果在该估测步骤之前，已计算出该视觉感测元件的该修正后感测结果，则依据该视觉感测元件的该修正后感测结果进行估测；回传该惯性感测元件的感测结果至一控制器，以及根据该惯性感测元件的该感测结果，修正该姿态信息、该位置信息与该速度信息；

在该视觉感测元件测量之前，先估测该载体的该姿态信息、该位置信息与该速度信息；如果在该估测步骤之前，已计算出该惯性感测元件的该修正后感测结果，则依据该惯性感测元件的该修正后结果进行估测；该视觉感测元件拍摄该载体所在室内的该环境特征点画面，并回传该感测结果；以及根据该视觉感测元件的该感测结果，修正该姿态信息、该位置信息与该速度信息；

其中，根据该视觉感测元件所拍摄的一实际画面，比较该环境特征点的一估测位置与一实际位置间的差异，以计算出一误差；依据所算出的该误差，修正对该环境特征点的该估测位置，并更新该地图；以及根据该误差，修正该姿态信息、该位置信息与该速度信息。

9.根据权利要求8所述的方法，其中，如果该室内环境具有复数环境特征点，则逐一修正该些环境特征点的各别估测位置，或是同时修正该些环境特征点的各别估测位置。

Images