CN103438904A - 一种使用视觉辅助校正的惯性定位方法及*** - Google Patents

Abstract

本发明公开了使用视觉辅助校正的惯性定位方法及***。该***包括惯性传感器、用于标识定位点的定位标识、用于获取定位标识的图像信息的摄像模块、图像处理单元和校正单元。图像处理单元根据摄像模块产生的图像信息计算空间位置信息，并将空间位置信息传输至校正单元，校正单元根据空间位置信息对惯性传感器的感测数据进行校正。该方法通过设置分开的摄像模块和定位标识；摄像模块设置在校正端、并且定位标识设置在惯性传感器端，或者定位标识设置在校正端、并且摄像模块设置在惯性传感器端；利用摄像模块拍摄定位标识，并根据拍摄结果计算得到校正数据；利用校正数据对惯性传感器的传感数据进行校正；具有校正时间短、校正准确、耗电量少等优点。

Description

一种使用视觉辅助校正的惯性定位方法及***

技术领域

本发明涉及惯性定位方法及***，尤其涉及使用视觉辅助校正的惯性定位方法及***。

背景技术

目前惯性传感器在空间定位方面的广泛应用，使我们也看到了在应用方面所存在的明显问题，最大的问题是一个已经做过校正定位3维空间中的一个点，在经过移动后这一个点的定位及定向信息就不正确了，使用者若需要取得精准的定位指向信息必需重新再按下校正键，当使用者不断的在移动时，需要不断的反复人为校正才能得到指那打那的效果。这个问题所引起的不便利性严重的打击了使用者的体验!

发明内容

本发明要解决的技术问题在于，提供一种校正简单、耗电少的、使用视觉辅助校正的惯性定位方法及***。

本发明解决其技术问题所采用的技术方案是：提供一种使用视觉辅助校正的惯性定位方法，包括以下步骤：

S1：设置分开的摄像模块和定位标识；所述摄像模块设置在校正端、并且所述定位标识设置在惯性传感器端，或者所述定位标识设置在校正端、并且所述摄像模块设置在惯性传感器端；

S2：利用所述摄像模块拍摄所述定位标识，并根据拍摄结果计算得到校正数据；利用所述校正数据对所述惯性传感器的传感数据进行校正。

在本发明使用视觉辅助校正的惯性定位方法的所述步骤S1中，所述摄像模块包括两个具有固定距离的摄像装置；所述定位标识包括一个或多个定位标识。

在本发明使用视觉辅助校正的惯性定位方法的所述步骤S2中，包括：

S2-1：基准设定步骤；

S2-2：校正步骤；

该基准设定步骤包括：

S2-1-1：将所述摄像装置和定位标识分开设置在已知的相对位置处，并量测得到距离数据；

S2-1-2：在该相对位置处，利用所述摄像装置拍摄获取所述定位标识在该相对位置处的图像信息，并将该图像信息转化为起始位置数据；

S2-1-3：根据所述起始位置数据、距离数据以及所述摄像装置之间的固定距离数据，计算得到所述摄像模块的基准数据；

所述校正步骤包括：

S2-2-1：利用所述摄像装置拍摄获取所述定位标识在任意校正位置的图像信息，并将该图像信息转化为校正位置数据；

S2-2-2：根据所述校正位置数据、摄像模块的基准数据、摄像装置之间的固定距离数据计算得到定位标识实际位置数据，并根据所述实际位置数据对所述惯性传感器端的的位置感测数据进行数据融合校正。

在本发明使用视觉辅助校正的惯性定位方法的所述步骤S2-1-2中，所述起始位置数据包括将所述相对位置的图像信息转换成的起始位置坐标；

S2-1-3：根据所述起始位置坐标、距离数据以及所述摄像装置之间的固定距离数据，计算得到所述摄像模块的基准数据；

在所述步骤S2-2-1中，所述校正位置数据包括将所述校正位置的图像信息转换成校正位置坐标；

在所述步骤S2-2-2中，所述定位标识的实际位置数据包括在所述校正位置的所述两个摄像装置与所述定位标识的连成直线的夹角、所述摄像装置与所述定位标识连成直线的矢量大小、以及计算出来的所述定位标识的实际位置坐标。

在本发明使用视觉辅助校正的惯性定位方法的所述步骤S2中，包括：

S2-1：在起始位置，利用所述摄像装置获取所述定位标识在该起始位置的图像信息，并将该图像信息转化为起始位置数据；

S2-2：在任意校正位置，利用所述摄像装置获取所述定位标识在该任意校正位置的图像信息，并将该图像信息转化为校正位置数据；

S2-3：将所述校正位置数据与所述起始位置数据进行计算，得到所述惯性传感器从所述起始位置移动到所述任意校正位置的位移修正数据；

S2-4：利用所述惯性传感器感测其从所述起始位置移动到所述任意校正位置的位移，获得实测位移数据；

S2-5：将所述位移修正数据与所述实测位移数据进行数据融合，校正所述惯性传感器的感测数据。

在本发明使用视觉辅助校正的惯性定位方法的所述步骤S2-1中，所述起始位置数据包括将所述起始位置的图像信息转换成起始位置坐标、并结合所述摄像装置的基准数据及摄像装置之间的固定距离数据计算得到所述起始位置的所述两个摄像装置与所述定位标识的连成直线的夹角、所述摄像装置与所述定位标识连成直线的矢量大小；

在所述步骤S2-2中，所述校正位置数据包括将所述校正位置的图像信息转换成校正位置坐标、并结合所述两个摄像装置的基准数据及摄像装置之间的固定距离数据计算得到所述校正位置的所述两个摄像装置与所述定位标识的连成直线的夹角、所述摄像装置与所述定位标识连成直线的矢量大小。

在本发明使用视觉辅助校正的惯性定位方法的所述步骤S1中，所述摄像模块包括至少一个摄像装置；所述定位标识包括两个或多个具有固定相对距离的定位标识。

在本发明使用视觉辅助校正的惯性定位方法的所述步骤S2中，包括：

S2-1：基准设定步骤；

S2-2：校正步骤；

该基准设定步骤包括：

S2-1-1：将所述摄像装置和定位标识分开设置在已知的相对位置处，并量测得到距离数据；

S2-1-2：在该相对位置处，利用所述摄像装置拍摄获取所述定位标识在该相对位置处的图像信息，并将该图像信息转化起始位置数据；

S2-1-3：利用惯性传感器端的惯性传感器感测在该相对位置处的起始姿态角信息；

S2-1-4：根据所述起始姿态角信息、所述起始位置数据、距离数据以及所述定位标识之间的固定距离数据，计算所述摄像模块的基准数据；

所述校正步骤包括：

S2-2-1：在任意校正位置，利用所述惯性传感器获得校正位置姿态角信息；

S2-2-2：利用所述摄像装置拍摄获取所述定位标识在该任意校正位置的图像信息，并将该图像信息转化为校正位置数据；

S2-2-3：根据所述校正位置姿态角信息、校正位置数据、摄像模块的基准数据、定位标识之间的固定距离数据计算得到实际位置数据，并根据所述实际位置数据对所述惯性传感器端的的感测数据进行数据融合校正。

在本发明使用视觉辅助校正的惯性定位方法的所述步骤S2中，包括：

S2-1：在起始位置，利用所述摄像装置获取所述定位标识在该起始位置的图像信息，并结合惯性传感器的初始姿态角信息得到起始位置数据；

S2-2：在任意校正位置，利用所述摄像装置获取所述定位标识在该任意校正位置的图像信息，并结合惯性传感器的校正位置姿态角信息得到校正位置数据；

S2-3：将所述校正位置数据与所述起始位置数据进行计算，得到所述惯性传感器从所述起始位置移动到所述任意校正位置的位移修正数据；

S2-4：利用所述惯性传感器感测其从所述起始位置移动到所述任意校正位置的位移，获得实测位移数据；

S2-5：将所述位移修正数据和所述实测位移数据进行数据融合，校正所述惯性传感器的感测数据。

本发明还提供一种使用视觉辅助校正的惯性定位***，包括惯性传感器，该***还包括：用于标识定位点的定位标识、用于获取所述定位标识的图像信息的摄像模块、图像处理单元和校正单元；

所述摄像模块设置于校正端、并且所述定位标识设置在惯性传感器端，所述校正单元通过无线或有线方式与所述惯性传感器通讯连接，或者所述定位标识设置在校正端、并且所述摄像模块设置在惯性传感器端，所述校正单元与所述惯性传感器通过无线或有线方式通讯连接；

所述图像处理单元根据所述摄像模块产生的图像信息计算空间位置信息，并将所述空间位置信息传输至所述校正单元，所述校正单元根据所述位置校正信息对所述惯性传感器的感测数据进行校正。

在本发明的使用视觉辅助校正的惯性定位***中，所述摄像模块包括两个具有固定距离的摄像装置，所述定位标识包括一个或多个定位标识；或者

所述摄像模块包括至少一个摄像装置；所述定位标识包括两个或多个具有固定相对距离的定位标识。

实施本发明带来的有益效果是：通过利用摄像模块拍摄分开设置的定位标识，来计算得到校正数据，实现对惯性传感器的传感数据的校正，具有校正时间短、校正准确、耗电量少等优点。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是本发明使用视觉辅助校正的惯性定位***的第一实施例的示意框体；

图2是本发明使用视觉辅助校正的惯性定位方法的第一实施例的基准设定步骤的原理示意图；

图3是本发明使用视觉辅助校正的惯性定位方法的第一实施例的校正步骤的原理示意图；

图4是本发明使用视觉辅助校正的惯性定位***的第三实施例的示意框体；

图5是本发明使用视觉辅助校正的惯性定位方法的第三实施例的基准设定步骤的原理示意图；

图6是本发明使用视觉辅助校正的惯性定位方法的第三实施例的校正步骤的原理示意图。

具体实施方式

为了对本发明的技术特征、目的和效果有更加清楚的理解，现对照附图详细说明本发明的具体实施方式。

本发明的使用视觉辅助校正的惯性定位方法及***，通过设置分开的摄像模块和定位标识；其中，摄像模块设置在校正端、并且定位标识设置在惯性传感器端，或者定位标识设置在校正端、并且摄像模块设置在惯性传感器端。然后，利用摄像模块拍摄定位标识，并根据拍摄结果计算得到校正数据；利用校正数据对惯性传感器的传感数据进行校正,可以在消耗很少电量的情况下实现对惯性定位装置中的惯性传感器的误差校正。

如图1-3所示，是本发明的使用视觉辅助校正的惯性定位***及方法的第一实施例。

该使用视觉辅助校正的惯性定位***，包括惯性传感器21、用于标识定位点的定位标识22、用于获取定位标识22的图像信息的摄像模块、图像处理单元12和校正单元13。

在本实施例中，摄像模块设置于校正端10，包括至少两个具有固定距离的摄像装置11；当然，摄像装置11的数量还可以更多。该校正端10可以为显示屏、射击荧幕、电视、游戏主机等各种固定设置的装置；而摄像装置11可以采用CMOS定位传感器或其他类型的摄像设备，用于捕捉定位标识22的图像。

该定位标识22设置在惯性传感器端20，来标识惯性传感器端20的位置。在本实施例中，该惯性传感器端20为游戏枪、三维鼠标等，该定位标识22为一个，设置于惯性传感器端20的前方，供摄像模块进行拍摄捕捉。该定位标识22可以为红外LED灯，当然，也可以选用其他的标识，例如反光贴、色差较大的贴纸等，能够便于在摄像模块拍摄的图像中被识别即可。

可以理解的，该定位标识22的数量也可以为两个或多个，通过识别不同定位标识22的ID信息，来依次得到定位标识22的位置。例如，通过控制LED灯分开亮起的时间,或控制LED光线闪烁明暗,使成能带有信息的光波(光波也是电磁波),带有个别ID讯息,以供识别。

该图像处理单元12和校正单元13同时设置在校正端10，图像处理单元12根据摄像模块产生的图像信息计算定位标识22的空间位置信息，并将空间位置信息传送至校正单元13，然后有校正单元13对空间位置信息进行处理，得到位置校正信息，并对惯性传感器21的感测数据进行校正。该校正单元13与惯性传感器21之间可以通过无线或有线的方式连接，从而将惯性传感器21的感测数据进行校正。

在使用视觉辅助校正进行惯性定位时，首先需要进行基准设定，然后再进行校正步骤。

执行该基准设定步骤时，将摄像装置11和定位标识22分开设置在已知的相对位置处，并量测得到摄像装置11与定位标识22之间的距离数据；利用摄像装置11拍摄获取定位标识22在该相对位置处的图像信息，并将该图像信息转化为起始位置数据；并根据起始位置数据、距离数据以及两摄像装置11之间的固定距离数据计算得到摄像模块的基准数据，并以该基准数据作为校正基准。

例如，如图2所示，站在2米距离（即距离数据）用带定位标识22（图中B点所指，如红外LED）的惯性传感器端20正对两摄像装置11（图中A点所指）连线的中间位置，通过摄像装置11拍摄2米距离的定位标识22的图像信息，并将该图像信息转换成起始位置数据。依照光沿直线传播的原理，可以通过相似三角形的边长成比例的方法，利用以下公式进行计算，以得到摄像模块的基准数据：

$\frac{{\mathrm{Hx}}_1}{h_1}+\frac{{\mathrm{Hx}}_2}{h_2}=L$

$\tan \mathrm{\α}=\frac{x_1}{h_1},\tan \mathrm{\β}=\frac{x_2}{h_2}$

其中，H为距离数据（可预先测量得到），L为两个摄像装置11之间的固定距离数据（可预先测量得到），x₁及x₂为起始位置数据（可通过摄像装置11拍摄投影得到），h₁和h₂分别为需要求得的基准数据，以该基准数据作为两摄像装置11的基准数据。可以理解的，该基准数据可以包括水平方向的、垂直方向的或其他方向的，以供后续校正步骤使用。

可以理解的，还可以在多个位置再次测定基准数据：将摄像装置11和定位标识22分开设置在第二相对位置处，并得到第二距离数据；利用摄像装置11拍摄获取定位标识22在该第二相对位置处的图像信息，并将该图像信息转化为第二起始位置数据；将第二起始位置数据、第二距离数据以及摄像装置11之间的固定距离数据计算得到摄像模块的第二基准数据，并将所述第二基准数据与基准数据融合得到基准数据。例如，在3米的位置再次校正，利用第一次的方法，从而得到更加准确的基准数据。

当传感数据发生偏差，造成传感器端的指向不准确时，可以通过传感器端的触发按键、***自动启动或其他方式触发，启动校正。

当校正启动时，利用摄像装置11拍摄获取定位标识22在任意校正位置的图像信息，并将该图像信息转化为校正位置数据；然后，根据校正位置数据、摄像模块的基准数据、摄像装置11之间的固定距离数据等，计算得到两个摄像装置11与定位标识22的连成直线的夹角、摄像装置11与定位标识22连成直线的矢量大小、计算出来的定位标识的实际位置坐标等，而得到定位标识22（即惯性传感器端20）的实际位置数据，并利用该实际位置数据对该校正位置的感测数据进行校正。

例如，如图3所示，当惯性传感器端20位于任意校正位置时，图中A指示为两摄像装置11的位置、B为定位标识的位置，可以根据以下公式：

$\frac{{\mathrm{Hx}}_1}{h_1}+\frac{{\mathrm{Hx}}_2}{h_2}=L$

$\tan \mathrm{\α}=\frac{x_1}{h_1},\tan \mathrm{\β}=\frac{x_2}{h_2}$

其中，H即为所求的距离（即惯性传感器端20与摄像装置11的距离），h₁和h₂分别为基准校正部分得到的摄像装置11的基准参数，x₁及x₂为通过摄像装置11拍摄并处理得到的定位标识22的校正位置数据；L为两个摄像装置11之间的固定距离数据，可预先测量。

利用以上两个公式可分别求得惯性传感器端20与摄像装置11的距离以及空间方位角，从而得到惯性传感器端20的实际位置数据，并根据该实际位置数据对惯性传感器21所感测的感测数据进行校正。实际位置数据对惯性传感器21的感测数据进行校正可以采用卡尔曼滤波、扩展卡尔曼滤波、最小二乘法等方法，对实际位置数据和感测数据融合后进行滤波计算得到。

可以理解的，除了垂直距离H之外，定位标识22分别与两个摄像装置11的水平距离，高度差也可依照类似比例关系求得，选定空间中的坐标原点是可以定出定位标识22的空间三维坐标的。

在本发明的使用视觉辅助校正的惯性定位***及方法的第二实施例中，通过融合起始位置和校正位置的数据，来实现传感数据的校正。

与上一实施例类似，该***同样包括惯性传感器21、用于标识定位点的定位标识22、用于获取定位标识22的图像信息的摄像模块、图像处理单元12和校正单元13等。摄像模块包括两个具有固定距离的摄像装置11，固定设置在校正端10；而定位标识22为一个或多个，固定在惯性传感器端20。

在起始位置，利用摄像装置11获取定位标识22在该起始位置的图像信息，并将该图像信息转化为起始位置数据。该起始位置数据包括将起始位置的图像信息转换成起始位置坐标、并结合摄像装置11的基准数据及固定距离数据计算得到起始位置的两个摄像装置11与定位标识22的连成直线的夹角、摄像装置11与定位标识22连成直线的矢量大小等。

然后，将惯性传感器端20移动到任意位置，作为校正位置，利用摄像装置11获取定位标识22在该任意校正位置的图像信息，并将该图像信息转化为校正位置数据。同样的，该校正位置数据包括将校正位置的图像信息转换成校正位置坐标、并结合两个摄像装置11的基准数据及固定距离数据计算得到校正位置的两个摄像装置11与定位标识22的连成直线的夹角、摄像装置11与定位标识22连成直线的矢量大小等。

然后，将得到的校正位置数据与起始位置数据进行计算，得到惯性传感器21从起始位置移动到任意校正位置的位移修正数据。例如，对惯性传感器端20从起始位置移动到校正位置，相对于摄像模块的夹角差、矢量差等数据，利用数字滤波器进行数字滤波，计算得到位移修正数据。可以理解的，数字滤波可以为卡尔曼滤波、扩展形卡尔曼滤波、最小二乘法、加权平均数等。

同时，惯性传感器端20从起始位置移动到校正位置的过程中，利用惯性传感器21进行感测，获得从起始位置移动到任意校正位置的实测位移数据。同样的，可以利用数字滤波器对实测位移数据进行数字滤波，数字滤波可以为卡尔曼滤波、扩展形卡尔曼滤波、最小二乘法、加权平均数等。

然后，将得到的位移修正数据与实测位移数据进行数据融合，消除累积漂移，校正惯性传感器21的感测数据，以实现惯性传感器端20的精确定位。

可以理解的，在本实施例中，可以采用第一实施例的方法，得到两摄像装置11的基准数据，在此不作赘述。

如图4所示，是本发明的使用视觉辅助校正的惯性定位***及方法的第三实施例。该***包括惯性传感器41、用于标识定位点的定位标识31、用于获取定位标识31的图像信息的摄像模块、图像处理单元32和校正单元33等，与第一实施例基本相同，故不赘述。

在本实施例中，摄像模块包括至少一个摄像装置42，固定安装在惯性传感器端40，而定位标识31包括两个或多个，并且以固定相对距离设置在校正端30。

该方法同样包括基准设定步骤以及校正步骤。执行该基准设定步骤时，将摄像装置42和定位标识31分开设置在已知的相对位置处，并量测得到摄像装置42与定位标识31之间的距离数据；利用摄像装置42拍摄获取定位标识31在该相对位置处的图像信息，并将该图像信息转化为起始位置数据；同时，利用惯性传感器端40的惯性传感器41感测在该相对位置处的起始姿态角信息。然后，根据起始姿态角信息、起始位置数据、距离数据以及定位标识31之间的固定距离数据，计算得到摄像模块的基准数据，并以该基准数据作为校正基准。

例如，如图5所示，站在2米距离（即距离数据）用带摄像装置42（图中A所指位置）的惯性传感器端40正对两定位标识31（如红外LED，图中B所指位置）连线的中间位置，通过摄像装置42拍摄2米距离的定位标识31的图像信息，并将该图像信息转换成起始位置数据。依照光沿直线传播的原理，可以通过相似三角形的边长成比例的方法，利用以下公式进行计算，以得到摄像模块的基准数据：

$h=\frac{\mathrm{LH}}{x_1+x_2}$

$\tan \mathrm{\α}=\frac{x_1}{h},\tan \mathrm{\β}=\frac{x_2}{h}$

其中，H为距离数据（可预先测量得到），L为两个定位标识31之间的固定距离数据（可预先测量得到），x₁及x₂为起始位置数据（可通过摄像装置42拍摄投影转换得到），h分别为需要求得的基准数据，以该基准数据作为摄像装置42的基准数据。可以理解的，该基准数据可以包括水平方向的、垂直方向的或其他方向的，以供后续校正步骤使用。

可以理解的，还可以在多个位置再次测定基准数据：将摄像装置42和定位标识31分开设置在第二相对位置处，并得到第二距离数据；利用摄像装置42拍摄获取定位标识31在该第二相对位置处的图像信息，并将该图像信息转化为第二起始位置数据；将第二起始位置数据、第二距离数据以及定位标识31之间的固定距离数据计算得到摄像模块的第二基准数据，并将第二基准数据与基准数据融合得到基准数据。例如，在3米的位置再次校正，利用第一次的方法，从而得到更加准确的基准数据。

当传感数据发生偏差，造成传感器端的指向不准确时，可以通过传感器端的触发按键、***自动启动或其他方式触发，启动校正。

当校正启动时，利用惯性传感器41获得校正位置姿态角信息；并且利用摄像装置42拍摄获取定位标识31在任意校正位置的图像信息，并将该图像信息转化为校正位置数据；然后，根据校正位置姿态角信息、校正位置数据、摄像装置42的基准数据、定位标识31之间的固定距离数据等，计算得到摄像装置42与两定位标识31的连成直线的夹角、摄像装置42与定位标识31连成直线的矢量大小、计算出来的摄像装置的实际位置坐标等，而得到摄像装置42（即惯性传感器端40）的实际位置数据，并利用该实际位置数据对该校正位置的感测数据进行校正。

例如，如图6所示，当惯性传感器端40位于任意校正位置时，图中，A所指位置为摄像装置42的位置，B所示为两定位标识31的位置，可以根据以下公式：

$H=\frac{\mathrm{Lh}}{x_1+x_2}$

$\tan \mathrm{\α}=\frac{x_1}{h},\tan \mathrm{\β}=\frac{x_2}{h}$

其中，H即为所求的距离（即惯性传感器端40与定位标识31的距离），h为基准校正部分得到的摄像装置42的基准参数，x₁及x₂为通过摄像装置42拍摄并处理得到的定位标识31的校正位置数据；L为两个定位标识31之间的固定距离，可预先测量。

利用以上两个公式可分别求得惯性传感器端40与定位标识31的距离以及空间方位角，从而得到惯性传感器端40的实际位置数据，并根据该实际位置数据对惯性传感器41所感测的感测数据进行校正。实际位置数据对惯性传感器41的感测数据进行校正可以采用卡尔曼滤波、扩展卡尔曼滤波、最小二乘法等方法，对实际位置数据和感测数据融合后进行滤波计算得到。

可以理解的，除了垂直距离H之外，定位标识31分别与两个摄像装置42的水平距离，高度差也可依照类似比例关系求得，选定空间中的坐标原点是可以定出定位标识31的空间三维坐标的。

在本发明的使用视觉辅助校正的惯性定位方法的第四实施例中，通过融合起始位置和校正位置的数据，来实现传感数据的校正。

与上一实施例类似，定位标识31包括两个具有固定距离的定位标识31，固定设置在校正端30；而摄像装置42为一个或多个，固定在惯性传感器端40。

在起始位置，利用摄像装置42获取定位标识31在该起始位置的图像信息，并结合惯性传感器41的初始姿态角信息得到起始位置数据。该起始位置数据包括将起始位置的图像信息转换成起始位置坐标、并结合摄像装置42的基准数据及定位标识31的固定距离数据计算得到起始位置的摄像装置42与两定位标识31的连成直线的夹角、摄像装置42与定位标识31连成直线的矢量大小等。

然后，将惯性传感器端40移动到任意位置，作为校正位置，利用摄像装置42获取定位标识31在该任意校正位置的图像信息，并结合惯性传感器41的校正位置姿态角信息得到校正位置数据。同样的，该校正位置数据包括将校正位置的图像信息转换成校正位置坐标、并结合摄像装置42的基准数据及定位标识31的固定距离数据计算得到校正位置的摄像装置42与两定位标识31的连成直线的夹角、摄像装置42与定位标识31连成直线的矢量大小等。

然后，将得到的校正位置数据与起始位置数据进行计算，得到惯性传感器41从起始位置移动到任意校正位置的位移修正数据。例如，对惯性传感器端40从起始位置移动到校正位置，相对于定位标识31的夹角差、矢量差等数据，利用数字滤波器进行数字滤波，计算得到位移修正数据。可以理解的，数字滤波可以为卡尔曼滤波、扩展形卡尔曼滤波、最小二乘法、加权平均数等。

同时，惯性传感器端40从起始位置移动到校正位置的过程中，利用惯性传感器41进行感测，获得从起始位置移动到任意校正位置的实测位移数据。同样的，可以利用数字滤波器对实测位移数据进行数字滤波，数字滤波可以为卡尔曼滤波、扩展形卡尔曼滤波、最小二乘法、加权平均数等。

然后，将得到的位移修正数据与实测位移数据进行数据融合，消除累积漂移，校正惯性传感器41的感测数据，以实现惯性传感器端40的精确定位。

可以理解的，在本实施例中，可以采用第一实施例的方法，得到两摄像装置42的基准数据，在此不作赘述。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的权利要求范围之内。

Claims (11)

1.一种使用视觉辅助校正的惯性定位方法，其特征在于，包括以下步骤：

S1：设置分开的摄像模块和定位标识；所述摄像模块设置在校正端、并且所述定位标识设置在惯性传感器端，或者所述定位标识设置在校正端、并且所述摄像模块设置在惯性传感器端；

S2：利用所述摄像模块拍摄所述定位标识，并根据拍摄结果计算得到校正数据；利用所述校正数据对所述惯性传感器的传感数据进行校正。

2.根据权利要求1所述的使用视觉辅助校正的惯性定位方法，其特征在于，在所述步骤S1中，所述摄像模块包括两个具有固定距离的摄像装置；所述定位标识包括一个或多个定位标识。

3.根据权利要求2所述的使用视觉辅助校正的惯性定位方法，其特征在于，在所述步骤S2中，包括：

S2-1：基准设定步骤；

S2-2：校正步骤；

该基准设定步骤包括：

S2-1-1：将所述摄像装置和定位标识分开设置在已知的相对位置处，并量测得到距离数据；

S2-1-2：在该相对位置处，利用所述摄像装置拍摄获取所述定位标识在该相对位置处的图像信息，并将该图像信息转化为起始位置数据；

S2-1-3：根据所述起始位置数据、距离数据以及所述摄像装置之间的固定距离数据，计算得到所述摄像模块的基准数据；

所述校正步骤包括：

S2-2-1：利用所述摄像装置拍摄获取所述定位标识在任意校正位置的图像信息，并将该图像信息转化为校正位置数据；

S2-2-2：根据所述校正位置数据、摄像模块的基准数据、摄像装置之间的固定距离数据计算得到定位标识实际位置数据，并根据所述实际位置数据对所述惯性传感器端的的位置感测数据进行数据融合校正。

4.根据权利要求3所述的使用视觉辅助校正的惯性定位方法，其特征在于，在所述步骤S2-1-2 中，所述起始位置数据包括将所述相对位置的图像信息转换成的起始位置坐标；S2-1-3：根据所述起始位置坐标、距离数据以及所述摄像装置之间的固定距离数据，计算得到所述摄像模块的基准数据；

在所述步骤S2-2-1中，所述校正位置数据包括将所述校正位置的图像信息转换成校正位置坐标；

在所述步骤S2-2-2中，所述定位标识的实际位置数据包括在所述校正位置的所述两个摄像装置与所述定位标识的连成直线的夹角、所述摄像装置与所述定位标识连成直线的矢量大小、以及计算出来的所述定位标识的实际位置坐标。

5.根据权利要求2所述的使用视觉辅助校正的惯性定位方法，其特征在于，在所述步骤S2中，包括：

S2-1：在起始位置，利用所述摄像装置获取所述定位标识在该起始位置的图像信息，并将该图像信息转化为起始位置数据；

S2-2：在任意校正位置，利用所述摄像装置获取所述定位标识在该任意校正位置的图像信息，并将该图像信息转化为校正位置数据；

S2-3：将所述校正位置数据与所述起始位置数据进行计算，得到所述惯性传感器从所述起始位置移动到所述任意校正位置的位移修正数据；

S2-4：利用所述惯性传感器感测其从所述起始位置移动到所述任意校正位置的位移，获得实测位移数据； 

S2-5：将所述位移修正数据与所述实测位移数据进行数据融合，校正所述惯性传感器的感测数据。

6.根据权利要求5所述的使用视觉辅助校正的惯性定位方法，其特征在于， 在所述步骤S2-1中，所述起始位置数据包括将所述起始位置的图像信息转换成起始位置坐标、并结合所述摄像装置的基准数据及摄像装置之间的固定距离数据计算得到所述起始位置的所述两个摄像装置与所述定位标识的连成直线的夹角、所述摄像装置与所述定位标识连成直线的矢量大小；

在所述步骤S2-2中，所述校正位置数据包括将所述校正位置的图像信息转换成校正位置坐标、并结合所述两个摄像装置的基准数据及摄像装置之间的固定距离数据计算得到所述校正位置的所述两个摄像装置与所述定位标识的连成直线的夹角、所述摄像装置与所述定位标识连成直线的矢量大小。

7.根据权利要求1所述的使用视觉辅助校正的惯性定位方法，其特征在于，在所述步骤S1中，所述摄像模块包括至少一个摄像装置；所述定位标识包括两个或多个具有固定相对距离的定位标识。

8.根据权利要求7所述的使用视觉辅助校正的惯性定位方法，其特征在于，在所述步骤S2中，包括：

S2-1：基准设定步骤；

S2-2：校正步骤；

该基准设定步骤包括：

S2-1-1：将所述摄像装置和定位标识分开设置在已知的相对位置处，并量测得到距离数据；

S2-1-2：在该相对位置处，利用所述摄像装置拍摄获取所述定位标识在该相对位置处的图像信息，并将该图像信息转化起始位置数据；

S2-1-3：利用惯性传感器端的惯性传感器感测在该相对位置处的起始姿态角信息；

S2-1-4：根据所述起始姿态角信息、所述起始位置数据、距离数据以及所述定位标识之间的固定距离数据，计算所述摄像模块的基准数据；

所述校正步骤包括：

S2-2-1：在任意校正位置，利用所述惯性传感器获得校正位置姿态角信息；

S2-2-2：利用所述摄像装置拍摄获取所述定位标识在该任意校正位置的图像信息，并将该图像信息转化为校正位置数据；

S2-2-3：根据所述校正位置姿态角信息、校正位置数据、摄像模块的基准数据、定位标识之间的固定距离数据计算得到实际位置数据，并根据所述实际位置数据对所述惯性传感器端的的感测数据进行数据融合校正。

9.根据权利要求7所述的使用视觉辅助校正的惯性定位方法，其特征在于，在所述步骤S2中，包括：

S2-1：在起始位置，利用所述摄像装置获取所述定位标识在该起始位置的图像信息，并结合惯性传感器的初始姿态角信息得到起始位置数据；

S2-2：在任意校正位置，利用所述摄像装置获取所述定位标识在该任意校正位置的图像信息，并结合惯性传感器的校正位置姿态角信息得到校正位置数据；

S2-3：将所述校正位置数据与所述起始位置数据进行计算，得到所述惯性传感器从所述起始位置移动到所述任意校正位置的位移修正数据；

S2-4：利用所述惯性传感器感测其从所述起始位置移动到所述任意校正位置的位移，获得实测位移数据；

S2-5：将所述位移修正数据和所述实测位移数据进行数据融合，校正所述惯性传感器的感测数据。

10.一种使用视觉辅助校正的惯性定位***，包括惯性传感器，其特征在于，

还包括：用于标识定位点的定位标识、用于获取所述定位标识的图像信息的摄像模块、图像处理单元和校正单元；

所述摄像模块设置于校正端、并且所述定位标识设置在惯性传感器端，所述校正单元通过无线或有线方式与所述惯性传感器通讯连接，或者所述定位标识设置在校正端、并且所述摄像模块设置在惯性传感器端，所述校正单元与所述惯性传感器通过无线或有线方式通讯连接；

所述图像处理单元根据所述摄像模块产生的图像信息计算空间位置信息，并将所述空间位置信息传输至所述校正单元，所述校正单元根据所述位置校正信息对所述惯性传感器的感测数据进行校正。

11.根据权利要求10所述的使用视觉辅助校正的惯性定位***，其特征在于，所述摄像模块包括两个具有固定距离的摄像装置，所述定位标识包括一个或多个定位标识；或者

所述摄像模块包括至少一个摄像装置；所述定位标识包括两个或多个具有固定相对距离的定位标识。

Images