CN1308505A - 运动跟踪系统 - Google Patents

Abstract

本发明涉及物体运动的跟踪,它通过获得和物体的运动相关的两个类型的测量来实现的,一种类型的测量包括声音测量。根据两种类型的测量中的一种测量,例如根据由惯性测量(140)单元获得的惯性测量更新物体位置或方向的估算。然后根据两种测量中的另一种例如根据由声学距离测量单元(110)获得的声学距离测量更新所述的估算。本发明的特征还在于对于选择的参考装置确定距离测量,所述的参考装置被固定在所述物体的环境中,例如被固定在超声信标阵列(120)中的超声信标(122)。

Description

运动跟踪系统

本发明涉及运动跟踪领域。

可以使用各种测量方式进行运动跟踪，其中包括惯性和声学测量方式，用于确定物体的位置和方向。

惯性运动跟踪基于测量围绕一组基本上正交的轴线的线加速度和角速度。在一种方法中，多个和被跟踪物体相连的旋转的回转仪响应被跟踪物体的旋转而产生和其转轴的转速成正比的力。这些力被测量并用于估算物体的角速度。可以使用基于微加工的振动元件和光波导的装置代替回转仪。

加速计产生和由线加速度产生的力成正比的信号。在惯性跟踪系统中，角速度和加速度信号被综合用于确定线速度、线位移和转动的总的角度。

由于由回转装置产生的信号含有噪声，所以综合处理导致噪声成分的积累，这一般被称为“漂移”。小型的和成本低的回转装置一般具有较大的误差。对于一个静止的物体，漂移速率可以高达每秒几度，对于转动的物体，每转90度可发生几度的漂移。方向估算中的误差也影响位置估算，因为估算的物体的方向在进行综合之前被用于把加速度的度量转换为固定的参考框架。在这种转换中的不精确性可以引起导致水平加速度测量偏差的重力。

用于校正漂移的一种方法是附加的检测器，例如倾角计和指南针，用于间歇地或连续地校正综合的惯性测量的漂移。例如，1997年7月8日公开的Eric M.Foxlin的美国专利5645077中披露了一种这样的方法。该专利在此列为参考。

用于运动跟踪的另一种方法使用声波测量在物体上的一个或几个点和在环境中的固定的参考点之间的距离。在一种被称为“outside-in”的结构中，一组在物体的固定点上的声发射器发射被在环境中的固定参考点的一组麦克风接收的脉冲。从发射器到麦克风经过的时间和发射器与麦克风之间的距离的估算值(即距离)成正比。从发射器到接收麦克风的范围估算值被用于通过三角测量法确定发射器的位置。在物体上的多个发射器的位置被组合，用于估计物体的方向。

其它的测量方法，例如对物体上的光源的光学跟踪的方法也可以用于跟踪物体的运动。

一般地说，在一个方面中，本发明是一种用于跟踪物体的运动的方法，其包括以下步骤：获得和物体的运动相关的两种类型的测量，其中一种类型的测量包括声学测量，根据两种测量中的一种测量例如根据惯性测量更新物体的方向或位置的估算，并且根据两种测量中的另一种测量例如根据声学测距更新所述估算。

在另一个方面，一般地说，本发明是一种用于跟踪物体运动的方法，包括以下步骤：选择一组参考装置中的一个装置，向所选择的参考装置发送控制信号，例如借助于发送无线控制信号，接收来自参考装置的距离测量信号，接收关于对选择的参考装置的距离的测量，并使用所接收的距离测量更新物体的位置估算和方向估算。所述的方法还可以包括根据距离测量信号的行程的时间确定距离测量。

本发明的优点包括提供一种6个自由度的跟踪能力，其可以在基本上不受限制的空间内应用，在所述空间内安装有可以扩展的超声波信标星座。惯性测量方法提供运动的平滑的和响应的检测，而超声波测量方法提供实时的误差校正，例如由系统的惯性跟踪元件的漂移引起的误差的校正。可以使用通常具有相当大的漂移的小的和廉价的惯性检测器，此时仍然可以提供具有有限的漂移的整个系统。体积小和重量轻的惯性检测器非常适用于虚拟的或增强真实性的显示系统的头戴式跟踪装置。通过使用超声波测量校正漂移，可以不需要可能对外部因素例如磁场的改变等敏感的漂移校正测量。超声波信标的星座在每个信标独立地工作因而不需要在信标当中布线时可以容易地被扩展。在任何时刻，跟踪装置只依赖于使用少量的超声波信标，借以允许跟踪装置工作的空间具有不规则的区域，例如在大楼中的多个房间。

本发明的另一个优点在于，通过使用“inside-out”结构，使得在声学距离测量中没有在声波发出之后由于物体的运动而引起的等待时间。

本发明的另一个优点在于，通过使用惯性测量，即使在例如由于信标的闭塞而不能进行声学测量时，也能继续进行跟踪。此时一旦惯性跟踪的漂移被校正，可以再继续进行声学测量。

在另一个优点中，本发明提供非全方位(Line-of-sight)冗余度，借以可以阻断在发送器和检测器之间的一个或几个路径，同时仍然能够进行物体的跟踪。

本发明的其它特点和优点从下面的说明和权利要求中可以清楚地看出。

图1表示跟踪装置和用于跟踪所述装置的声学信标的星座；

图2表示跟踪装置处理器的元件；

图3表示组合的惯性和声学跟踪方法；

图4表示惯性测量单元(IMU)；

图5表示超声距离测量单元(URM)和超声信标；

图6表示用于跟踪装置处理器中和惯性测量单元以及超声测量单元连接的输入/输出接口；

图7a表示导航和物体参考框架；

图7b表示相互跟踪装置；

图8表示惯性跟踪器的信号流程图；

图9表示超声距离测量子系统的信号流图；

图10是包括惯性跟踪器和卡尔曼预报器以及更新器元件的跟踪装置的信号流图；

图11是卡尔曼预报器的信号流图；

图12是卡尔曼更新器的信号流图；

图13是跟踪程序的信号流图；

图14a表示关于第一被跟踪物体的第二物体的跟踪；

图14b表示多个装置的相互跟踪；

图15表示头戴式显示系统；

图16表示用于电视的摄像机跟踪系统；以及

图17表示在汽车上的物体的跟踪。

参看图1，一直进行其位置和方向估算的跟踪装置100在一个大的房间内自由运动。例如，跟踪装置100可以被固定在操作者的头上的头戴式显示器(HUD)上，当操作者运动并且改变其头的方向时，跟踪装置100通过房间而运动，并且改变其方向。跟踪装置100包括处理器130，其和用于提供关于线加速度和转动速度的惯性测量的惯性测量单元(IMU)140相连。当跟踪装置100在房间内运动时，处理器130使用惯性测量确定跟踪装置100的运动。

处理器130还和用于接收从超声信标阵列120即信标“星座”发出的超声信号的3个超声距离测量单元(URM)110耦联。超声信标阵列120包括独立的超声信标122，它们被设置在环境中的固定位置，例如被设置在一个大的房间的天花板上，例如呈间隔为2英尺的规则的网格图案。处理器130使用来自特定的超声信标122的信号和已知的所述信标的三维位置，估算到这些信标的距离，借以检测跟踪装置100的运动。每个超声信标122响应从跟踪装置100发出的红外指令信号112发送超声脉冲114。具体地说，在跟踪装置100上的每个URM110对所有的超声信标122发送红外(IR)信号。这些IR信号包括地址信息，使得只有一个信标或者只有少量的信标识别为其发送的IR信号，并且响应所述信号。响应IR信号，被寻址的信标立即发送超声脉冲，该脉冲然后被一个或几个URM110接收。处理器130得知被寻址的信标立即响应IR指令时，便通过测量从发出IR指令到检测到超声脉冲时间延迟确定行程的时间。超声脉冲的行程时间被用于估算信标的距离，然后利用该距离更新跟踪装置100的位置和方向。

基于惯性测量和超声信号的两种测量方法具有局限性，单独依靠哪一种测量方法取得的结果都不会比利用两种方法的组合所得的结果精确。跟踪装置100组合两种测量方式的测量结果。跟踪装置100在得到两种测量方式的测量结果时，或者经过一个时间延迟，便调整其位置和方向(即6个自由度“6-DOF”)的估算，以便反映两种测量方式的测量结果。为此，处理器130控制一个扩展的卡尔曼滤波器(EKF)，其被用于组合所述的两种测量，并保持不断的估算跟踪装置100的位置和方向，并且一直保持在那些测量结算中不确定性的估算。

参看图2，处理器130包括中央处理单元(CPU)200，例如Intel80486微处理器，程序存储器220，例如只读存储器(ROM)，和工作存储器230，例如动态随机存取存储器(RAM)。CPU200还和用于提供与IMU140、URM110的连接的输入输出接口210耦联。输入输出接口210包括用于提供和IMU140、URM110的数字接口的数字逻辑。

IMU140提供一个对惯性测量结果编码而获得的串行数据流201。输入输出接口210把这个串行数据转换成为一种并行的形式212，以便传递给CPU200。每个URM110接收串行信号211，该信号用于驱动IR发光二极管510向超声信标122(图1)发射IR控制信号。输入输出接口210接收来自CPU的用于识别一个或几个超声信标的地址信息，并向每个URM110提供串行信号，然后把所述串行信号加于发射的IR信号上(例如通过幅值调制)。同一个串行信号被提供给所有的URM110，它们同时发送相同的IR信号。每个URM110依次向输入输出接口210提供超声脉冲到达的逻辑信号202。输入输出接口210包括用于确定来自信标的超声脉冲的行程时间并用于确定到信标的距离估算的计时器。这些距离估算被提供给CPU200。

一种跟踪算法被存储在程序存储器220中并由CPU200执行，用于把从输入输出接口210获得的测量值转换为位置和方向估算值。CPU200还和固定数据存储器240相连，所述存储器例如包括超声信标的位置的预定图形和URM110的麦克风的位置的信息。处理器130还包括通信接口260，用于使CPU200和其它装置相连，例如显示装置280，其根据跟踪装置100的位置和方向修改其显示。

通过参看图3可以理解系统的操作，图3是图1所示的房间的二维平面图(从上看)。一列空白的圆圈和箭头310a-e表示在一个时间段序列的每个时间段跟踪装置100的实际位置和方向。根据以前的测量结果，并根据在第一时间段的惯性测量结果，实心的圆圈和箭头312a表示在第一时间段由跟踪装置100估算的跟踪装置的位置和方向。在下一个时间段，跟踪装置100运动到位置310b，根据新的惯性测量结果，跟踪装置100将其位置估算更新到312b。对于下一个时间段重复上述处理，得到实际的位置310c和估算的位置312c。

在到达位置310b之后，跟踪装置100发送一个IR指令，如虚线320所示，其具有一个超声传感器122的地址。在收到IR指令之后(基本上没有延迟)，超声传感器122发送超声脉冲，如波324所示。波324经过一定时间到达跟踪装置100，此时跟踪装置的实际位置是330。当波326到达跟踪装置时，根据到达的时间，跟踪装置100估算其在位置332。

在下一个时间段，跟踪装置100首先根据惯性测量估算其位置312d。使用关于超声传感器122的位置和位置332间隔的距离信息以及测量的超声波的行程时间，跟踪装置100计算一个精确的位置估算312d’。在实际位置310e和估算位置312e使用惯性测量重复所述的处理。

一般地说，在每一个时间段可以使用惯性测量和超声测量，虽然可以较少地使用超声测量。在每一个时间段，位置和方向(姿势)被更新。通过使用相互隔开一定距离的多个麦克风，超声脉冲可以提供关于位置和方向的信息。

参看图4，惯性测量单元(IMU)140包括3个角速度检测器(例如微加工的振动转动检测器或小的旋转回转仪)420a-c和3个线加速度检测器410a-c。这些检测器沿着在跟踪装置100的参考框架中保持固定的3个正交的轴线排列。每个加速度检测器提供基本上和沿着相应的轴线的加速度成正比的信号，每个角速度检测器提供基本上和围绕相应的轴线旋转的速度成正比的信号。

当惯性测量单元140的方向改变时，信号例如加速度信号则相应于在房间的固定(导航)参考框架内方向的改变。惯性测量单元140还包括信号接口430，其接收来自6个加速度计和加速度检测器的每个的信号411，并发送一个串行数据流413，其多路传输加速度和加速度信号的数字表示。如同下面要讨论的，加速度和角速度信号是不精确的，其具有附加的偏移和比例误差。这些比例和偏移误差可能和装置的运动有关。

参见图5，每个超声测量单元110包括红外(IR)发光二极管(LED)510，其由IR信号发生器512驱动。信号发生器512接收来自输入输出接口210(图2)的串行信号211，并驱动IR LED 510向一个或几个超声信标122发送所述信号。要测量其距离的超声信标的地址被在串行信号211中编码。每个超声信标122包括IR检测器540，如果在超声距离测量单元110和所述超声信标之间具有足够短的无障碍路径，则所述IR检测器接收所述IR检测信号，然后所述信号被IR信号译码器542译码。这个译码的信号包括由超声距离测量单元发送的地址信息。控制电路560接收译码的IR信号，并且确定是否那个超声信标是真正要被寻址的，如果是，便通知脉冲发生器552对超声传感器550提供一个信号，使其产生超声脉冲。所述脉冲通过空气到达超声距离测量单元110，其中麦克风520接收超声脉冲并向脉冲检测器522传递相应的电信号，脉冲检测器522产生一个表示脉冲的到达的逻辑信号。所述脉冲检测信号被传递到输入输出接口210(图2)。如同下面所要讨论的，利用行程时间测量距离是不十分精确的。其中的误差源包括脉冲检测中的定时误差，例如由于空气温度或气流以及从超声信标发出的超声波沿不同的传播方向的不一致性而引起的声音传播速度的改变。

输入/输出接口210包括用于实施图6所示的逻辑功能的电路(即可编程的逻辑阵列)。IMU数据缓冲器630接收来自IMU140的串行编码的加速度和角速度数据413，并作为输出向CPU200提供6个加速度角速度测量值631。输入/输出接口210还包括信标地址缓冲器610。CPU200(图2)提供要测量其距离的超声信标的地址。信标地址缓冲器610存储所述地址，并以串行的形式向每个URM110提供所述地址。在由每个URM110发送(以及由超声信标122接收)地址的同时，3个计数器620a-c被复位，并开始从0以固定的时钟速率(例如2MHz)增加。当每个URM110检测到来自信标的脉冲时，相应的脉冲检测信号被传递给相应的计数器，其便停止计数。然后，CPU200利用所述计数值作为超声脉冲从超声信标到达每个URM110的行程时间的测量值。参见图7a-b，跟踪装置100(图1)确定其在房间的导航参考框架中的位置，如标号为N(北)、E(东)、和D(下)的轴线710所示。位置r⁽ⁿ⁾730是一个矢量，其具有分别沿着N、E和D方向离开轴线710的位移分量(r_N ⁽ⁿ⁾,r_E ⁽ⁿ⁾,r_D ⁽ⁿ⁾)^T。跟踪装置100还确定其姿势(方向)。

参见图7b，物体的方向按照为对准物体的方向所需的滚动角、俯仰角和偏转(欧拉)角表示为θ=(Ψ,θ,Φ)^T，物体的方向由坐标轴720表示，导航方向由坐标轴710表示。这3个欧拉角被表示为3×3的方向余弦矩阵C_b ⁿ(θ)，其在参考物体框架内变换坐标的矢量，其中主要按照顺序应用围绕z,y和x轴的偏转、俯仰和滚动运动。方向余弦矩阵可以定义如下：

C_b ⁿ的上标和下标表示该矩阵取在“b”(物体)参考框架内的矢量，并提供在“n”(导航)参考框架内的矢量。

参看图8，包括旋转检测器420a-c和加速度检测器410a-c的惯性检测器800向惯性跟踪器810提供惯性测量信号。惯性跟踪器810执行图8中所示的信号流的离散的时间近似。首先，回转仪补偿820修正(矢量)角速度信号ω，以便把测量偏差考虑进去。在本例中，只校正附加的偏差δω。其它的偏差例如乘积误差(例如不正确的比例系数)，以及由于安装不精确而引起的误差也可以被校正。加速度计补偿830类似地校正加速度信号a^(b)中的附加偏差δa^(b)。如同下面要充分讨论的，使用超声波测量计算包括偏差项δω和δa^(b)。

方向积分840根据进行过偏差校正的旋转信号更新方向估算。在本例中，使用方向的方向余弦表示进行方向积分。连续的微分方程C_b ⁿ(t)=C_b ⁿ(t)S(ω(t))的离散的时间表示被用于以固定的速率更新方向余弦矩阵，一般每秒在100和200次之间。通过改变对于离散的时间系统的表示(例如C_k=C_b ⁿ(kΔt))，按下式实现方向余弦矩阵的离散时间更新：

$C_k=C_{k-1}(I+\frac{\sin \mathrm{\δ\θ}}{\mathrm{\δ\θ}}S\left(\underset{\‾}{\mathrm{\δ\θ}}\right)+\frac{1-\cos \mathrm{\δ\θ}}{\mathrm{\δ}{\mathrm{\θ}}^2}S{\left(\underset{\‾}{\mathrm{\δ\θ}}\right)}^2)$ 其中 $\underset{\‾}{\mathrm{\δ\θ}}=\frac{{\stackrel{\mathrm{\ω}}{\‾}}_{k-1}+{\stackrel{\mathrm{\ω}}{\‾}}_k}{2}\mathrm{\Δt},\mathrm{\δ\θ}=\left|\right|\underset{\‾}{\mathrm{\δ\θ}}\left|\right|$ 以及 $S\left(\underset{\‾}{\mathrm{\δ\θ}}\right)=\left[\begin{array}{ccc}0& -{\mathrm{\δ\θ}}_z& {\mathrm{\δ\θ}}_y\\ {\mathrm{\δ\θ}}_z& 0& -{\mathrm{\δ\θ}}_x\\ -{\mathrm{\δ\θ}}_y& {\mathrm{\δ\θ}}_x& 0\end{array}\right]$ 是δθ的斜边对称矩阵。注意S(δΘ)满足下式：

                 S(δθ)²=δθ²I-δθδθ^T.

为了确保C_k确实是方向余弦矩阵，在每次迭代之后其行被正交化(orthonormalized)，以便消除可能被输入到每项中的数值或近似误差。

根据被跟踪的方向余弦矩阵C_k，坐标变换850接收在物体参考框架内进行过偏差校正的加速度信号，并按照下式输出在导航参考框架内的加速度信号：

二重积分850按照下式计算速度和位置 ${\underset{\‾}{v}}_k^{\left(n\right)}={\underset{\‾}{v}}_{k-1}^{\left(n\right)}+\frac{{\underset{\‾}{a}}_{k-1}^{\left(n\right)}+{\underset{\‾}{a}}_k^{\left(n\right)}}{2}\mathrm{\Δt},$ 且 ${\underset{\‾}{r}}_k^{\left(n\right)}={\underset{\‾}{r}}_{k-1}^{\left(n\right)}+{\underset{\‾}{v}}_{k-1}^{\left(n\right)}\mathrm{\Δt}+\frac{2{\underset{\‾}{a}}_{k-1}^{\left(n\right)}+{\underset{\‾}{a}}_k^{\left(n\right)}}{6}\mathrm{\Δ}{t}^2\•$

欧拉角计算部分870接收方向余弦矩阵，并输出相应的欧拉角。惯性跟踪器810的输出是(θ,r⁽ⁿ⁾)^T。惯性跟踪器的状态包括一个15维的矢量，其由5组三维值组成：

如同下面详细说明的，惯性跟踪器810接收从超声距离测量导出的误差更新信号δx，用于校正方向、速度和位置值，并用于更新回转仪和加速度计偏差校正元件的参数。

参见图9，信标顺序器910接收来自惯性跟踪器810的位置估算r⁽ⁿ⁾。信标顺序器910使用超声信标122(图1所示)的位置(和地址)的信标图形915，确定在每一个时间段哪一个信标被触发，以便产生超声距离测量信号。例如，信标顺序器910确定和当前位置最接近的信标，并在每一个时间段在这些信标当中循环。当位置估算改变时，一般地说，最接近的一组信标也改变。在信标顺序器910依次触发每个信标之后，相应的超声脉冲到达并且被跟踪装置检测到。每个脉冲产生对于每个用于检测该脉冲的麦克风产生一个距离测量值。在本实施例中，每个脉冲产生一组3个距离测量值，每个测量值来自3个URM110中的一个麦克风。

继续参看图9，距离测量920相应于接收超声距离估算的处理。用于距离测量的相关的参数是被寻址的信标的位置b⁽ⁿ⁾，用于检测所述脉冲的麦克风的位置m^(b)，距离估算本身d_r，以及脉冲被检测的时间t_r，其被用于校正测量中的等待时间。注意，如果位置估算没有误差，因而距离估算完全精确，则距离估算应当满足：d_r=‖b⁽ⁿ⁾-(r⁽ⁿ⁾(t_r)+C_b ⁿ(t_r)m^(b))‖

使用来自这个等式的偏差校正惯性跟踪器810的参数和输出。

跟踪装置100使用互补卡尔曼滤波器当输入距离测量值时，通过不断地更新被跟踪的数量改进跟踪的位置和方向的估算。参看图10，所述的方法涉及两个相关的元件。当惯性跟踪器810更新其输出x时，卡尔曼预测器1010保持x中的误差的计算的协方差矩阵P。例如，在惯性跟踪器810中没有任何漂移补偿时，协方差矩阵P将相应于一直在增加的误差。

在这种方法中使用的第二个元件是卡尔曼更新器1020，其接收来自距离测量920的信息，并使用这个测量信息确定估算积累的误差δx，其被送回惯性跟踪器810，用于更新x。此外，在每个超声测量之后，卡尔曼更新器1020计算在更新之后在x中的误差的新估算的协方差矩阵，其被回送到卡尔曼预测器1010。每个超声测量部分地校正惯性跟踪器810的输出。连续的超声更新序列确保误差为最小。

惯性跟踪器810是一种对其输入进行非线性处理的处理器，因此，由高斯噪声驱动的用于纯线性滤波器的卡尔曼滤波器的结构是不合适的。通过使用一般被称为“扩展的卡尔曼滤波器”(EKF)，便可以使用一种线性化的动态系统模型，其表征在惯性跟踪器810的输出x中的误差的传播。由EKF模拟的误差是：

 δx=(φ,δω^(b),δr⁽ⁿ⁾,δv⁽ⁿ⁾,δa^(b))^T

具有相应于惯性跟踪器的矢量输出的分量。注意，在物体坐标系统中模拟误差项δa^(b)，而不在导航坐标系统中模拟，并且其它元件直接相应于惯性跟踪器810的输出中的误差。线性化的误差传播模型的参数包括状态转移矩阵，和用于驱动所述误差模型的驱动噪声的协方差矩阵。状态转移矩阵和驱动噪声协方差矩阵依赖于惯性跟踪器的输出。在没有任何测量值的情况下，误差处理的平均值保持为0。不过，误差的协方差增加。误差传播的线性化模型是：

             δx_k=F(x_k-1)δx_k-1+w_k-1·

其中的项F_k=F(x_k-1)是从在惯性跟踪器810中使用的更新方程的扰动分析中导出的，并且相应于下面的误差传播方程：φ_k=φ_k-1-Cⁿ _bδω_k-1,δω_k=δω_k-1, ${\mathrm{\δr}}_k={\mathrm{\δr}}_{k-1}+\mathrm{\Δt}{\mathrm{\δv}}_{k-1}-\frac{1}{2}\mathrm{\Δ}{t}^2(C_b^n\mathrm{\δ}{a}_{k-1}^{\left(b\right)}-S\left({\mathrm{\φ}}_{k-1}\right)(a_{k-1}^{\left(n\right)}+{(0,0-g)}^T))$ δy_k=δv_k-1+Δtδa^(b) _k-1-ΔtS(φ_k-1)(a⁽ⁿ⁾ _k-1 +(0,0,-g)^T),andδa^(b) _k=δa^(b) _k-1

其中处理噪声w_k的协方差被假定为是对角线的。这些协方差矩阵的项是从对惯性跟踪器810提供的惯性测量值中的已知的误差源中得到的，包括附加的偏移误差、比例误差、检测器和物体轴线的对准误差和来自检测器本身的信号噪声。各个变量依赖于惯性跟踪器的输出，如下式表示： $Q_k=\mathrm{diag}({\mathrm{\σ}}_{{\mathrm{\φ}}_x}^2,{\mathrm{\σ}}_{{\mathrm{\φ}}_y}^2,{\mathrm{\σ}}_{{\mathrm{\φ}}_z}^2,{\mathrm{\σ}}_{\mathrm{\ω}}^2,{\mathrm{\σ}}_{\mathrm{\ω}}^2,{\mathrm{\σ}}_{\mathrm{\ω}}^2,{\mathrm{\σ}}_{r_x}^2,{\mathrm{\σ}}_{r_y}^2,{\mathrm{\σ}}_{r_z}^2,{\mathrm{\σ}}_{v_x}^2,{\mathrm{\σ}}_{v_y}^2,{\mathrm{\σ}}_{v_z}^2,{\mathrm{\σ}}_a^2,{\mathrm{\σ}}_a^2,{\mathrm{\σ}}_a^2)$ 其中的各个变量项按照下式被参数化： ${\mathrm{\σ}}_{{\mathrm{\φ}}_x}=\mathrm{Gyroscale}{\mathrm{\ω}}_x\mathrm{\Δt}+\mathrm{GyroAlign}({\mathrm{\ω}}_y+{\mathrm{\ω}}_x)\mathrm{\Δt}+\mathrm{GyroNoise}\sqrt{\mathrm{\Δr}}$ ${\mathrm{\σ}}_{{\mathrm{\φ}}_y}=\mathrm{GyroScale}{\mathrm{\ω}}_y\mathrm{\Δt}+\mathrm{GyroAlign}({\mathrm{\ω}}_x+{\mathrm{\ω}}_z)\mathrm{\Δt}+\mathrm{GyroNoise}\sqrt{\mathrm{\Δt}}$ ${\mathrm{\σ}}_{{\mathrm{\φ}}_z}=\mathrm{GyroScale}{\mathrm{\ω}}_z\mathrm{\Δt}+\mathrm{GyroAlign}({\mathrm{\ω}}_x+{\mathrm{\ω}}_y)\mathrm{\Δt}+\mathrm{GyroNoise}\sqrt{\mathrm{\Δt}}$ σ_ω= GyroBiasChangeRate Δt ${\mathrm{\σ}}_{r_x}={\mathrm{\σ}}_{r_y}={\mathrm{\σ}}_{r_z}=0$ ${\mathrm{\σ}}_{v_x}=\mathrm{AccelScale}{a}_x\mathrm{\Δt}+\mathrm{AccelAlign}(a_y+a_z)\mathrm{\Δt}+\mathrm{AccelNoise}\sqrt{\mathrm{\Δt}}$ ${\mathrm{\σ}}_{v_y}=\mathrm{AccelScale}{a}_y\mathrm{\Δt}+\mathrm{AccelAlign}(a_x+a_y)\mathrm{\Δt}+\mathrm{AccelNoise}\sqrt{\mathrm{\Δt}}$ ${\mathrm{\σ}}_{v_z}=\mathrm{AccelScale}{a}_z\mathrm{\Δt}+\mathrm{AccelAlign}(a_x+a_y)\mathrm{\Δt}+\mathrm{AccelNoise}\sqrt{\mathrm{\Δt}}$ σ² _a=AccelBiasChangeRate Δt

其中GyroScale,AccelScale,GyroAlign,AccelAlign相应于在用于仪器误差补偿的校准系数中的不确定性的程度。一般地说，可以使用非对角线处理噪声协方差。

参见图11，卡尔曼预测器1010具有两级，误差初始化级1110首先按照上述计算F_k和Q_k。然后，协方差传播级1120利用迭代方法更新误差协方差，其中在每个时间段使用卡尔曼协方差传播方程：

              P_k = F_k-1P_k-1F^T _k-1+Q_k

当卡尔曼预测器1010接收到作为超声距离测量结果而产生的更新的协方差P(+)时，则更新的协方差代替当前误差协方差P。

参见图12，卡尔曼更新器1020接收来自惯性跟踪器810的距离测量920的输出以及位置和方向的估算值，以及来自卡尔曼预测器1010的位置和方向估算的误差的协方差，并计算误差估算和由应用所述误差估算而得到的更新的协方差。卡尔曼更新器1020的第一级是测量余项计算1210。期望的距离和测量的距离之间的差利用下式计算：

         δd_r=d_r-‖b⁽ⁿ⁾-(r⁽ⁿ⁾(t_r)+Cⁿ _b(t_r)m^(b))‖.

注意，一般地说，在最初检测到距离测量之后的某个时刻使用距离测量。为了考虑这个时延，使用在超声脉冲到达时的跟踪装置的位置和方向而不使用在测量时的位置和方向。使用当前的位置、方向以及线速度和角速度估算，向回推断测量时间，从而确定r⁽ⁿ⁾(t_r)和C_b ⁿ(t_r)。

为了应用卡尔曼更新方程，使用下面的线性化观测方程模拟这个余项：

                δd_r=H(x,b,dr,m)δx+v.

观测矩阵H_k=H(x_k.b,d_r,m)是位置和方向中的误差对距离测量误差的线性影响。附加噪声v具有偏差R(x_k,b,d_r,m)。

H_k具有以下形式： $H_K=(\frac{b_D{m}_g-b_g{m}_D+r_g{m}_N-r_D{m}_g}{d_r},\frac{b_N{m}_D-b_D{m}_N+r_D{m}_N-r_N{m}_D}{d_y},\frac{b_g{m}_N-b_N{m}_g+r_N{m}_g-r_g{m}_N}{D_r},$ $0,0,0,\frac{r_N+m_N-b_N}{d_r},\frac{r_g+m_E-b_E}{d_r},\frac{r_D+m_D-b_D}{d_r},0,0,0,0,0,0,)$

导出偏差R(x_k,b,d_r,m)用于模拟和超声距离测量相关的各种现象。例如，当距离增加时，脉冲检测更加困难，这部分地由于脉冲扩散，因而使用增加的距离模拟相关的距离测量误差。偏差R(x_kb,d_r,m)具有以下形式：

                           R=σ² _u+σ² _t并被参数化为：

                           σ² _u=NoiseFloor+NoiseScale d_r以及

                           σ² _t=(kΔt-t_r)H_k(ω_x,ω_y,ω_z,0,0,0,v_x,v_y,v_z,0,0,0,0,0,0))^T

H_k的前两项可以交替地被设置为0，以便进行加速度测量倾斜校准(如果需要更精确)。如果第三项被设为0，则用一个较长的时间间隔进行偏转漂移校正，不过精度较高。

卡尔曼更新器1020包括测量接收/拒绝级1230。接收/拒绝级1230接收测量余项δx，和测量余项的计算的偏差R。如果测量余项的大小大于计算的测量余项的标准偏差的一个预定的倍数，则该测量因被怀疑而被拒绝，例如，这种测量结果可以由于超声脉冲检测器被提前或推迟触发而得到。否则，测量余项便被进一步处理，以便计算状态误差估算δx。使用卡尔曼滤波更新方程，卡尔曼增益计算1240按下式计算卡尔曼增益：

        K=P_kH^T _k(H_kP_kH^T _k+R)^-1

然后，误差估算1250计算误差估算为δx-K δd，并且协方差更新器1260计算更新的误差协方差如下：

              P(+)=(I-KH)P_k,

然后使用δx的分量更新惯性跟踪器810。计算出的项δω和δa^(b)分别被送到回转仪偏差校正单元820和加速度计偏差校正单元830(图8)，在那里被分别附加到当前存储的偏差参数上．计算的项δv⁽ⁿ⁾和δ⁽ⁿ⁾被分别送到重积分单元860(图8)，在那里它们被分别附加到v⁽ⁿ⁾和r⁽ⁿ⁾的当前估算上。最后，按照下式更新方向余弦矩阵：

                C_k-(I-s(φ))C_k,

并被重新进行正交化。

再次参看图1，超声信标阵列120包括被排列成规则图案的各个超声信标122。例如，这些信标可以被排列成间距大约为2英尺的方格，最好具有3mm或更小的精度。对这些信标设置有限数量的地址，在本例中由于硬件的限制只利用8个不同的地址。因此，当跟踪装置向一个地址发出IR指令时，一般地说，将有多个超声信标接收信号并响应所述信号。只有具有任何特定地址的最近的一个信标用于距离测量。不过，因为有多个信标可以响应每个IR指令，脉冲检测电路可能被过早地触发，例如通过来自在以前的重复中的被触发的信标的脉冲，但是该信标足够远，使得其发出的脉冲直到下一次重复才到达。为了避免这个预触发问题，脉冲检测器522(图5)只在预计所需的脉冲要到达的时间窗口被选通。这便避免了由来自其它信标的脉冲或由前面的脉冲的长时间的不断的反射而形成的信号造成的误触发。

在本说明中，用于跟踪和卡尔曼更新的程序，初始位置和方向估算被假定为是已知的。但是不一定是这种情况，跟踪装置100可以使用自动获取算法。有限数量的超声信标的地址作为初始获取算法的基础。首先，跟踪装置触发具有每个可允许的地址的信标，并测量到每个地址的最接近的一个信标的距离。然后，根据距离测量确定4个最接近的信标的地址。跟踪装置包括一个信标图形，所述图形包括所有信标的位置和地址。这些信标被这样排列，使得4个最接近的信标的地址把可能的位置局限于房间的一个小的部分。如果根据最接近的信标不能确定，则使用到信标的实际的距离进行三角测量处理来进行确定。最初的方向基于对于每个麦克风的相对距离测量。

整个跟踪处理可以被归纳为图13所示的流程图。首先，使用上述的方法获得初始位置和方向(步1310)。然后程序进入每次处理一个时间段的循环。在进入等待下一个时间段之后(步1320)接收初始测量(1330)，并且被跟踪的变量x和误差协方差P通过使用惯性测量被更新(1340)。如果得到尚未处理的超声距离测量(1350)，则使用距离测量计算误差更新δx，并更新误差协方差P(+)(1360)。然后使用误差更新和新的误差协方差更新惯性跟踪器和卡尔曼预测器(1370)。然后程序确定是否在该时间段必须进行距离测量(1380)。因为对于每个脉冲进行3个距离测量，但是每个时间段只进行一次距离测量，所以可能有在下一个时间段将要进行的积累的距离测量。因此，对于若干个将来的时间段不必进行新的距离测量。考虑到下一个超声脉冲的预期的行程时间(其一般大于一个时间段)，程序确定是否在该时间段应当发送IR指令(1380)，如果是，则选择下一个信标地址(1390)，如果不是，则向那个信标发送IR指令(1395)。然后程序在步1320再次开始循环，等待下一个时间间隔的开始。

可以使用若干个不同的方法。在上述的实施例中，每个时间段只使用一次距离测量。此外，如果处理器130具有足够的计算能力，则在每一个时间段可以使用所有可利用的距离测量。这另一种方法通过从步1370向回循环到步1350直到进行所有的距离测量来实现。另外，除去对每一个距离测量的数量依次进行卡尔曼更新之外，可以在每个时间段中都可以使用类似的更新方程更新向量观测和相关的观测噪声。此外，除去直到下一个时间段进行不同的距离测量处理之外，当得到距离测量的结果时，都可以将其考虑进去，而不和惯性跟踪器的更新同步。

上述的程序可以和其它测量方式组合。例如，可以使用倾角计对扩展的卡尔曼滤波器提供允许校正方向漂移的测量。此外，除去使用3个或多个允许进行所有的3个旋转度的校正的麦克风之外，可以使用两个麦克风和所述倾角计组合进行距离测量。用这种方式，可以根据倾角计进行一些漂移校正，但是不需要用指南针进行漂移校正，因为其对磁场的变化敏感。也可以使用3个以上的麦克风提供较大的冗余度，并且允许更多的旋转自由度。

作为在环境中的固定位置安装信标和在跟踪装置上设置麦克风的通常被称为“inside-out”结构的一种替代方案，有一种相反的被称为“outside-in”的结构。此时跟踪装置提供超声脉冲，而具有坐标的麦克风阵列检测跟踪装置的位置。注意在脉冲到达麦克风的时刻，一般地说，跟踪装置已经移动到一个新的位置。这种重复的测量必须利用和上述的距离测量中使用的重复补偿相似的方式进行补偿。

信标122不必被设置在一个平面阵列中，它们可以被设置在墙壁上和天花板上，或者被设置在环境中的其它支撑物上。例如，信标可以被设置在灯具固定物上。信标的数量可以被选择，以便和用户的要求匹配，并且可以根据各种准则选择信标的位置，例如可利用的合适的固定点和几何方面的考虑，并且信标的图案可以被设置使得和选择的信标的位置和数量相匹配。在星座中信标的数量可以由用户增加或减少，只要信标图形保持更新即可。

从跟踪装置到信标的指令信号可以使用IR发送之外的其它方式发送。例如可以使用RF，可见光或声波。跟踪装置也可以通过线路和信标相连。

利用“inside-outside-in”结构可以跟踪两个或多个物体。参见图14a，跟踪装置100和上述一样跟踪其位置。第二个跟踪装置1400包括按照已知的相互位置关系排列的3个可寻址的超声信标1410。通过触发信标1410发送由跟踪装置100上的URM110接收的超声脉冲，跟踪装置可以确定第二个跟踪装置的相对位置和方向。一个进一步的扩展提供相对位置和方向估算的增加的精度，其涉及使用被固定在跟踪装置1400上的第二惯性测量装置和向跟踪装置100发送惯性测量。如果只在第二物体上设置单个的信标，则可以使用超声距离测量检测相对位置，而不需跟踪第二个装置的相对方向。

参见图14b，可以使用由多个跟踪装置构成的“相互跟踪网络”。这些跟踪装置跟踪相对于环境中的其它装置的各自的位置，在所述环境中包括固定的信标和其它运动的被跟踪的物体。这可以利用和跟踪装置相连的附加的通信系统来实现，例如RF局域网。

在上述的实施例中，信标阵列的“图形”被假定为是精确的。因为距离测量包括冗余的信息，在信标布局中的误差可以用迭代方法消除并被更新，借以改善精度。具体地说，信标的布局误差可被包括在扩展的卡尔曼滤波器的状态中，并且来自每个信标的距离测量将随时用于布局误差的估算。也可以使用一种单独的初始自动的“成形”方式，其中通过在房间内的一个或几个位置的距离测量和三角测量计算，可以确定信标的位置。这些被自动确定的位置可被用作已知的位置，或者用作初始的估算，其然后通过使用卡尔曼滤波器被进一步更新。在这种类型的方法中，信标可被无规则地设置在房间内，而不需要被精确地定位。

上述的跟踪方法有若干种应用。第一种应用涉及使跟踪装置和头戴的显示器相连。参见图15，头戴显示器1510使用户能够直接观看物理对象1520，例如工件。显示器1510使用工件1520在房间参考框架中的已知位置，把信息叠加在用户观看工件的视野中。例如，在对大型装置施加一个线束时，叠加的信息可以包括关于线束的正确放置的信息。也可以使用类似的头戴显示器在虚拟的现实系统中提供被用户观看的完整的图像，而不把图像叠加在由用户观看的实际图像上。

另一种实际应用涉及跟踪电视拍摄中的摄像机的位置。参看图16，电视摄制中的一般技术是在空白(一般是单色的)背景的前方拍摄物体1620，然后以电子方式叠加作为背景的另一个图像(表示为1630)。这种技术的困难在于，当摄像机1610运动时，背景图像应当改变，以便反映摄像机的运动。通过在照相机1610上设置跟踪装置100，可以跟踪摄像机的位置和方向，通过用于接收摄像机的改变的位置和方向的图像处理器自动地修正背景图像。这种方法使得能够构成大的被存储在图像处理器中的“虚拟场景”，借以可以使用多个并且不断改变的摄像机的角度。

另一种应用涉及检测汽车中的物体的运动，例如，在汽车碰撞试验中。参看图17，在碰撞的汽车1710内的假人1720可以使用跟踪装置100进行跟踪。此外，第二个物体例如防火墙上的一点可以使用上述的inside-outside-in方法利用信标1730进行跟踪。这使得能够在汽车的参考框架内跟踪假人并且相对于假人跟踪汽车内的一点。

其它的应用包括自动导航，库存物品、资产或人员的跟踪，用于破坏控制的虚拟的或扩大真实性的船只，摄像机的跟踪，娱乐(例如音乐主题和游戏节目)的跟踪，用于运动捕获的整个物体的跟踪，以及武器的跟踪。

另一个实施例也可以使用其它方法进行惯性跟踪。例如，不使用方向余弦矩阵进行方向积分，可以同样地使用欧拉角或四元法进行方向积分。注意线性化误差传播系统矩阵和驱动噪声协方差在一定程度上取决于使用的具体的跟踪算法。此外，可以改变卡尔曼滤波器的状态，例如以便包括其它的项。这方面的一个例子不仅跟踪加速度计附加偏移，如上面的实施例中所述，而且跟踪加速度计信号、误对准和声音速度的乘积偏差。

也可以使用其它的距离测量方法，包括声音相位，RF或光的行程时间，RF或光的相位，以及机械电缆延伸。

也可以使用熔合(fusing)惯性和声音测量的其它方法代替卡尔曼滤波方法。例如，神经网络，基于规则的推理，或模糊逻辑系统，或者使用优化方法使这些测量方法相结合。

在上面的说明中，只使用了8个不同的超声信标地址。此外，每个信标可以被单独地编址，或者可以使用大量的共用地址。如果信标能够被单独地寻址，可以进行初始获取，例如，通过使信标也响应“组地址”，或者以这种方式在获取阶段响应对各个信标寻址的指令序列，使得跟踪装置通过首先找到在范围内的一个信标对其初始位置“zero in(归零)”，然后根据跟踪装置已知的信标图形检索越来越接近的信标。当跟踪区域由几个不同的房间构成时也可以使用这种方法。首先，确定跟踪装置所在的房间，然后确定其在房间中的位置。

应当理解，上面的说明只是用于说明本发明，而不限制本发明的范围，本发明的范围应由所附的权利要求限定。其它的方面、优点和改型都落在下面的权利要求的范围内。

Claims (15)

1．一种用于跟踪物体的运动的方法，其包括以下步骤：

获得和物体的运动相关的两种类型的测量，其中一种类型的测量包括声学测量；

根据两种测量中的一种测量更新物体的方向或位置的估算；以及

根据两种测量中的另一种测量更新所述估算。

2．如权利要求1所述的方法，其中一种类型的测量包括声学距离测量。

3．如权利要求1所述的方法，其中另一种类型的测量包括惯性测量。

4．如权利要求1所述的方法，其中所述估算是方向估算。

5．一种用于跟踪物体运动的装置，包括：

两个检测器系统，其被构成分别用于获得和物体的运动相关的两种类型的测量，其中一种类型的测量包括声学测量；以及

和所述两个检测器系统相连的处理器，其被构成用于根据两种测量中的一种测量更新物体的方向或位置的估算，并根据两种测量中的另一种测量更新所述估算。

6．一种跟踪装置，包括：

检测器系统，其中包括：

惯性检测器；和

一组和惯性检测器刚性连接的一个或几个声音检测器；以及

处理器，其被编程用于执行以下功能：

接收来自惯性检测器的惯性测量结果；

使用接收的惯性测量结果更新位置估算和方向估算；

选择多个声学参考装置中的一个；

接收关于在检测器系统和选择的声学参考装置之间的距离的声学距离测量结果；

使用接收的距离测量结果更新位置估算和方向估算。

7．如权利要求6所述的跟踪装置，其中检测器系统包括用于发送控制信号编码和选择的声学参考装置的识别符的发送器，并且每个声学检测器包括用于接收来自声学参考装置的声音信号的麦克风。

8．如权利要求6所述的跟踪装置，其中所述一组一个或几个声学检测器包括两个或多个声学检测器。

9．如权利要求6所述的跟踪装置，其中使用接收的惯性测量结果更新位置估算和方向估算包括更新位置和方向估算中的不确定性；以及使用接收的惯性测量结果更新位置估算和方向估算包括确定距离测量中的不确定性，并使用距离测量中的不确定性更新位置和方向估算中的不确定性。

10．一种用于跟踪物体运动的方法，包括以下步骤：

选择多个参考装置中的一个装置；

向所选择的参考装置发送控制信号；

接收来自参考装置的距离测量信号；

接收关于对选择的参考装置的距离的测量；以及

使用所接收的距离测量更新物体的位置估算和方向估算。

11．如权利要求10所述的方法，包括：

根据距离测量信号的行程的时间确定距离测量。

12．如权利要求10所述的方法，其中发送控制信号包括发送无线控制信号。

13．一种被存储在加速计可读介质中的软件，包括用于使计算机执行以下功能的指令：

选择多个参考装置中的一个装置；

向所选择的参考装置发送控制信号；

接收来自参考装置的距离测量信号；

接收关于对选择的参考装置的距离的测量；以及

使用所接收的距离测量更新物体的位置估算和方向估算。

14．一种跟踪系统，包括：

声学参考系统，其中包括多个声学参考装置；以及

跟踪装置，包括：

检测器系统，其包括惯性检测器和一组与惯性检测器刚性连接的一个或几个声音检测器；以及

处理器，其被编程用于执行以下功能：接收来自惯性检测器的惯性测量结果，使用接收的惯性测量结果更新位置估算和方向估算，选择多个声学参考装置中的一个，接收关于在检测器系统和选择的声学参考装置之间的距离的声学距离测量结果，使用接收的距离测量结果更新位置估算和方向估算。

15．如权利要求14所述的系统，其中检测器系统包括用于发送控制信号编码和选择的声学参考装置的识别符的发送器，并且每个声学检测器包括用于接收来自声学参考装置的声音信号的麦克风，并且其中每个声学参考装置包括用于接收来自检测器系统的控制信号的接收器和用于发送声音信号的声学传感器。

Images