CN101866005A - 空间感测装置、移动载体与其定位与控制的运算方法 - Google Patents

Abstract

空间感测装置、移动载体与其定位与控制的运算方法。该空间感测装置，适用于一移动载体，包括姿态角计算模块、位置计算模块和运算处理系统。姿态角计算模块可以根据一个或多个感测器的输入信号融合运算，而计算出此移动载体目前在空间中与不同方向轴之间所夹的姿态角度。另外，位置计算模块则可以依据这些姿态角度和加速度参数来计算移动载体目前在空间中的位置，并输出一定位信息给运算处理系统。此外，运算处理系统可以通过机械波距离感测器可以获得另一空间定位信息。藉此，运算处理系统就可以将两者定位信息融合计算而获得一即时三度空间定位信息，以控制移动载体的在空间中的运动轨迹。

Description

空间感测装置、移动载体与其定位与控制的运算方法

技术领域

本发明涉及一种定位与周围环境感测的技术，且特别涉及一种移动载体在空间中运动时所进行的定位与周围环境感测的技术。

背景技术

众所皆知地，目前的定位技术以全球卫星定位最为普遍。然而，全球卫星定位技术还是有许多的限制，尤其是受到地形和环境的限制尤为明显。这是因为全球卫星定位的技术是通过接收由地球轨道上的定位卫星所发出的定位信号，而进行三角定位所达成。因此，在一些环境中，例如，在大楼中，或是在水面下，由于无法有效地接收卫星信号，导致而无法应用全球卫星定位的技术。

在一些已知的专利中，也提供一些技术，以在特殊环境中，例如水面下，也能够使用全球卫星定位的技术。例如WO2008048346案，就提供了一种水下潜水器的定位技术。在此篇专利中，发明人是利用水面上的浮标接收定位卫星所发出的定位信号，并且计算浮标与潜艇之间的相对位置。接着，再从浮标接收所接收到的定位信号，然后利用浮标与潜舰之间的相对位置来修正计算潜舰的位置。

虽然在先前技术中，潜舰可以利用水面上的浮标来接收定位信号，并且进而对潜舰本身进行定位。然而，由于信号在水面下传输时，会受到水作为介质的干扰，而使得信号的可靠度大为降低。此外，也由于定位信号需要经由浮标再传送给潜舰进行运算，因此已知的技术无法在收不到GPS信号时进行即时定位。

另外，也有些技术是利用电磁波，例如光学的方式，来作定位的技术。然而在一些环境中，电磁波的定位同样也受限制。例如，当一水下机器人在一水族箱作业时，如果是水下机器人发出电磁波来进行定位，则会因为水族箱的缸壁是透明玻璃的材质，因此电磁波并不会被反射，而是会穿透缸壁。如此一来，就无法通过电磁波来进行定位。

发明内容

本发明提供一种移动载体，可以在一些特殊环境下运动时进行定位，并且可以依据周围的环境而调整其运动的轨迹。

本发明又提供一种空间感测装置，可以在一移动载体在一空间中移动时对其即时地进行定位。

另外，本发明还提供一种移动载体在空间中运动时的控制方法，可以控制移动载体在空间中的动向。

本发明提供一种移动载体，包括感测模块、定位系统、机械波收发装置、运算处理系统和控制系统。感测模块是用来感测移动载体在一空间中的动向，并输出至少一组空间参数给定位系统。藉此，定位系统可以依据这些空间参数而对移动载体进行定位，并输出一定位信息。另外，机械波收发装置可以发射一机械波至空间中，并且在机械波遭遇一物体而被反射后，接收被反射的机械波，而产生一环境信息。其中，环境信息可以和定位信息一起被传送至运算处理系统。因此，运算处理系统可以依据定位信息和环境信息而产生一即时运算信息给控制系统。藉此，控制系统就可以依据此即时运算信息，而控制移动载体在空间中的动向。

从另一观点来看，本发明也提供一种空间感测装置，其包括姿态角计算模块和位置计算模块。姿态角计算模块可以依据一移动载体在空间中运动时所产生的多个角速度参数以及加速度参数或是磁力线切割角度参数，而计算出此移动载体目前在空间中与不同方向轴之间所夹的姿态角度。另外，位置计算模块则可以依据这些姿态角度和多个加速度参数来计算移动载体目前在空间中的位置，并输出一定位信息。

从另一观点来看，本发明还提供一种移动载体在空间中运动时的控制方法，包括检测移动载体在空间中的动向，并且依据检测的结果而对移动载体定位，而产生一定位信息。另外，本发明也可以从移动载体向空间发出一机械波，并且接收被物体反射后的机械波，以获得一环境信息。藉此，本发明可以对定位信息和环境信息进行运算，以控制移动载体在空间动向。

由于本发明是依据所接收到的空间参数来进行定位，因此本发明具有较佳的精确度。另外，本发明是利用机械波来检测周围的环境的变化，因此本发明还可以适用在一些例如是水下的特殊环境。

为让本发明的上述和其他目的、特征和优点能更明显易懂，下文特举优选实施例，并配合附图，作详细说明如下。

附图说明

图1绘示为依照本发明的一优选实施例的一种移动载体的系统方块图。

图2绘示为依照本发明的一优选实施例的定位系统与感测模块的系统方块图。

图3A绘示为一种角速度参数的示意图。

图3B绘示为一种姿态角的示意图。

图4绘示为依照本发明的一优选实施例的姿态角计算模块、位置计算模块和校正单元的系统方块图。

图5绘示为依照本发明的一优选实施例的一种运算处理系统的系统方块图。

图6绘示为依照本发明的一优选实施例的一种控制系统的系统方块图。

【主要元件符号说明】

102：空间感测装置

104：控制系统

106：感测模块

108：机械波收发装置

112：定位系统

114：运算处理模块

202：角速度感测器

204：加速度感测器

212：姿态角计算模块

214：位置计算模块

216：校正单元

302：移动载体

402：四元素运算单元

404：方向余弦运算单元

406：加速度运算单元

408：加速度积分器

410：速度积分器

412：坐标转换运算单元

502：地图比对模块

504：数据比对模块

602：运算单元

604：控制单元

612：显示模块

COMP1：比对结果

D：方向

EIFO：环境信息

ERR：误差值

FD1、FD2：反馈数据

IN：输入指令

O：原点

PIFO：定位信息

REOP：即时运算信息

X(ref)、Y(ref)、Z(ref)、X(B)、Y(B)、Z(B)：坐标轴

a_x，g、a_y，g、a_z，g：加速度参数

a_x、a_y、a_z：加速度分量

e0_t、e1_t、e2_t、e3_t：四元素运算元

e0_t-1、e1_t-1、e2_t-1、e3_t-1：反馈四元素运算元

p、q、r：角速度参数

x_G、y_G、z_G：地球坐标系坐标值

Z_x、Z_y和Z_z：载体与环境的相对距离值

θ、φ、ψ：姿态角

具体实施方式

以下的叙述将伴随着对应的图示，来说明本发明所提供的移动载体和其系统的具体实施例。本发明所提供的移动载体可以是一机器人，而其作业的空间可以是水下的空间，但是本发明并不以此为限。

图1绘示为依照本发明的一优选实施例的一种移动载体的系统方块图。请参照图1，本实施例所提供的移动载体包括空间感测装置102和控制系统104。其中，空间感测装置102可以依据移动载体在一空间中运动的动向，而对移动载体在空间中的位置进行即时的定位。另外，空间感测装置102还可以判断移动载体所在空间的环境的变化。当空间感测装置102获得以上的信息后，可以将其传送给控制系统104。藉此，控制系统104可以依据输入指令IN以及空间感测装置102所输出的信息，而合适地控制移动载体在空间中的运动轨迹。

为了有效地对移动载体进行定位，以及判断移动载体所在的空间中环境的变化，本实施例还包括感测模块106和机械波收发装置108，二者可以分别耦接空间检测装置102。感测模块106可以感测移动载体在空间中运动的动向，并且可以输出多个空间参数给空间感测装置102，以便对移动载体进行即时的定位。另外，机械波收发装置108则可以向移动载体作业的空间发射一机械波，并且在机械波遭遇到物体而被反射时，则可以接收被反射的机械波。藉此，机械波收发装置108就可以依据被反射的机械波，而输出一环境信息EIFO给空间感测装置102。

在一些实施例中，当移动载体作业的环境为水下的环境时，则机械波收发装置108利用一声纳装置来实现。换句话说，机械波发射装置108所发出的机械波可以是声纳波。由于声纳波的频率很低，适合在密度大于空气的介质中传递。因此，当移动载体在水下作业时，就适合用声纳波来进行环境的探测。

请继续参照图1，空间检测装置102可以包括定位系统112和运算处理系统114。其中，定位系统112可以耦接感测模块106，以接收其输出的空间参数，并且定位系统112的输出可以耦接至运算处理模块114。此外，运算处理模块114则可以耦接机械波收发装置108，以接收其输出的环境信息EIFO，并且依据所接收到的信息而输出一即时运算信息REOP给控制系统104。

图2绘示为依照本发明的一优选实施例的定位系统与感测模块的系统方块图。请参照图2，在本实施例中，感测模块106包括角速度感测器202和加速度感测器204。角速度感测器202可以利用陀螺仪来实现，其用来感测移动载体在空间运动时在不同方向轴的角速度，并且产生多个角速度参数p、q和r。另外，加速度感测器204可以利用加速规来实现，其可以感测移动载体在空间中运动时在每个方向轴上的加速度，并且产生多个加速度参数a_x，g、a_y，g和a_z，g。

图3A绘示为角速度参数的示意图。请参照图3A，其中坐标轴X(ref)、Y(ref)和Z(ref)所代表的坐标系统，是一参考坐标系统。当一移动载体302在此参考坐标系统中移动时，其移动的方向可以定义为一本体Z(B)轴，并且依据此本体Z(B)轴，可以另外定义出本体X(B)轴和本体Y(B)轴。而上述的角速度参数p、q和r，则是物体302在本体X(B)轴、本体Y(B)轴和本体Z(B)轴所产生的角速度。

请再参照图2，在本实施例中，上述的角速度参数p、q和r、以及加速度参数a_x、a_y和a_z都可以被送至定位系统112，以对移动载体在空间中的位置进行即时的定位。定位系统112可以包括姿态角计算模块212、位置计算模块214和校正单元216。其中，姿态角计算模块212可以耦接角速度感测器202和校正单元216，而位置计算模块214除了耦接姿态角计算模块212之外，还可以耦接校正单元216和运算处理系统114。另外，运算处理系统114的输出也可以耦接至校正单元216。

姿态角计算模块212可以依据角速度参数p、q和r，以及校正单元216所输出的第一反馈数据FD1，而计算出移动载体的姿态角θ、φ和ψ。图3B绘示为一种姿态角的示意图。请合并参照图3B，依据图3A中的参考坐标系统和本体坐标系统，就可以定义出移动载体302的姿态角θ、φ和ψ。

姿态角计算模块212可以将所计算出来的姿态角θ、φ和ψ送至位置计算模块214。藉此，计算模块214可以依据姿态角θ、φ和ψ、加速度参数a_x，g、a_y，g和a_z，g和一第二反馈数据FD2而计算出移动载体302目前在空间中的位置坐标x_t、y_t和z_t，并且产生定位信息PIFO给运算处理系统114和校正单元216。

图4绘示为依照本发明的一优选实施例的姿态角计算模块、位置计算模块和校正单元的系统方块图。请参照图4，姿态角计算模块212包括四元素运算单元402和方向余弦运算单元404。四元素运算单元402可以耦接例如图2中的角速度感测器202和校正单元216，以接收角速度参数p、q和r，以及第一反馈数据FD1。而通过角速度参数p、q、r和第一反馈数据FD1，四元素运算单元402就可以计算出四元素运算元(Quatemion)e0_t、e1_t、e2_t和e3_t，并且传送给方向余弦(Direction Cosine)运算单元404。当方向余弦运算单元404接收到四元素运算元e0_t、e1_t、e2_t和e3_t时，可以将其进行余弦转换，并且依据第一反馈数据FD1以获得姿态角θ、φ和ψ。在本实施例中，第一反馈数据FD1包括在前一单位时间中所获得的四元素运算元(e0、e1、e2、e3)_t-1和姿态角(θ、φ、ψ)_t-1。

另外，位置计算模块214则包括加速度运算单元406、加速度积分器408、速度积分器410和坐标转换运算单元412。加速度运算单元406可以耦接方向余弦运算单元404，并且耦接加速度积分器408。另外，速度积分器410也可以耦接加速度积分器408，并且耦接坐标转换运算单元412。其中，加速度积分器408和速度积分器410还可以耦接例如图2中的校正单元216，而坐标转换运算单元412则可以耦接图2中的运算处理系统114。

加速度运算单元406还可以耦接例如图2中的加速度感测器204，以接收加速度参数a_x，g、a_y，g和a_z，g。由于加速度感测器204所感测到的加速度参数a_x，g、a_y，g和a_z，g，是含有地球重力加速度的成分，而并非单纯移动载体的加速度。因此，就需要加速度运算单元406依据姿态角θ、φ、ψ，而将重力加速度的因素从感测器量测到的加速度参数a_x，g、a_y，g、a_z，g抽离，而获得移动载体在空间中不同方向轴上的实际加速度分量a_x、a_y、a_z。以图3为例，加速度运算单元406所获得的速度值a_x、a_y、a_z，就是移动载体302朝D方向运动时，在各方向轴X、Y和Z上的加速度分量。

接着，加速度运算单元可以将加速度分量a_x、a_y和a_z送至加速度积分器408。此时，加速度积分器408就可以依据第二反馈数据FD2，而将加速度分量a_x、a_y和a_z进行积分运算，并获得移动载体在空间中各方向上的速度分量v_x、v_y和v_z。

当加速度积分器408获得速度分量v_x、v_y和v_z后，可以输出至速度积分器410。藉此，速度积分器410可以依据第二反馈数据FD2，而将速度分量v_x、v_y和v_z进行积分运算，并且获得移动载体在体轴坐标空间中各方向上的位移值x_B、y_B、z_B。而此位移值x_B、y_B、z_B可以被送至坐标转换运算单元412。藉此，坐标转换运算单元412就可以依据一转移矩阵而对位移值x_B、y_B、z_B进行运算，以获得移动载体在地球坐标空间中的地球坐标系坐标值x_G、y_G、z_G，并且当作定位信息PIFO送给运算处理系统114。在本实施例中，第二反馈数据FD2包括在前一单位时间中所获得的速度分量(v_x、v_y和v_z)_t-1、地球坐标系坐标值(x_G、y_G、z_G)_t-1和位移值(x_B、y_B、z_B)_t-1。

图5绘示为依照本发明的一优选实施例的一种运算处理系统的系统方块图。请参照图5，本实施例中的运算处理系统114包括地图比对模块502和数据比对模块504。地图比对模块502内建有移动载体所在空间的地图模型，并且地图比对模块502可以数据比对模块504。另外，数据比对模块504还可以耦接控制系统104和机械波收发装置108。

当地图比对模块502收到定位信息PIFO时，可以比对内建的地图模型，以确定该物体是否为空间内原始的地形地物，并且地图比对模块502可以输出比对结果COMP1给数据比对模块。此时，数据比对模块504可以依据机械波收发装置108利用返回的机械波所计算出来的载体与环境的相对距离值Z_x、Z_y和Z_z组成的环境信息EIFO，与位置计算模块计算出的载体在地球坐标系统中的位置x_G、y_G、z_G比对，并获得一误差值ERR。此时，数据比对模块504可以将此误差值ERR送至定位系统112中的校正单元216，并且当作即时运算信息REOP送至控制系统104。

请合并参照图2和图5，当校正单元216接收到此误差值ERR时，可以判断此误差值ERR是否大于一预设值。如果是校正单元216发现此误差值ERR并未大于预设值时，则利用环境信息EIFO来校正定位信息PIFO，并且产生对应的第一反馈数据FD1及第二反馈数据FD2。相对地，如果是误差值ERR大于预设值时，代表在移动载体在空间中运动的路径上可能有障碍物的出现。此时，校正单元216则将原始的定位信息PIFO当作反馈数据第一FD1及第二反馈数据FD2而输出。

图6绘示为依照本发明的一优选实施例的一种控制系统的系统方块图。请参照图6，本实施例中的控制系统104包括运算单元602和控制单元604。运算单元602可以耦接运算处理系统114中的数据比对单元404，并且耦接控制单元104。另外，运算单元602还可以接收使用者所输入的指令IN。藉此，运算单元602可以将输入指令IN与即时运算信息REOP进行混合运算，并且将运算结果RSL送至控制单元604。因此，如果是移动载体在空间中运动并且发现运动的方向上有障碍物时，则控制单元604就可以依据运算单元602所产生的运算结果RSL，而控制移动载体的动向，以闪避障碍物而到达目的地。在一些实施例中，控制单元604可以利用单芯片来实现。

在一些选择实施例中，移动载体上还可以配置有显示模块612，例如是液晶显示器或是发光二极管。此显示模块612是用来反应并且显示目前移动载体的状况。例如，当移动载体604发现有障碍物时，控制单元604可以点亮显示模块612。藉此，使用者就可以验证移动载体的动作反应是否正确。

综上所述，由于本发明可以利用感测模块所产生的空间参数来对移动载体进行定位，因此本发明除了可以准确地对移动载体的位置进行定位，还可以检测移动载体即时的姿态。另外，本发明还可以利用机械波来探测周围环境的变化，因此本发明还可以应用在一些特殊的环境中。除此之外，本发明是结合感测模块和机械波交替来检测，因此可以降低噪声的影响。

虽然本发明已以优选实施例公开如上，然其并非用以限定本发明，本领域技术人员，在不脱离本发明的精神和范围内，当可作些许的更动与润饰，因此本发明的保护范围当视所附权利要求书所界定者为准。

Claims (18)

1.一种移动载体，包括：

一感测模块，感测该移动载体在一空间中的动向，并输出至少一组空间参数；

一定位系统，耦接该感测模块，以依据这些空间参数而对该移动载体进行定位，并输出一定位信息；

一机械波收发装置，用以发射一机械波至该空间中，并在该机械波遭遇一物体而被反射后，接收被反射的机械波，而产生一环境信息；

一运算处理系统，耦接该定位系统和该机械波收发装置，以依据该定位信息和该环境信息而产生一即时运算信息；以及

一控制系统，耦接该运算处理模块，以依据该即时运算信息控制该移动载体在该空间中的动向。

2.如权利要求1所述的移动载体，其中该感测模块包括：

一角速度感测器，感测该移动载体在该空间中的角速度，并产生多个角速度参数给该定位系统；以及

一加速度感测器，感测该移动载体在该空间中的各方向轴上的加速度，并产生多个加速度参数给该定位系统。

3.如权利要求2所述的移动载体，其中该定位系统包括：

一四元素运算单元，耦接该角速度感测器，并接收多个反馈四元素运算元，以依据一第一反馈数据而将这些角速度参数转换成多个即时四元素运算元；

一方向余弦运算单元，耦接该四元素运算单元，以依据这些即时四元素运算元和该第一反馈数据而计算出该移动载体目前在该空间中与不同方向轴之间所夹的姿态角度；

一加速度运算单元，耦接该方向余弦运算单元，以依据这些姿态角度，并将重力加速度的因素从这些加速度参数中抽离，而计算出该移动载体在各方向上的重力加速度分量；

一加速度积分器，耦接该加速度运算单元，并接收这些角速度参数，以依据一第二反馈数据而将这些重力加速度分量进行积分运算，并获得该移动载体在不同方向上的速度分量；

一速度积分器，耦接该加速度积分器，以依据该第二反馈数据而将这些速度分量进行积分运算，并获得该移动载体在不同方向的位移值；

一坐标转换运算单元，耦接该速度积分器，以依据这些位移值计算出该移动载体在该空间中的相对位置坐标值，并当作该定位信息送给该运算处理系统；以及

一校正单元，耦接该运算处理系统，以依据该即时运算信息来决定是否对这些相对位置坐标值进行校正处理，以产生该第一反馈数据和该第二反馈数据。

4.如权利要求3所述的移动载体，其中该第一反馈数据包括在前一单位时间所获得的这些四元素运算元和这些姿态角度，而该第二反馈数据则包括在前一单位时间所获得的这些速度分量、这些相对位置坐标值和该移动载体在不同方向的位移值。

5.如权利要求1所述的移动载体，其中该机械波为声纳波。

6.如权利要求1所述的移动载体，其中该运算处理系统包括：

一地图比对模块，耦接该定位系统，并具有该移动载体所在的空间的地图模型，以依据该定位信息产生一地图坐标数据；以及

一数据比对模块，耦接该地图比对模块、该机械波收发装置和该控制系统，以比对该地图坐标数据和该环境信息进行比对，并产生一比较值。

7.如权利要求6所述的移动载体，其中该控制系统包括：

一运算单元，耦接该数据比对模块，以依据该比较值而输出一运算结果；以及

一控制单元，耦接该运算单元，以依据运算结果来控制该移动载体在该空间中的动向。

8.如权利要求1所述的移动载体，还包括一显示模块，用以显示该控制系统的状态。

9.如权利要求8所述的移动载体，其中显示模块包括发光二极管或液晶显示器。

10.一种空间感测装置，适于当一移动载体在一空间中运动时对其定位，而该空间感测装置包括：

一姿态角计算模块，用以依据该移动载体在该空间中运动时所产生的多个角速度参数并依据一第一反馈数据，而计算出该移动载体目前在该空间中与不同方向轴之间所夹的姿态角度；以及

一位置计算模块，耦接该姿态角计算模块，以依据这些姿态角度、多个加速度参数和一第二反馈数据，而计算该移动载体目前在该空间中的相对位置坐标值，以当作一定位信息输出，其中这些角速度参数是该移动载体在该空间中运动时在不同方向轴上的角速度。

11.如权利要求10所述的空间感测装置，其中该姿态角计算模块包括：

一四元素运算单元，接收这些角速度参数和该第一反馈数据，以将这些角速度参数转换成多个即时四元素运算元；以及

一方向余弦运算单元，耦接该四元素运算单元，以依据这些即时四元素运算元和该第一反馈数据而计算出这些姿态角度。

12.如权利要求10所述的空间感测装置，其中该位置计算模块包括：

一加速度运算单元，耦接该姿态角计算模块，以依据这些姿态角度，并将重力加速度的因素从这些加速度参数中抽离，而计算出该移动载体在各方向上的加速度分量；

一加速度积分器，耦接该加速度运算单元，并接收这些角速度参数，以依据该第二反馈数据而将这些重力加速度分量进行积分运算，并获得该移动载体在不同方向上的加速度分量；

一速度积分器，耦接该加速度积分器，以依据该第二反馈数据而将这些速度分量进行积分运算，并获得该移动载体在不同方向的位移值；以及

一坐标转换运算单元，耦接该速度积分器，以依据这些位移值计算出该移动载体在该空间中的相对位置坐标值，并当作该定位信息。

13.如权利要求10所述的空间感测装置，其中该移动载体具有一声纳装置，用来发射一声纳波，且在该声纳波被物体反射后接收被反射后的声纳波，以获得一环境信息。

14.如权利要求13所述的空间感测装置，还包括一运算处理系统，耦接该位置计算模块和该声纳装置，以依据这些相对位置坐标值和该环境信息而产生一即时运算信息。

15.如权利要求14所述的空间感测装置，其中该运算处理系统包括：

一地图比对模块，耦接该位置计算模块，并具有该移动载体所在的空间的地图模型，以依据该定位信息产生一地图坐标数据；以及

一数据比对模块，耦接该地图比对模块、该机械波收发装置和该控制系统，以比对该地图坐标数据和该环境信息进行比对，并产生一比较值。

16.一种移动载体在空间中的控制方法，包括下列步骤：

检测该移动载体在该空间中的动向，并依据检测的结果而对该移动载体定位，而产生一定位信息；

从该移动载体向该空间发出一机械波，并接收被物体反射后的机械波，以获得一环境信息；以及

对该定位信息和该环境信息进行运算，以控制该移动载体在该空间动向。

17.如权利要求16所述的控制方法，其中产生该定位信息的步骤，包括下列步骤：

检测该移动载体在该空间的不同方向轴上的角速度，并依据一第一反馈数据而获得该移动载体目前在该空间中的姿态角；

检测该移动载体在该空间不同方向轴上的加速度，并产生多个加速度参数；

依据这些姿态角，并将重力加速度的因素从这些加速度参数中抽离，而计算出该移动载体在该空间的不同方向轴上的加速度分量；

依据这些角速度参数和一第二反馈数据，而将这些加速度分量积分，而获得该移动载体在该空间中不同方向上的速度分量；

依据该第二反馈数据而将这些速度分量积分，并获得该移动载体在该空间中不同方向上的位移值；以及

依据这些位移值而计算出该移动载体在该空间中的相对位置坐标值，以当作该定位信息。

18.如权利要求16所述的控制方法，其中该机械波为声纳波。

Images