具体实施方式

下面将参照附图更详细地描述本公开内容的示例性实施方式。虽然附图中显示了本公开内容的示例性实施方式，然而应当理解，可以以各种形式实现本公开内容而不应被这里阐述的实施方式所限制。相反，提供这些实施方式是为了能够更透彻地理解本公开内容，并且能够将本公开内容的范围完整地传达给本领域技术人员。在以下详细描述中没有任何内容旨在表明任何特定组件、特征或步骤对于本发明是必不可少的。本领域技术人员将会理解，在不脱离本公开内容的范围内各种特征或步骤可以彼此替代或结合。

图1示出了依据本公开内容示例性实施方式的基于多方向超声测距及IMU进行三维定位的方法的流程图。IMU(Inertial Measurement Unit)是惯性测量单元，其是主要用来检测和测量加速度与旋转运动的传感器。惯性测量单元通常包括加速度计和角速度计(陀螺仪)，它们是惯性系统的核心部件。本公开内容提出了一种基于多方向超声测距及IMU进行三维定位的方法，包括步骤：S1，根据IMU测量的数据来计算本体的朝向；S2，通过多个超声波传感器来获得本体与墙面的距离；S3，根据所计算的朝向和所获得的距离来计算本体的三维坐标；S4，使用四元数球面线性插值(Slerp)对所述朝向进行插值来获得所述朝向的轨迹，例如朝向的轨迹方程；S5，使用样条插值(例如，Catmull-Rom Spline插值)对所述三维坐标进行插值来获得所述三维坐标的轨迹，例如所述三维坐标的轨迹方程。

在优选实施方式中，根据IMU测量的数据来计算本体的朝向包括采用IMU update及Madgwick算法结合所述IMU的加速度计和陀螺仪得到俯仰(yaw)、滚转(pitch)和偏航(roll)角速度。由于IMU update算法运行较长时间后yaw角速度精度可能下降，可以通过地磁传感器矫正其yaw角速度。关于IMU update及Madgwick算法的详细内容可以参考论文https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/。

其中，需要求解用四元数表示的朝向的微分方程如下：

其中，dQ为四元数表示的朝向，q₀，q₁，q₂，q₃表示上一时刻的四元数，ω_x，ω_y，ω_z分别表示围绕x，y，z轴旋转的角速度。

图2示出了依据本公开内容示例性实施方式的多个超声波传感器的布置的示意图；图3示出了依据本公开内容示例性实施方式的多个超声波传感器进行测距的示意图。只有朝向数据，尚无法对本体进行三维坐标的定位。因此本方案需要多个方向的超声波传感器来获得本体与墙面的距离，进而结合朝向数据推算本体的三维坐标。在优选实施方式中，所述多个超声波传感器是三个超声波测距传感器，所述三个超声波测距传感器的纵轴两两垂直。

在多个超声波测距传感器测量本体与墙面的距离时，多个超声波测距传感器发出的多个方向的超声波回波可能会因为多次反射而被其他传感器接收到，这会对距离计算造成干扰，使得无法稳定计算回波距离。例如，可以采用时分方式，每次只有一个超声波测距传感器工作，但这样会使得定位系统消耗更多的等待时间。本发明的技术方案采用抗干扰编码技术，对超声波发射器发出的波形进行编码，使不同的超声波发射器发出不同的超声波编码序列，从而接收端可以根据接收到的超声波形判断是否是其对应的回波。在优选实施方式中，对所述三个超声波测距传感器发出的波形进行不同地编码，使不同的超声波测距传感器发出不同的超声波编码序列，从而根据接收到的超声波编码序列判断是否是对应的回波。例如，采用二进制编码方式，使用1个字节对超声波信号进行编码控制。具体地，三个超声波测距传感器以不同频率，优选以40Hz的频率，发出超声波编码序列，其由8个比特表示。图4示出了依据本公开内容示例性实施方式的多个超声波传感器的不同编码方式的示意图。所述三个超声波测距传感器输出的超声波编码序列的8个比特分别为11111111、11010111以及11101011，其中0表示不输出超声波，即静音，1表示输出超声波。

图5和图6示出了依据本公开内容示例性实施方式的计算本体的三维坐标的示意图。如图5所示，采用如下公式计算本体所在水平面距离房顶的高度h1：

将上述公式用于其他两个墙面，则可得到本体所在平面到其他两个墙面的距离h2和h3，由此可测得本体的三维坐标，如图6所示。

针对三维位置的轨迹，使用通过三维定位给出的前后两个三维位置之间的轨迹。样条插值算法是使用一种数学函数对一些限定的点值，通过控制估计方差，利用一些特征节点，用多项式拟合的方法来产生平滑的插值曲线。在本发明的实施方式中，采用Catmull-Rom样条插值对所述三维坐标进行插值，以获得两个三维位置之间的轨迹曲线。假设存在四个顶点P_i-2，P_i-1，P_i，P_i+1，则针对在P_i-1，P_i之间的轨迹进行插值，可以采用如下公式进行：

其中u可从0变化到1，则坐标将从P_i-1变化到P_i，τ表示曲线弯曲的程度。

考虑用四元数表示的前后两个三维朝向q₁和q₂，则对三维朝向进行球面线性插值，其结果是如下方程描述的四元数：

其中u可从0变化到1，则朝向从q₁变化到q₂，θ表示q₁和q₂之间的夹角。

在优选实施方式中，可以将本发明的多个超声波传感器组合成一个整体，其中所述IMU和所述多个超声波传感器通过粘合剂、固定螺丝和专用模具之一固定在一起，以实现传感器的模块化组装，便于更换单个传感器以取得较高的灵活性和可配置性。在优选实施方式中，所述IMU和所述多个超声波传感器通过电路集成在一起，以实现传感器组合体的小型化和一体化。多个传感器的数据传输方式例如可以包括：(1)通过无线或有线的方式传输到计算机，由计算机上的程序计算并插值；(2)使用单片机等微小型计算装置，实现本地计算，并将计算结果传输到计算机，以便虚拟现实等应用使用。计算机的示例包括个人计算机(例如便携式PC)、平板电脑或平板PC(例如，iPad、Galaxy Tab)、电话、智能手机(例如，iPhone、支持Android的设备、)或个人数字助理。传感器数据的传输或计算结果(使用单片机等微小型计算装置本地计算的结果)数据的传输方式例如可以通过例如通用串行总线(USB)端口、以太网端口、IEEE1394端口等有线方式传输；或通过例如蓝牙、wifi、红外接口等无线方式进行传输。传感器数据的融合计算包括对朝向数据和距离数据进行三维位置解算，对朝向数据插值，对三维坐标插值。对于三维位置的解算以及插值计算可以采用软件程序来实现。软件程序可以用任何编程语言编写，例如C、C++、Java或Visual Basic。软件程序也可以在应用框架上运行，例如框架、框架或任何其他应用框架。

本发明的技术方案采用上述的方法和设备，能够避免复杂设置环境并且无需对室内环境进行提前布置，无需设置导航站，另外以低成本并且高灵活性的方式解决三维定位问题，并且进一步避免了多个超声波测距之间的信号干扰，并且计算简单快速，能够编码到单片机或简单的计算单元中执行，成本具有巨大优势。

在本公开内容的一个方面，还提供了一种基于多方向超声测距及IMU进行三维定位的设备，包括处理器和存储器，其中所述存储器上存储有计算机程序指令，所述计算机程序指令在由所述处理器执行时实现上述任一项的方法。

在本公开内容的又一方面，还提供了一种机器可读存储介质，其上存储有计算机程序指令，其中所述计算机程序指令在由处理器执行时实现如上文所述的用于进行时间管理的方法。在一些实施方式中，机器可读存储介质是数字处理设备的有形组件。在另一些实施方式中，机器可读存储介质可选地是可从数字处理设备移除的。在一些实施方式中，举非限制性示例而言，机器可读存储介质可以包括U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、闪速存储器、可编程只读存储器(PROM)、可擦除可编程只读存储器(EPROM)、固态存储器、磁碟、光盘、云计算系统或服务等。

应当理解，本公开内容的方法实施方式中记载的各个步骤可以按照不同的顺序执行，和/或并行执行。此外，方法实施方式可以包括附加的步骤和/或省略执行示出的步骤。本发明的范围在此方面不受限制。

在本文所提供的说明书中，说明了大量具体细节。然而，应当理解，本公开内容的实施方式可以在没有这些具体细节的情况下实践。在一些实施方式中，并未详细示出公知的方法、结构和技术，以便不模糊对本说明书的理解。

虽然本文已经示出和描述了本发明的示例性实施方式，但对于本领域技术人员容易理解的是，这样的实施方式只是以示例的方式提供的。本领域技术人员现将会在不偏离本发明的情况下想到许多更改、改变和替代。应当理解，在实践本发明的过程中可以采用对本文所描述的本发明实施方式的各种替代方案。以下权利要求旨在限定本发明的范围，并因此覆盖这些权利要求范围内的方法和结构及其等同项。