CN114403760A - 可移动载体的定位方法、装置和扫地机器人 - Google Patents

可移动载体的定位方法、装置和扫地机器人 Download PDF

Info

Publication number
CN114403760A
CN114403760A CN202111578633.7A CN202111578633A CN114403760A CN 114403760 A CN114403760 A CN 114403760A CN 202111578633 A CN202111578633 A CN 202111578633A CN 114403760 A CN114403760 A CN 114403760A
Authority
CN
China
Prior art keywords
parameter
distance
deltay
centers
driving wheels
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202111578633.7A
Other languages
English (en)
Other versions
CN114403760B (zh
Inventor
聂宏
李照龙
牟荣胜
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tianjin Sigma Microelectric Technique Co ltd
Original Assignee
Tianjin Sigma Microelectric Technique Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Tianjin Sigma Microelectric Technique Co ltd filed Critical Tianjin Sigma Microelectric Technique Co ltd
Priority to CN202111578633.7A priority Critical patent/CN114403760B/zh
Publication of CN114403760A publication Critical patent/CN114403760A/zh
Application granted granted Critical
Publication of CN114403760B publication Critical patent/CN114403760B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • A—HUMAN NECESSITIES
    • A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
    • A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
    • A47L11/00—Machines for cleaning floors, carpets, furniture, walls, or wall coverings
    • A47L11/40—Parts or details of machines not provided for in groups A47L11/02 - A47L11/38, or not restricted to one of these groups, e.g. handles, arrangements of switches, skirts, buffers, levers
    • A47L11/4011—Regulation of the cleaning machine by electric means; Control systems and remote control systems therefor
    • A—HUMAN NECESSITIES
    • A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
    • A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
    • A47L11/00—Machines for cleaning floors, carpets, furniture, walls, or wall coverings
    • A47L11/24—Floor-sweeping machines, motor-driven
    • A—HUMAN NECESSITIES
    • A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
    • A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
    • A47L11/00—Machines for cleaning floors, carpets, furniture, walls, or wall coverings
    • A47L11/40—Parts or details of machines not provided for in groups A47L11/02 - A47L11/38, or not restricted to one of these groups, e.g. handles, arrangements of switches, skirts, buffers, levers
    • A47L11/4002—Installations of electric equipment
    • A47L11/4008—Arrangements of switches, indicators or the like
    • A—HUMAN NECESSITIES
    • A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
    • A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
    • A47L11/00—Machines for cleaning floors, carpets, furniture, walls, or wall coverings
    • A47L11/40—Parts or details of machines not provided for in groups A47L11/02 - A47L11/38, or not restricted to one of these groups, e.g. handles, arrangements of switches, skirts, buffers, levers
    • A47L11/4063—Driving means; Transmission means therefor
    • A47L11/4066—Propulsion of the whole machine
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B11/00—Measuring arrangements characterised by the use of optical techniques
    • G01B11/002—Measuring arrangements characterised by the use of optical techniques for measuring two or more coordinates
    • A—HUMAN NECESSITIES
    • A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
    • A47L—DOMESTIC WASHING OR CLEANING; SUCTION CLEANERS IN GENERAL
    • A47L2201/00—Robotic cleaning machines, i.e. with automatic control of the travelling movement or the cleaning operation
    • A47L2201/04—Automatic control of the travelling movement; Automatic obstacle detection

Landscapes

  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本公开的实施例公开了一种可移动载体的定位方法、装置和扫地机器人。该方法中，根据旋转角度ΔαO(i)、移动距离ΔyO(i)、旋转角度ΔαM(i)、移动距离ΔyM(i)以及陀螺仪检测到的可移动载体的旋转角度ΔαT(i)得到两个驱动轮中心的连线中点的移动距离ΔyN(i)，移动距离ΔyN(i)的方向垂直于两个驱动轮中心的连线方向；根据移动距离ΔyN(i)计算两个驱动轮中心的连线中点在固定坐标系的坐标位置。该方法对移动距离ΔyN(i)的估计更加准确可靠。

Description

可移动载体的定位方法、装置和扫地机器人
技术领域
本公开涉及移动物体定位技术领域，尤其涉及一种可移动载体的定位方法、装置和扫地机器人。
背景技术
本部分旨在为权利要求书陈述的实施方式提供背景或上下文。此处的描述不因为包括在本部分中就承认是现有技术。
为了高效清扫作业范围内的全部区域，实现既不漏扫某些区域，又不重复清扫某些区域，扫地机器人需要能够准确检测其运动时的移动距离。码表，陀螺仪及光学位移检测装置是扫地机器人常用的位移检测传感器。扫地机器人需要有效融合多个位移检测传感器的输出数据，得到精确可靠的位移数据。如何高效准确地融合这些位移检查传感器的输出数据，是本领域技术人员持续研究的技术问题。
发明内容
本公开提供一种可移动载体的定位方法、装置和扫地机器人。
本公开提供的技术方案如下。
第一方面，本公开的实施例提供一种可移动载体的定位方法，在所述可移动载体上安装有光学位移检测装置、左码表、右码表和陀螺仪，所述可移动载体具有两个驱动轮，所述光学位移检测装置在所述可移动载体所处平面的正投影与所述两个驱动轮中心的连线所在直线在所述可移动载体所处平面的正投影不重叠，所述左码表用于确定所述两个驱动轮中左驱动轮在第i个时间间隔内的移动距离ΔSL(i)，所述右码表用于确定所述两个驱动轮中右驱动轮在第i个时间间隔内的移动距离ΔSR(i)，所述定位方法包括：
在所述可移动载体处于运动状态时，获得在第i个时间间隔内的所述光学位移检测装置在第一方向上的移动距离Δx(i)和在第二方向上的移动距离Δy(i)，所述第一方向和所述第二方向分别为所述光学位移检测装置的坐标系的两个坐标轴的正方向，所述可移动载体旋转动作的旋转轴垂直于所述可移动载体所处平面，所述第一方向和所述第二方向彼此垂直且平行于所述可移动载体所处平面；
根据所述移动距离Δx(i)和所述移动距离Δy(i)估算出所述可移动载体的旋转角度ΔαO(i)以及所述两个驱动轮中心的连线中点在垂直于所述两个驱动轮中心的连线的方向上的移动距离ΔyO(i)；
根据所述移动距离ΔSL(i)和所述移动距离ΔSR(i)估算出所述可移动载体的旋转角度ΔαM(i)以及所述两个驱动轮中心的连线中点在垂直于所述两个驱动轮中心的连线的方向上的移动距离ΔyM(i)；
根据所述旋转角度ΔαO(i)、所述移动距离ΔyO(i)、所述旋转角度ΔαM(i)、所述移动距离ΔyM(i)以及所述陀螺仪检测到的所述可移动载体的旋转角度ΔαT(i)得到所述两个驱动轮中心的连线中点在垂直于所述两个驱动轮中心的连线的方向上的移动距离ΔyN(i)；
根据所述移动距离ΔyN(i)计算所述两个驱动轮中心的连线中点在固定坐标系的坐标位置。
第二方面，本公开的实施例提供一种可移动载体的位移检测装置，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行计算机程序时实现第一方面的方法。
第三方面，本公开的实施例提供一种扫地机器人，在所述扫地机器人上安装有光学位移检测装置、左码表、右码表和陀螺仪，所述扫地机器人具有两个驱动轮，所述光学位移检测装置在所述可移动载体所处平面的正投影与所述两个驱动轮中心的连线所在直线在所述可移动载体所处平面的正投影不重叠，所述左码表用于确定所述两个驱动轮中左驱动轮的移动距离，所述右码表用于确定所述两个驱动轮中右驱动轮的移动距离，所述扫地机器人还包括第二方面的位移检测装置。
在本公开的至少一个实施例中，根据光学位移检测装置的输出确定出可移动载体的旋转角度以及两个驱动轮连线中点的移动距离量，根据两个码表的输出再次确定出可移动载体的旋转角度以及两个驱动轮连线中点的移动距离量，进一步根据陀螺仪的输出还可以确定可移动载体的旋转角度，基于这5个数据进行数据融合，可以更为高效准确地得到两个驱动轮连线中点的移动距离量。由于得到的两个驱动轮连线中点的移动距离量的数值更为可靠，基于两个驱动轮连线中点的移动距离计算得到的可移动载体的位置信息也更加可靠。
附图说明
此处所说明的附图用来提供对本公开的进一步理解，构成本公开的一部分。本公开的示意性实施例及其说明用于解释本公开，并不构成对本公开的不当限定。
图1a为本公开的可移动载体的部分零部件在可移动载体所处平面的正投影。
图1b为本公开的实施例提供的可移动载体在旋转过程中各类参数的关系图。
图2为本公开的实施例提供的可移动载体的定位方法的流程示意图。
图3为本公开的实施例提供的可移动载体的位移检测装置的结构示意图。
具体实施方式
为了更清楚的阐释本公开的整体构思，下面结合说明书附图以示例的方式进行详细说明。
在本公开中，应理解，诸如“包括”或“具有”等术语旨在指示本说明书中所公开的特征、数字、步骤、行为、部件、部分或其组合的存在，并且不排除一个或多个其他特征、数字、步骤、行为、部件、部分或其组合存在的可能性。
另外还需要说明的是，在不冲突的情况下，本公开中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本公开。
参考图1a，可移动载体例如是扫地机器人，其具有左驱动轮l、右驱动轮r、被动轮f、光学位移检测装置P、以及陀螺仪T。
参考图1b，对可移动载体进行旋转时的各种参数进行介绍。假定当前左驱动轮l的转速与右驱动轮r的转速是不同的，可移动载体按照图1b当前视角向右后方转弯。左驱动轮l与右驱动轮r两者中心的距离为距离D，其为一个已知的固定值。光学位移检测装置P到左驱动轮l和右驱动轮r的连线的垂足与左驱动轮l的距离为D1。
可移动载体的旋转中心必然位于左驱动轮l和右驱动轮r的连线所在直线上。通过确定出左驱动轮l的转速以及转动方向(是向前转动还是向后转动)、右驱动轮r的转速以及转动方向(是向前转动还是向后转动)可以确定出旋转中心O的位置。这属于本领域的公知常识，在此不进行详细描述。
本公开中“两个驱动轮的连线”特指“两个驱动轮中心的连线”。本公开的计算过程中，光学位移检测装置被认为是一个点，该点位于光学位移检测装置的中心。
光学位移检测装置P安装在两个驱动轮l、r的连线之外。换言之，光学位移检测装置P在可移动载体所处平面的正投影与两个驱动轮l、r中心的连线所在直线在所述可移动载体所处平面的正投影不重叠。光学位移检测装置P到两个驱动轮l、R的连线的垂直距离H为一个已知的固定值。坐标系xPy为扫地机的坐标系，坐标系x’Py’为光学位移检测装置的坐标系，二者有一个夹角
Figure BDA0003426255590000041
其为一个已知的固定值。
假定当前可移动载体顺时针旋转。O点为旋转中心，其必然在左驱动轮l和右驱动轮r的连线上。旋转中心O到光学位移检测装置的距离为R(当旋转中心O点位置确定后，通过勾股定理即可确定出距离R的数值)，二者连线方向与可移动载体的坐标系y轴夹角为安装偏差角度θ(当旋转中心O点的位置确定后，通过简单的三角关系运算即可确定安装偏差角度θ)。当可移动载体旋转一个很小的角度Δα(i)(为了更清晰展示该角度，故意将其画的比较大)光学位移检测装置P的位置切换至位置P’。
本申请对陀螺仪(未示出)在可移动载体的安装位置不做限定。
结合图1b并参考图2，本公开的实施例提供一种可移动载体的定位方法。可移动载体例如是扫地机器人或者运输机器人等任意在平面上移动的载体。扫地机器人每经历一个很短的时间间隔根据光学位移检测装置、左码表、右码表以及陀螺仪的输出确定一次当前的位置。图1b展示的是扫地机器人在某一个时间间隔前后的位置的变化。该方法包括以下步骤。
在步骤101中、在所述可移动载体处于运动状态时，获得在第i个时间间隔内所述光学位移检测装置在第一方向上的移动距离Δx(i)和在第二方向上的移动距离Δy(i)，所述第一方向和所述第二方向分别为所述光学位移检测装置的坐标系的两个坐标轴的正方向，所述可移动载体旋转动作的旋转轴垂直于所述可移动载体所处平面，所述第一方向和所述第二方向彼此垂直且平行于所述可移动载体所处平面。
本公开关注是诸如扫地机器人在平面上运动时的情况，可移动载体所处平面可以认为是一个平整的平面。
在一种实施方式中，移动距离Δx(i)和移动距离Δy(i)直接采用光学位移检测装置的输出值(光学位移检测装置在所述第一方向上的移动距离Δx和其在所述第二方向上的移动距离Δy)。
在另一种实施方式中，可以对光学位移检测装置的输出值(其在所述第一方向上的移动距离Δx和其在所述第二方向上的移动距离Δy)进行适当修正从而得到更加准确的移动距离Δx(i)和移动距离Δy(i)。
在步骤102中、根据所述移动距离Δx(i)和所述移动距离Δy(i)估算出所述可移动载体的旋转角度ΔαO(i)以及所述两个驱动轮中心的连线中点在垂直于所述两个驱动轮中心的连线的方向上的移动距离ΔyO(i)。
在这一步骤中，基于光学位移检测装置的输出对可移动载体的旋转角度ΔαO(i)以及两个驱动轮中心的连线中点的移动距离ΔyO(i)进行估计。容易理解，在一个很短的瞬间内，可移动载体是不会发生沿两个驱动轮中心的连线方向的漂移的(或者说本公开不考虑可移动载体发生漂移的状态)，因此在这个很短的瞬间，在平行于两个驱动轮中心的连线方向上，两个驱动轮中心的连线中点的移动距离(未在图1b中示出)ΔxO(i)＝0。
容易理解，两个驱动轮中心的连线，指的是两个驱动轮各自的中心之间的连线。即以驱动轮的中心位置表示驱动轮的位置。
以下结合图1b，光学位移检测装置P的输出移动距离为：
Figure BDA0003426255590000061
Figure BDA0003426255590000062
将公式(1)和(2)的移动距离转换为扫地机器人的坐标系的移动距离：
Figure BDA0003426255590000063
Figure BDA0003426255590000064
从公式(3)可以推出，旋转角度Δα(i)根据光学位移检测装置输出的估计值为：
Figure BDA0003426255590000065
同时，从图1b可以推出，若在第i时间间隔根据光学位移检测装置输出得到的扫地机器人两个驱动轮连线中点的在垂直于该连线的方向上的移动距离的估计值是：
Figure BDA0003426255590000066
结合公式(4)后可以将公式(6)改写为：
Figure BDA0003426255590000067
公式(7)中，Δα(i)用前面计算得到旋转角度ΔαO(i)进行估计。
在步骤102中，基于光学位移检测装置的输出确定出了可移动载体的旋转角度和两个驱动轮连线中点位移的一组估计值ΔαO(i)和ΔyO(i)。
在步骤103中，根据所述移动距离ΔSL(i)和所述移动距离ΔSR(i)估算出所述可移动载体的旋转角度ΔαM(i)以及所述两个驱动轮中心的连线中点在垂直于所述两个驱动轮中心的连线的方向上的移动距离ΔyM(i)。
结合图1b，在第i时间间隔，扫地机器人左驱动轮1的码表位移测量输出与Δα(i)关系为：
ΔSL(i)＝-(Rsinθ-D1)Δα(i) (8)
扫地机器人右驱动轮r的码表位移测量输出与Δα(i)关系为：
ΔSR(i)＝-(Rsinθ-D1+D)Δα(i) (9)
注意，扫地机器人在做如图1b的顺时针旋转时，两码表的输出均为负值(即两个轮子都是向后旋转的)。因此，从公式(8)和(9)，可以推出，旋转角度Δα(i)的根据码表输出的估计值为：
Figure BDA0003426255590000071
同时，从图1b可以推出，若在第i时间间隔，根据码表输出得到的扫地机器人两驱动轮连线中点的沿垂直于该连线的方向上的移动距离的估计值是：
Figure BDA0003426255590000072
在步骤103中，基于两个码表的输出确定出了可移动载体的旋转角度和两个驱动轮连线中点位移的一组估计值ΔαM(i)和ΔyM(i)。
在步骤104中，根据所述旋转角度ΔαO(i)、所述移动距离ΔyO(i)、所述旋转角度ΔαM(i)、所述移动距离ΔyM(i)以及所述陀螺仪检测到的所述可移动载体的旋转角度ΔαT(i)得到所述两个驱动轮中心的连线中点在垂直于所述两个驱动轮中心的连线的方向上的移动距离ΔyN(i)。
该步骤中，可以以旋转角度ΔαT(i)作为基准，判断旋转角度ΔαO(i)和ΔαM(i)中哪一项更为接近旋转角度ΔαT(i)并且接近的程度足够大，则选择哪一项对应的移动距离作为预测的当前时间间隔的移动距离ΔyN(i)。也可以在此基础上进行更周密的设计。
该步骤中对这5个数据进行数据融合，从而得到更准确的移动距离ΔyN(i)。注意陀螺仪输出的旋转角度ΔαT(i)无需进行数据处理，可直接应用在融合算法中。
在步骤105中，根据所述移动距离ΔyN(i)计算所述两个驱动轮中心的连线中点在固定坐标系的坐标位置。
具体地，根据移动距离ΔyN(i)进行几何运算，从而推导出扫地机器人在固定坐标系中的坐标位置。
在本公开的实施例中，根据光学位移检测装置的输出确定出可移动载体的旋转角度以及两个驱动轮连线中点的移动距离量，根据两个码表的输出再次确定出可移动载体的旋转角度以及两个驱动轮连线中点的移动距离量，进一步根据陀螺仪的输出还可以确定可移动载体的旋转角度，基于这5个数据进行数据融合，可以更为精确地得到两个驱动轮连线中点的移动距离量。由于得到的两个驱动轮连线中点的移动距离量的数值更为可靠，基于两个驱动轮连线中点的移动距离计算得到的可移动载体的位置信息也更加可靠。
以下对基于上述5个变量进行数据融合的过程进行详细说明。
根据所述旋转角度ΔαO(i)、所述移动距离ΔyO(i)、所述旋转角度ΔαM(i)、所述移动距离ΔyM(i)以及所述陀螺仪检测到的所述可移动载体的旋转角度ΔαT(i)得到所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)。
对短期内的所述位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差进行统计，得到所述参数EY(i)。参数EY(i)反应的是在最近的一段时间内所述位移变量ΔyO(i)和所述移动距离ΔyM(i)是否比较接近。
对短期内的所述旋转角度ΔαO(i)与所述旋转角度ΔαT(i)的偏差和短期内的所述旋转角度ΔαM(i)与所述旋转角度ΔαT(i)的偏差进行比较，得到所述参数Eα(i)。参数Kα(i)反应的是所述旋转角度ΔαO(i)与所述旋转角度ΔαT(i)二者谁更接近于所述旋转角度ΔαT(i)，以及接近的程度。通常情况下陀螺仪输出的角度信息是相对更为精确的，可以作为评判标准。
根据短期内所述光学位移检测装置提供的像素均值Su(i)和画质评价值Sq(i)评价当前光学位移检测装置输出的数据的可靠性，得到所述参数ES(i)。参数ES(i)反应的是光学位移检测装置本身的可靠性。当可移动载体在诸如地毯上移动时比其在反光界面上移动时，光学位移检测装置的输出是更可靠的。
统计短期内所述移动距离ΔyO(i)几乎为0的频次，得到所述参数EZ(i)。
举例而言，所述参数EY(i)按照如下公式计算：
EY(i)＝KYEY(i-1)+[ΔYM(i)-ΔYO(i)]2 (12)；
其中，EY(i-1)为参数EY(i)在上一时间间隔的数值，KY为衰减因子。
KY的建议值是6/7。当EY(i)＜CY时，判定在最近的一段时间内，码表和光学位移检测装置对可移动载体(以下扫地机器人为例说明)两个驱动轮连线中点的移动距离的估计差别不大，即光学位移检测装置和码表的输出可能都是有效的。反之，当EY(i)≥CY时，表明在最近的一段时间内，移动距离ΔyO(i)和移动距离ΔyM(i)的平均差别较大，光学位移检测装置和码表的输出中有一个是无效的。CY的建议值是2.5×10-3。
例如，所述参数Eα(i)按照如下公式计算：
Eα(i)＝KαEα(i-1)+[ΔαM(i)-ΔαT(i)]2-[ΔαO(i)-ΔαT(i)]2 (13)；
其中，Eα(i-1)为参数Eα(i)在上一时间间隔的数值，Kα为衰减因子。
Kα的建议值是2/3。当Eα(i)＞Cd+时，判定在最近的一段时间内，ΔαM(i)的平均误差大于ΔαO(i)的平均误差，即在最近的一段时间内，光学位移检测装置的输出更为可靠。Cd+是一个正的阈值，建议值是1.8×10-5。当Eα(i)＜Cd-时，判定在最近的一段时间内，ΔαO(i)的平均误差大于ΔαM(i)的平均误差，即在最近的一段时间内，码表的输出更为可靠。Cd-是一个负的阈值，建议值是-1.0×10-4。
例如，所述参数ES(i)按照如下公式计算：
ES(i)＝KSES(i-1)+Sq(i)-CSSu(i) (14)；
其中，KS为衰减因子，ES(i-1)为参数ES(i)在上一时间间隔的数值，CS为设定比例系数。
Sq(i)和Su(i)是光学位移检测装置提供的与检测质量及可靠性相关的两个参数，Su(i)代表的是图像的像素均值，Sq(i)代表的是图像的品质。
KS的建议值是4/5，CS的建议值是13/32。当ES(i)＞CS+时，判定在最近的一段时间内，光学位移检测装置的输出是可靠的。CS+的建议值是0。当ES(i)＜CS-时，判定在最近的一段时间内，光学位移检测装置的输出明显失效。CS-是一个负的阈值，建议值是-60。
例如，所述参数EZ(i)按照如下公式计算：
Figure BDA0003426255590000101
其中，KZ为衰减因子，EZ(i-1)为EZ(i)上一时间间隔的数值，CO为设定阈值。
KZ的建议值是4/5，CO的建议值是0.05。当EZ(i)＞CZ时，本算法判定在最近的一段时间内，ΔyO(i)很少有接近零的情况。CZ的建议值是3.2。
以上4个参数是用递归的方式进行更新的。当然，也可以设置滑动时间窗口，对时间窗口内的数据进行运算得到上述4个参数当前的取值。
可选地，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)小于或等于设定阈值CY(表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差足够小)、同时在第i时间间隔|ΔαM(i)-ΔαT(i)|＜|ΔαO(i)-ΔαT(i)|、并且|ΔαM(i)-ΔαT(i)|＜Cα的情况下，令ΔyN(i)＝ΔyM(i)，其中，Cα为设定弧度阈值。
此处以及后文中Cα的建议值均可以是3.5×10-3弧度(0.2度)。
换言之，如果近期内码表和光学位移检测装置的输出差异很小、在当前时间间隔根据码表输出判断得到的旋转角度比根据光学位移检测装置的输出判断得到的旋转角度更准确，并且根据码表输出判断得到的旋转角度与根据陀螺仪输出的旋转角度二者足够接近，那么认定根据码表的输出所判断出的移动距离量是足够可信的，用根据码表计算得出的移动距离ΔyM(i)作为移动距离ΔyN(i)的估计值。
可选地，根据参数EY(i)、参数Kα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)小于或等于设定阈值CY(表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差足够小)、同时在第i时间间隔|ΔαM(i)-ΔαT(i)|＜|ΔαO(i)-ΔαT(i)|、并且|ΔαM(i)-ΔαT(i)|≥Cα的情况下，令
Figure BDA0003426255590000111
其中，Cα为设定弧度阈值，D为所述两个驱动轮中心的距离值，D1为所述光学位移检测装置到所述两个驱动轮中心的连线的正投影点与左驱动轮中心的距离值，ΔyM(i-1)为上一时间间隔ΔyM(i)的取值，ΔyO(i-1)为上一时间间隔ΔyO(i)的取值。
换言之，如果近期内码表和光学位移检测装置的输出差异很小、在当前时间间隔根据码表输出判断得到的旋转角度比根据光学位移检测装置的输出得到的旋转角度更准确，但根据码表输出判断出的旋转角度仍然误差较大，即码表的输出和光学位移检测装置的输出均不可靠，考虑运动的连续性，沿用前一时间间隔所得计算结果作为当前时间间隔的计算结果。
可选地，根据参数EY(i)、参数Kα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)小于或等于设定阈值CY(表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差足够小)、同时在第i时间间隔|ΔαM(i)-ΔαT(i)|≥|ΔαO(i)-ΔαT(i)|、并且|ΔαO(i)-ΔαT(i)|＜Cα的情况下，令ΔyN(i)＝ΔyO(i)，其中，Cα为设定弧度阈值。
换言之，如果近期内码表和光学位移检测装置的输出差异很小、在当前时间间隔根据码表输出判断得到的旋转角度不如根据光学位移检测装置的输出判断得到的旋转角度准确，根据光学位移检测装置的输出判断得到的旋转角度与根据陀螺仪输出的旋转角度二者足够接近，那么认定根据光学位移检测装置的输出所判断出的移动距离量是足够可信的，用根据光学位移检测装置计算得出的移动距离ΔyO(i)作为移动距离ΔyN(i)的估计值。
可选地，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)小于设定阈值CY(表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差足够小)、同时在第i时间间隔|ΔαM(i)-ΔαT(i)|≥|ΔαO(i)-ΔαT(i)|、并且|ΔαO(i)-ΔαT(i)|≥Cα的情况下，令
Figure BDA0003426255590000121
其中，Cα为设定弧度阈值，D为所述两个驱动轮中心的距离值，D1为所述光学位移检测装置到所述两个驱动轮中心的连线的正投影点与左驱动轮中心的距离值，ΔyM(i-1)为上一时间间隔ΔyM(i)的取值，ΔyO(i-1)为上一时间间隔ΔyO(i)的取值。
换言之，如果近期内码表和光学位移检测装置的输出差异很小、在当前时间间隔根据码表输出判断得到的旋转角度不如根据光学位移检测装置的输出得到的旋转角度准确，但根据光学位移检测装置的输出所述判断出的旋转角度仍然误差较大，即码表的输出和光学位移检测装置的输出均不可靠，考虑运动的连续性，沿用前一时间间隔所得计算结果作为当前时间间隔的计算结果。
可选地，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY(表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差大)、所述参数Eα(i)大于设定阈值Cd+(表明近期内旋转角度ΔαO(i)比旋转角度ΔαM(i)更接近旋转角度ΔαT(i))、并且在第i时间间隔|ΔαO(i)-ΔαT(i)|＜Cα的情况下，令ΔyN(i)＝ΔyO(i)，其中，Cα为设定弧度阈值。
换言之，如果近期内码表和光学位移检测装置的输出差异很大，那么根据参数Eα(i)做进一步的判断。如果近期根据光学检测装置的输出得到旋转角度比根据码表输出得到的旋转角度明显更可靠，进一步在当前时间间隔根据光学位移检测装置的输出得到旋转角度是足够准确的，那么认定根据光学位移检测装置的输出所判断出的移动距离量是足够可信的，用根据光学位移检测装置计算得出的移动距离ΔyO(i)作为移动距离ΔyN(i)的估计值。
可选地，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY(表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差大)、所述参数Eα(i)大于设定阈值Cd+(表明近期内旋转角度ΔαO(i)比旋转角度ΔαM(i)更接近旋转角度ΔαT(i))、并且在第i时间间隔|ΔαO(i)-ΔαT(i)|≥Cα的情况下，令
Figure BDA0003426255590000131
其中，Cα为设定弧度阈值，D为所述两个驱动轮中心的距离值，D1为所述光学位移检测装置到所述两个驱动轮中心的连线的正投影点与左驱动轮中心的距离值，ΔyM(i-1)为上一时间间隔ΔyM(i)的取值，ΔyO(i-1)为上一时间间隔ΔyO(i)的取值。
换言之，如果近期内码表和光学位移检测装置的输出差异很大，那么根据参数Eα(i)做进一步的判断。如果近期根据光学检测装置的输出得到旋转角度比根据码表输出得到的旋转角度明显更可靠，但在当前时间间隔根据光学检测装置的输出得到旋转角度依然是不准确的，即码表的输出和光学位移检测装置的输出均不可靠，考虑运动的连续性，沿用前一时间间隔所得计算结果作为当前时间间隔的计算结果。
可选地，根据参数EY(i)、参数Kα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY(表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差大)、所述参数Eα(i)小于设定阈值Cd-(表明近期内旋转角度ΔαM(i)比旋转角度ΔαO(i)更接近旋转角度ΔαT(i))、并且在第i时间间隔|ΔαM(i)-ΔαT(i)|＜Cα的情况下，令ΔyN(i)＝ΔyM(i)，其中，Cα为设定弧度阈值。
换言之，如果近期内码表和光学位移检测装置的输出差异很大，那么根据参数Eα(i)做进一步的判断。如果近期根据码表的输出得到旋转角度比根据光学位移检测装置的输出得到的旋转角度明显更可靠，进一步在当前时间间隔根据码表的输出得到旋转角度是足够准确的，那么认定根据码表的输出所判断出的移动距离量是足够可信的，用根据码表计算得出的移动距离ΔyM(i)作为移动距离ΔyN(i)的估计值。
可选地，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY(表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差大)、所述参数Eα(i)小于设定阈值Cd-(表明近期内旋转角度ΔαM(i)比旋转角度ΔαO(i)更接近旋转角度ΔαT(i)、并且在第i时间间隔|ΔαM(i)-ΔαT(i)|≥Cα的情况下，令
Figure BDA0003426255590000141
其中，Cα为设定弧度阈值，D为所述两个驱动轮中心的距离值，D1为所述光学位移检测装置到所述两个驱动轮中心的连线的正投影点与左驱动轮中心的距离值，ΔyM(i-1)为上一时间间隔ΔyM(i)的取值，ΔyO(i-1)为上一时间间隔ΔyO(i)的取值。
换言之，如果近期内码表和光学位移检测装置的输出差异很大，那么根据参数Eα(i)做进一步的判断。如果近期根据码表的输出得到旋转角度比根据光学位移检测装置输出得到的旋转角度明显更可靠，但在当前时间间隔根据码表的输出得到旋转角度依然是不准确的，即码表的输出和光学位移检测装置的输出均不可靠，考虑运动的连续性，沿用前一时间间隔所得计算结果作为当前时间间隔的计算结果。
可选地，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY(表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差大)、所述参数Eα(i)大于或等于设定阈值Cd-且所述参数Eα(i)小于或等于设定阈值Cd+(表明近期内旋转角度ΔαM(i)和旋转角度ΔαO(i)与旋转角度ΔαT(i)的接近程度无明显差异)、ES(i)＜CS-(表明近期内所述光学位移检测装置的输出明显失效)、并且在第i时间间隔|ΔαM(i)-ΔαT(i)|＜Cα的情况下，令ΔyN(i)＝ΔyM(i)，其中，Cα为设定弧度阈值，CS-为设定失效评价阈值。
换言之，如果近期内码表和光学位移检测装置的输出差异很大，那么根据参数Eα(i)做进一步的判断。如果近期根据码表的输出得到旋转角度与根据光学位移检测装置的输出得到的旋转角度二者差异并不明显，那么进一步对光学位移检测装置的输出自身的可信度进行评价。如果近期内光学位移检测装置的输出明显失效，而在当前时间间隔根据码表的输出得到旋转角度是足够准确的，那么认定根据码表的输出所判断出的移动距离量是足够可信的，用根据码表计算得出的移动距离ΔyM(i)作为移动距离ΔyN(i)的估计值。
可选地，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY(表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差大)、所述参数Eα(i)大于或等于设定阈值Cd-且所述参数Eα(i)小于或等于设定阈值Cd+(表明近期内旋转角度ΔαM(i)和旋转角度ΔαO(i)与旋转角度ΔαT(i)的接近程度无明显差异)、ES(i)＜CS-(表明近期内所述光学位移检测装置的输出明显失效)、并且在第i时间间隔|ΔαM(i)-ΔαT(i)|≥Cα的情况下，令
Figure BDA0003426255590000161
其中，Cα为设定弧度阈值，D为所述两个驱动轮中心的距离值，D1为所述光学位移检测装置到所述两个驱动轮中心的连线的正投影点与左驱动轮中心的距离值，ΔyM(i-1)为上一时间间隔ΔyM(i)的取值，ΔyO(i-1)为上一时间间隔ΔyO(i)的取值，CS-为设定失效评价阈值。
换言之，如果近期内码表和光学位移检测装置的输出差异很大，那么根据参数Eα(i)做进一步的判断。如果近期根据码表的输出得到旋转角度与根据光学位移检测装置的输出得到的旋转角度二者差异并不明显，那么进一步对光学位移检测装置的输出自身的信号质量进行评价。如果近期内光学位移检测装置的输出明显失效，并且在当前时间间隔根据码表的输出得到旋转角度也是不够准确的，考虑运动的连续性，沿用前一时间间隔所得计算结果作为当前时间间隔的计算结果。
可选地，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY(表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差大)、所述参数Eα(i)大于或等于设定阈值Cd-且所述参数Eα(i)小于或等于设定阈值Cd+(表明近期内旋转角度ΔαM(i)和旋转角度ΔαO(i)与旋转角度ΔαT(i)的接近程度无明显差异)、ES(i)＞CS+(表明近期内所述光学位移检测装置的输出可靠)、并且在第i时间间隔|ΔαO(i)-ΔαT(i)|＜Cα的情况下，令ΔyN(i)＝ΔyO(i)，其中，Cα为设定弧度阈值，CS-为设定失效评价阈值。
换言之，如果近期内码表和光学位移检测装置的输出差异很大，那么根据参数Eα(i)做进一步的判断。如果近期根据码表的输出得到旋转角度与根据光学位移检测装置的输出得到的旋转角度二者差异并不明显，那么进一步对光学位移检测装置的输出自身的信号质量进行评价。如果近期内光学位移检测装置的输出明显可靠，并且在当前时间间隔根据光学位移检测装置的输出得到旋转角度是足够准确的，那么认定根据光学位移检测装置的输出所判断出的移动距离量是足够可信的，用根据光学位移检测装置计算得出的移动距离ΔyO(i)作为移动距离ΔyN(i)的估计值。
可选地，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY(表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差足够大)、所述参数Eα(i)大于或等于设定阈值Cd-且所述参数Eα(i)小于或等于设定阈值Cd+(表明近期内旋转角度ΔαM(i)和旋转角度ΔαO(i)与旋转角度ΔαT(i)的接近程度无明显差异)、ES(i)＞CS+(表明近期内所述光学位移检测装置的输出可靠)、并且在第i时间间隔|ΔαO(i)-ΔαT(i)|≥Cα的情况下，令
Figure BDA0003426255590000171
其中，Cα为设定弧度阈值，D为所述两个驱动轮中心的距离值，D1为所述光学位移检测装置到所述两个驱动轮中心的连线的正投影点与左驱动轮中心的距离值，ΔyM(i-1)为上一时间间隔ΔyM(i)的取值，ΔyO(i-1)为上一时间间隔ΔyO(i)的取值，CS+为设定有效评价阈值。
换言之，如果近期内码表和光学位移检测装置的输出差异很大，那么根据参数Eα(i)做进一步的判断。如果近期根据码表的输出得到旋转角度与根据光学位移检测装置的输出得到的旋转角度二者差异并不明显，那么进一步对光学位移检测装置的输出自身的信号质量进行评价。如果近期内光学位移检测装置的输出信号质量明显可靠，但在当前时间间隔根据光学位移检测装置的输出得到旋转角度仍然是不准确的，那么认定根据光学位移检测装置的输出所判断出的移动距离量依然是不够可信的，考虑运动的连续性，沿用前一时间间隔所得计算结果作为当前时间间隔的计算结果。
可选地，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY(表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差大)、所述参数Eα(i)大于或等于设定阈值Cd-且所述参数Eα(i)小于或等于设定阈值Cd+(表明近期内旋转角度ΔαM(i)和旋转角度ΔαO(i)与旋转角度ΔαT(i)的接近程度无明显差异)、CS-≤ES(i)≤CS+(表明近期内所述光学位移检测装置的输出可靠性不明确)、参数Ez(i)大于设定阈值CZ(表明光学位移检测装置的数据近期内较少地接近于0)，并且在第i时间间隔|ΔαO(i)-ΔαT(i)|＜Cα的情况下，令ΔyN(i)＝ΔyO(i)，其中，Cα为设定弧度阈值，CS-为设定失效评价阈值，CS+为设定有效评价阈值。
换言之，如果近期内码表和光学位移检测装置的输出差异很大，那么根据参数Eα(i)做进一步的判断。如果近期根据码表的输出得到旋转角度与根据光学位移检测装置的输出得到的旋转角度二者差异并不明显，那么进一步对近期内光学位移检测装置的输出自身的信号质量进行评价。如果近期内光学位移检测装置的输出是否可靠不能得出明确结论，那么进一步对光学位移检测装置的数据近期内是否经常接近于0进行判断。光学位移检测装置的数据短期内经常接近于0，则表明根据光学位移检测装置得到的数据不可靠，否则表明根据光学位移检测装置得到的数据可靠。当近期内光学位移检测装置的输出并未频繁接近于0，并且在当前时间间隔根据光学位移检测装置的输出得到的旋转角度也是足够准确的，用根据光学位移检测装置计算得出的移动距离ΔyO(i)作为移动距离ΔyN(i)的估计值。
可选地，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY(表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差大)、所述参数Eα(i)大于或等于设定阈值Cd-且所述参数Eα(i)小于或等于设定阈值Cd+(表明近期内旋转角度ΔαM(i)和旋转角度ΔαO(i)与旋转角度ΔαT(i)的接近程度无明显差异)、CS-≤ES(i)≤CS+(表明近期内所述光学位移检测装置的输出可靠性不明确)、参数Ez(i)大于设定阈值CZ(表明光学位移检测装置的数据近期内较少地接近于0)，并且在第i时间间隔|ΔαO(i)-ΔαT(i)|≥Cα的情况下，令
Figure BDA0003426255590000191
其中，Cα为设定弧度阈值，D为所述两个驱动轮中心的距离值，D1为所述光学位移检测装置到所述两个驱动轮中心的连线的正投影点与左驱动轮中心的距离值，ΔyM(i-1)为上一时间间隔ΔyM(i)的取值，ΔyO(i-1)为上一时间间隔ΔyO(i)的取值，CS-为设定失效评价阈值，CS+为设定有效评价阈值。
换言之，如果近期内码表和光学位移检测装置的输出差异很大，那么根据参数Eα(i)做进一步的判断。如果近期根据码表的输出得到旋转角度与根据光学位移检测装置的输出得到的旋转角度二者差异并不明显，那么进一步对光学位移检测装置的输出自身的信号质量进行评价。如果近期内光学位移检测装置的输出是否可靠不能得出明确结论，那么进一步对光学位移检测装置的数据近期内是否经常接近于0进行判断。光学位移检测装置的数据近期内经常接近于0，则表明根据光学位移检测装置得到的数据不可靠，否则表明根据光学位移检测装置得到的数据可靠。当近期内光学位移检测装置的输出并未频繁接近于0，但在当前时间间隔根据光学位移检测装置的输出得到的旋转角度依然不够准确的，考虑运动的连续性，沿用前一时间间隔所得计算结果作为当前时间间隔的计算结果。
可选地，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY(表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差大)、所述参数Eα(i)大于或等于设定阈值Cd-且所述参数Eα(i)小于或等于设定阈值Cd+(表明近期内旋转角度ΔαM(i)和旋转角度ΔαO(i)与旋转角度ΔαT(i)的接近程度无明显差异)、CS-≤ES(i)≤CS+(表明近期内所述光学位移检测装置的输出可靠性不明确)、参数Ez(i)小于或等于设定阈值CZ(表明光学位移检测装置的数据近期内较多地接近于0)，并且在第i时间间隔|ΔαM(i)-ΔαT(i)|＜Cα的情况下，令ΔyN(i)＝ΔyM(i)，其中，Cα为设定弧度阈值，CS-为设定失效评价阈值，CS+为设定有效评价阈值。
换言之，如果近期内码表和光学位移检测装置的输出差异很大，那么根据参数Eα(i)做进一步的判断。如果近期根据码表的输出得到旋转角度与根据光学位移检测装置的输出得到的旋转角度二者差异并不明显，那么进一步对近期内光学位移检测装置的输出自身的信号质量进行评价。如果光学位移检测装置的输出是否可靠不能得出明确结论，那么进一步对光学位移检测装置的数据近期内是否经常接近于0进行判断。光学位移检测装置的数据近期内经常接近于0，则表明根据光学位移检测装置得到的数据不可靠，否则表明根据光学位移检测装置得到的数据可靠。当近期内光学位移检测装置的输出频繁接近于0，并且在当前时间间隔根据码表的输出得到的旋转角度是足够准确的，用根据码表计算得出的移动距离ΔyM(i)作为移动距离ΔyN(i)的估计值。
可选地，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY(表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差大)、所述参数Eα(i)大于或等于设定阈值Cd-且所述参数Eα(i)小于或等于设定阈值Cd+(表明近期内旋转角度ΔαM(i)和旋转角度ΔαO(i)与旋转角度ΔαT(i)的接近程度无明显差异)、CS-≤ES(i)≤CS+(表明近期内所述光学位移检测装置的输出可靠性不明确)、参数Ez(i)小于或等于设定阈值Cd-(表明光学位移检测装置的数据近期内较多地接近于0)，并且在第i时间间隔|ΔαM(i)-ΔαT(i)|≥Cα的情况下，令
Figure BDA0003426255590000211
其中，Cα为设定弧度阈值，D为所述两个驱动轮中心的距离值，D1为所述光学位移检测装置到所述两个驱动轮中心的连线的正投影点与左驱动轮中心的距离值，ΔyM(i-1)为上一时间间隔ΔyM(i)的取值，ΔyO(i-1)为上一时间间隔ΔyO(i)的取值，CS-为设定失效评价阈值，CS+为设定有效评价阈值。
换言之，如果近期内码表和光学位移检测装置的输出差异很大，那么根据参数Eα(i)做进一步的判断。如果近期根据码表的输出得到旋转角度与根据光学位移检测装置的输出得到的旋转角度二者差异并不明显，那么进一步对近期内光学位移检测装置的输出自身的信号质量进行评价。如果光学位移检测装置的输出是否可靠不能得出明确结论，那么进一步对光学位移检测装置的数据近期内是否经常接近于0进行判断。光学位移检测装置的数据近期内经常接近于0，则表明根据光学位移检测装置得到的数据不可靠，否则表明根据光学位移检测装置得到的数据可靠。当近期内光学位移检测装置的输出频繁接近于0，根据光学位移检测装置得到的信息不可靠。进一步在当前时间间隔根据码表的输出得到的旋转角度也是不够准确的，考虑运动的连续性，沿用前一时间间隔所得计算结果作为当前时间间隔的计算结果。
在得到了移动距离ΔyN(i)的准确估值之后，既可以计算得出可移动载体的两个驱动轮连线中点坐标的准确位置。
具体地，根据所述移动距离ΔyN(i)在所述可移动载体的坐标系计算在其两个坐标轴所在方向的移动距离ΔxR(i)和移动距离ΔyR(i)；
根据所述陀螺仪在上一时间间隔的总转角αT(i-1)得到所述可移动载体在固定坐标系沿其两个坐标轴所在方向的移动距离ΔxF(i)和移动距离ΔyF(i)；
根据所述移动距离ΔxF(i)和所述移动距离ΔyF(i)在所述固定坐标系更新所述可移动载体的坐标。
具体地，参考图1b，扫地机器人两个驱动轮连线中点的实际移动距离是：
Figure BDA0003426255590000221
Figure BDA0003426255590000222
在上述公式中，因为Δα(i)是一个接近于0的值，运用了泰勒展开公式，即sinΔα(i)≈Δα(i)，及
Figure BDA0003426255590000223
扫地机器人两个驱动轮连线中点实际的移动距离可以简化为：
Figure BDA0003426255590000224
Figure BDA0003426255590000225
根据公式(6)和(11)及步骤104的数据融合的结果，扫地机器人两个驱动轮连线中点的实际移动距离可以如下计算：
Figure BDA0003426255590000226
ΔyR(i)＝ΔyN(i)(24) (21)。
在扫地机器人旋转时，扫地机器人的坐标角度总是在不停的改变，因此得出的位移也是不同坐标系的位移，不能直接相加。为了将角度改变后的不同坐标系的位移转化为一个固定坐标系的位移，我们需要做如下坐标变换：
ΔxF(i)＝ΔxR(i)cosαT(i-1)+ΔyR(i)sinαT(i-1) (22)；
ΔyF(i)＝ΔyR(i)cosαT(i-1)-ΔxR(i)sinαT(i-1) (23)；
其中，αT(i-1)是旋转角度ΔαT(i)对应的时间间隔之前陀螺仪的总转角。在陀螺仪旋转ΔαT(i)之后，αT(i)的值要调整为：
αT(i)＝αT(i-1)+ΔαT(i) (24)。
在陀螺仪旋转ΔαT(i)之后，扫地机器人两个驱动轮连线中点在固定坐标系的坐标更新为：
xF(i)＝xF(i-1)+ΔxF(i) (25)；
yF(i)＝yF(i-1)+ΔyF(i) (26)。
基于与前述方法相同的发明构思，参考图3，本公开的实施例提供一种可移动载体的位移检测装置，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行计算机程序时实现上述各种可能的实现方式中任意一项的可移动载体的定位方法。
本公开的实施例提供一种计算机可读存储介质。存储介质存储有计算机程序，计算机程序被处理器执行时实现上述各种可能的实现方式中任意一项的方法。
本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于实体实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
专业人员应该还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本公开的范围。
结合本文中所公开的实施例描述的方法或算法的步骤可以用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。
以上仅为本公开的实施例而已，并不用于限制本公开。对于本领域技术人员来说，本公开可以有各种更改和变化。凡在本公开的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本公开的权利要求范围之内。

Claims (29)

1.一种可移动载体的定位方法，其特征在于，在所述可移动载体上安装有光学位移检测装置、左码表、右码表和陀螺仪，所述可移动载体具有两个驱动轮，所述光学位移检测装置在所述可移动载体所处平面的正投影与所述两个驱动轮中心的连线所在直线在所述可移动载体所处平面的正投影不重叠，所述左码表用于确定所述两个驱动轮中左驱动轮在第i个时间间隔内的移动距离ΔSL(i)，所述右码表用于确定所述两个驱动轮中右驱动轮在第i个时间间隔内的移动距离ΔSR(i)，所述定位方法包括：
在所述可移动载体处于运动状态时，获得在第i个时间间隔内所述光学位移检测装置在第一方向上的移动距离Δx(i)和在第二方向上的移动距离Δy(i)，所述第一方向和所述第二方向分别为所述光学位移检测装置的坐标系的两个坐标轴的正方向，所述可移动载体旋转动作的旋转轴垂直于所述可移动载体所处平面，所述第一方向和所述第二方向彼此垂直且平行于所述可移动载体所处平面；
根据所述移动距离Δx(i)和所述移动距离Δy(i)估算出所述可移动载体的旋转角度ΔαO(i)以及所述两个驱动轮中心的连线中点在垂直于所述两个驱动轮中心的连线的方向上的移动距离ΔyO(i)；
根据所述移动距离ΔSL(i)和所述移动距离ΔSR(i)估算出所述可移动载体的旋转角度ΔαM(i)以及所述两个驱动轮中心的连线中点在垂直于所述两个驱动轮中心的连线的方向上的移动距离ΔyM(i)；
根据所述旋转角度ΔαO(i)、所述移动距离ΔyO(i)、所述旋转角度ΔαM(i)、所述移动距离ΔyM(i)以及所述陀螺仪检测到的所述可移动载体的旋转角度ΔαT(i)得到所述两个驱动轮中心的连线中点在垂直于所述两个驱动轮中心的连线的方向上的移动距离ΔyN(i)；
根据所述移动距离ΔyN(i)计算所述两个驱动轮中心的连线中点在固定坐标系的坐标位置。
2.根据权利要求1的方法，其特征在于，旋转角度ΔαO(i)按照如下公式计算：
Figure FDA0003426255580000021
其中，
Figure FDA0003426255580000022
为所述光学位移检测装置的坐标系与所述可移动载体的坐标系的偏差角度，H为所述光学位移检测装置相对于所述两个驱动轮中心的连线的垂直距离。
3.根据权利要求1所述的方法，其特征在于，所述移动距离ΔyO(i)按照如下公式确定：
Figure FDA0003426255580000023
其中，
Figure FDA0003426255580000024
为所述光学位移检测装置的坐标系与所述可移动载体的坐标系的偏差角度，D1为所述光学位移检测装置到所述两个驱动轮中心的连线的垂足与左驱动轮中心的距离值，D为所述两个驱动轮中心的距离值。
4.根据权利要求1所述的方法，其特征在于，所述旋转角度ΔαM(i)按照如下公式计算：
Figure FDA0003426255580000025
其中，D为所述两个驱动轮中心的距离值。
5.根据权利要求1所述的方法，其特征在于，ΔyM(i)按照如下公式计算：
Figure FDA0003426255580000026
6.根据权利要求1所述的方法，其特征在于，根据所述旋转角度ΔαO(i)、所述移动距离ΔyO(i)、所述旋转角度ΔαM(i)、所述移动距离ΔyM(i)以及所述陀螺仪检测到的所述可移动载体的旋转角度ΔαT(i)得到所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)；其中，
对近期内的所述位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差进行统计，得到所述参数EY(i)；
对近期内的所述旋转角度ΔαO(i)与所述旋转角度ΔαT(i)的偏差和近期内的所述旋转角度ΔαM(i)与所述旋转角度ΔαT(i)的偏差进行比较，得到所述参数Eα(i)；
根据近期内所述光学位移检测装置提供的像素均值Su(i)和画质评价值Sq(i)评价当前光学位移检测装置输出的数据的可靠性，得到所述参数ES(i)；
统计近期内所述移动距离ΔyO(i)几乎为0的频次，得到所述参数EZ(i)。
7.根据权利要求6所述的方法，其特征在于，所述参数EY(i)按照如下公式计算：
EY(i)＝KYEY(i-1)+[ΔYM(i)-ΔYO(i)]2；
其中，EY(i-1)为参数EY(i)在上一时间间隔的数值，KY为衰减因子。
8.根据权利要求6所述的方法，其特征在于，所述参数Eα(i)按照如下公式计算：
Eα(i)＝KαEα(i-1)+[ΔαM(i)-ΔαT(i)]2-[ΔαO(i)-ΔαT(i)]2；
其中，Eα(i-1)为参数Eα(i)在上一时间间隔的数值，Kα为衰减因子。
9.根据权利要求6所述的方法，其特征在于，所述参数ES(i)按照如下公式计算：
ES(i)＝KSES(i-1)+Sq(i)-CSSu(i)；
其中，KS为衰减因子，ES(i-1)为参数ES(i)在上一时间间隔的数值，CS为设定比例系数。
10.根据权利要求6所述的方法，其特征在于，所述参数EZ(i)按照如下公式计算：
Figure FDA0003426255580000031
其中，KZ为衰减因子，EZ(i-1)为EZ(i)上一时间间隔的数值，CO为设定阈值。
11.根据权利要求6所述的方法，其特征在于，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)小于或等于设定阈值CY而表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差足够小、同时在第i时间间隔|ΔαM(i)-ΔαT(i)|＜|ΔαO(i)-ΔαT(i)|、并且|ΔαM(i)-ΔαT(i)|＜Cα的情况下，令ΔyN(i)＝ΔyM(i)，其中，Cα为设定弧度阈值。
12.根据权利要求6所述的方法，其特征在于，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)小于或等于设定阈值CY而表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差足够小、同时在第i时间间隔|ΔαM(i)-ΔαT(i)|＜|ΔαO(i)-ΔαT(i)|、并且|ΔαM(i)-ΔαT(i)|≥Cα的情况下，令
Figure FDA0003426255580000041
其中，Cα为设定弧度阈值，D为所述两个驱动轮中心的距离值，D1为所述光学位移检测装置到所述两个驱动轮中心的连线的垂足与左驱动轮中心的距离值，ΔyM(i-1)为上一时间间隔ΔyM(i)的取值，ΔyO(i-1)为上一时间间隔ΔyO(i)的取值。
13.根据权利要求6所述的方法，其特征在于，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)小于或等于设定阈值CY而表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差足够小、同时在第i时间间隔|ΔαM(i)-ΔαT(i)|≥|ΔαO(i)-ΔαT(i)|、并且|ΔαO(i)-ΔαT(i)|＜Cα的情况下，令ΔyN(i)＝ΔyO(i)，其中，Cα为设定弧度阈值。
14.根据权利要求6所述的方法，其特征在于，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)小于或等于设定阈值CY而表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差足够小、同时在第i时间间隔|ΔαM(i)-ΔαT(i)|≥|ΔαO(i)-ΔαT(i)|、并且|ΔαO(i)-ΔαT(i)|≥Cα的情况下，令
Figure FDA0003426255580000051
其中，Cα为设定弧度阈值，D为所述两个驱动轮中心的距离值，D1为所述光学位移检测装置到所述两个驱动轮中心的连线的垂足与左驱动轮中心的距离值，ΔyM(i-1)为上一时间间隔ΔyM(i)的取值，Δyo(i-1)为上一时间间隔ΔyO(i)的取值。
15.根据权利要求6所述的方法，其特征在于，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY而表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差大、所述参数Eα(i)大于设定阈值Cd+而表明近期内旋转角度ΔαO(i)比旋转角度ΔαM(i)更接近旋转角度ΔαT(i)、并且在第i时间间隔|Δαo(i)-ΔαT(i)|＜Cα的情况下，令ΔyN(i)＝ΔyO(i)，其中，Cα为设定弧度阈值。
16.根据权利要求6所述的方法，其特征在于，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY而表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差大、所述参数Eα(i)大于设定阈值Cd+而表明近期内旋转角度ΔαO(i)比旋转角度ΔαM(i)更接近旋转角度ΔαT(i)、并且在第i时间间隔|ΔαO(i)-ΔαT(i)|≥Cα的情况下，令
Figure FDA0003426255580000061
其中，Cα为设定弧度阈值，D为所述两个驱动轮中心的距离值，D1为所述光学位移检测装置到所述两个驱动轮中心的连线的垂足与左驱动轮中心的距离值，ΔyM(i-1)为上一时间间隔ΔyM(i)的取值，ΔyO(i-1)为上一时间间隔ΔyO(i)的取值。
17.根据权利要求6所述的方法，其特征在于，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY而表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差大、所述参数Eα(i)小于设定阈值Cd-而表明近期内旋转角度ΔαM(i)比旋转角度ΔαO(i)更接近旋转角度ΔαT(i)、并且在第i时间间隔|ΔαM(i)-ΔαT(i)|＜Cα的情况下，令ΔyN(i)＝ΔyM(i)，其中，Cα为设定弧度阈值。
18.根据权利要求6所述的方法，其特征在于，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY而表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差大、所述参数Eα(i)小于设定阈值Cd-而表明近期内旋转角度ΔαM(i)比旋转角度ΔαO(i)更接近旋转角度ΔαT(i)、并且在第i时间间隔|ΔαM(i)-ΔαT(i)|≥Cα的情况下，令
Figure FDA0003426255580000062
Figure FDA0003426255580000071
其中，Cα为设定弧度阈值，D为所述两个驱动轮中心的距离值，D1为所述光学位移检测装置到所述两个驱动轮中心的连线的正投影点与左驱动轮中心的距离值，ΔyM(i-1)为上一时间间隔ΔyM(i)的取值，Δyo(i-1)为上一时间间隔ΔyO(i)的取值。
19.根据权利要求6所述的方法，其特征在于，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY而表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差大、所述参数Eα(i)大于或等于设定阈值Cd-且所述参数Eα(i)小于或等于设定阈值Cd+而表明近期内旋转角度ΔαM(i)和旋转角度ΔαO(i)与旋转角度ΔαT(i)的接近程度无明显差异、ES(i)＜CS-而表明近期内所述光学位移检测装置的输出明显失效、并且在第i时间间隔|ΔαM(i)-ΔαT(i)|＜Cα的情况下，令ΔyN(i)＝ΔyM(i)，其中，Cα为设定弧度阈值，CS-为设定失效评价阈值。
20.根据权利要求6所述的方法，其特征在于，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY而表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差大、所述参数Eα(i)大于或等于设定阈值Cd-且所述参数Eα(i)小于或等于设定阈值Cd+而表明近期内旋转角度ΔαM(i)和旋转角度ΔαO(i)与旋转角度ΔαT(i)的接近程度无明显差异、ES(i)＜CS-而表明近期内所述光学位移检测装置的输出明显失效、并且在第i时间间隔|ΔαM(i)-ΔαT(i)|≥Cα的情况下，令
Figure FDA0003426255580000072
Figure FDA0003426255580000081
其中，Cα为设定弧度阈值，D为所述两个驱动轮中心的距离值，D1为所述光学位移检测装置到所述两个驱动轮中心的连线的正投影点与左驱动轮中心的距离值，ΔyM(i-1)为上一时间间隔ΔyM(i)的取值，Δyo(i-1)为上一时间间隔ΔyO(i)的取值，CS-为设定失效评价阈值。
21.根据权利要求6所述的方法，其特征在于，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY而表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差大、所述参数Eα(i)大于或等于设定阈值Cd-且所述参数Eα(i)小于或等于设定阈值Cd+而表明近期内旋转角度ΔαM(i)和旋转角度ΔαO(i)与旋转角度ΔαT(i)的接近程度无明显差异、ES(i)＞CS+而表明近期内所述光学位移检测装置的输出可靠、并且在第i时间间隔|ΔαO(i)-ΔαT(i)|＜Cα的情况下，令ΔyN(i)＝ΔyO(i)，其中，Cα为设定弧度阈值，CS+为设定有效评价阈值。
22.根据权利要求6所述的方法，其特征在于，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY而表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差大、所述参数Eα(i)大于或等于设定阈值Cd-且所述参数Eα(i)小于或等于设定阈值Cd+而表明近期内旋转角度ΔαM(i)和旋转角度ΔαO(i)与旋转角度ΔαT(i)的接近程度无明显差异、ES(i)＞CS+而表明近期内所述光学位移检测装置的输出可靠、并且在第i时间间隔|ΔαO(i)-ΔαT(i)|≥Cα的情况下，令
Figure FDA0003426255580000082
Figure FDA0003426255580000091
其中，Cα为设定弧度阈值，D为所述两个驱动轮中心的距离值，D1为所述光学位移检测装置到所述两个驱动轮中心的连线的正投影点与左驱动轮中心的距离值，ΔyM(i-1)为上一时间间隔ΔyM(i)的取值，ΔyO(i-1)为上一时间间隔ΔyO(i)的取值，CS+为设定有效评价阈值。
23.根据权利要求6所述的方法，其特征在于，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY而表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差大、所述参数Eα(i)大于或等于设定阈值Cd-且所述参数Eα(i)小于或等于设定阈值Cd+而表明近期内旋转角度ΔαM(i)和旋转角度ΔαO(i)与旋转角度ΔαT(i)的接近程度无明显差异、CS-≤ES(i)≤CS+而表明近期内所述光学位移检测装置的输出可靠性不明确、参数Ez(i)大于设定阈值CZ而表明光学位移检测装置的数据近期内较少地接近于0，并且在第i时间间隔|ΔαO(i)-ΔαT(i)|＜Cα的情况下，令ΔyN(i)＝ΔyO(i)，其中，Cα为设定弧度阈值，CS-为设定失效评价阈值，CS+为设定有效评价阈值。
24.根据权利要求6所述的方法，其特征在于，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY而表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差大、所述参数Eα(i)大于或等于设定阈值Cd-且所述参数Eα(i)小于或等于设定阈值Cd+而表明近期内旋转角度ΔαM(i)和旋转角度ΔαO(i)与旋转角度ΔαT(i)的接近程度无明显差异、CS-≤ES(i)≤CS+而表明近期内所述光学位移检测装置的输出可靠性不明确、参数Ez(i)大于设定阈值Cd-而表明光学位移检测装置的数据近期内较少地接近于0，并且在第i时间间隔|ΔαO(i)-ΔαT(i)|≥Cα的情况下，令
Figure FDA0003426255580000101
其中，Cα为设定弧度阈值，D为所述两个驱动轮中心的距离值，D1为所述光学位移检测装置到所述两个驱动轮中心的连线的正投影点与左驱动轮中心的距离值，ΔyM(i-1)为上一时间间隔ΔyM(i)的取值，ΔyO(i-1)为上一时间间隔ΔyO(i)的取值，CS-为设定失效评价阈值，CS+为设定有效评价阈值。
25.根据权利要求6所述的方法，其特征在于，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY而表明近期内位移变量ΔyO(i)和所述移动距离ΔyM(i)的偏差大、所述参数Eα(i)大于或等于设定阈值Cd-且所述参数Eα(i)小于或等于设定阈值Cd+而表明近期内旋转角度ΔαM(i)和旋转角度ΔαO(i)与旋转角度ΔαT(i)的接近程度无明显差异、CS-≤ES(i)≤CS+而表明近期内所述光学位移检测装置的输出可靠性不明确、参数Ez(i)小于或等于设定阈值CZ而表明光学位移检测装置的数据近期内较多地接近于0，并且在第i时间间隔|ΔαM(i)-ΔαT(i)|＜Cα的情况下，令ΔyN(i)＝ΔyM(i)，其中，Cα为设定弧度阈值，CS-为设定失效评价阈值，CS+为设定有效评价阈值。
26.根据权利要求8所述的方法，其特征在于，根据参数EY(i)、参数Eα(i)、参数ES(i)和参数Ez(i)中的至少一个参数确定所述两个驱动轮中心的连线中点的移动距离ΔyN(i)，包括：
在所述参数EY(i)大于设定阈值CY而表明近期内位移变量Δyo(i)和所述移动距离ΔyM(i)的偏差大、所述参数Eα(i)大于或等于设定阈值Cd-且所述参数Eα(i)小于或等于设定阈值Cd+而表明近期内旋转角度ΔαM(i)和旋转角度ΔαO(i)与旋转角度ΔαT(i)的接近程度无明显差异、CS-≤ES(i)≤CS+而表明近期内所述光学位移检测装置的输出可靠性不明确、参数Ez(i)小于或等于设定阈值CZ而表明光学位移检测装置的数据近期内较多地接近于0，并且在第i时间间隔|ΔαM(i)-ΔαT(i)|≥Cα的情况下，令
Figure FDA0003426255580000111
其中，Cα为设定弧度阈值，D为所述两个驱动轮中心的距离值，D1为所述光学位移检测装置到所述两个驱动轮中心的连线的正投影点与左驱动轮中心的距离值，ΔyM(i-1)为上一时间间隔ΔyM(i)的取值，ΔyO(i-1)为上一时间间隔ΔyO(i)的取值，CS-为设定失效评价阈值，CS+为设定有效评价阈值。
27.根据权利要求1所述的方法，其特征在于，根据所述移动距离ΔyN(i)计算所述两个驱动轮中心的连线中点在固定坐标系的坐标位置，包括：
根据所述移动距离ΔyN(i)在所述可移动载体的坐标系计算在其两个坐标轴所在方向的移动距离ΔxR(i)和移动距离ΔyR(i)；
根据所述陀螺仪在上一时间间隔的总转角αT(i-1)得到所述可移动载体在固定坐标系沿其两个坐标轴所在方向的移动距离ΔxF(i)和移动距离ΔyF(i)；
根据所述移动距离ΔxF(i)和所述移动距离ΔyF(i)在所述固定坐标系更新所述可移动载体的坐标。
28.一种可移动载体的位移检测装置，其特征在于，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行计算机程序时实现根据权利要求1至27任一项所述的方法。
29.一种扫地机器人，其特征在于，在所述扫地机器人上安装有光学位移检测装置、左码表、右码表和陀螺仪，所述扫地机器人具有两个驱动轮，所述光学位移检测装置在所述可移动载体所处平面的正投影与所述两个驱动轮中心的连线所在直线在所述可移动载体所处平面的正投影不重叠，所述左码表用于确定所述两个驱动轮中左驱动轮的移动距离，所述右码表用于确定所述两个驱动轮中右驱动轮的移动距离，所述扫地机器人还包括根据权利要求28所述的位移检测装置。
CN202111578633.7A 2021-12-22 2021-12-22 可移动载体的定位方法、装置和扫地机器人 Active CN114403760B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111578633.7A CN114403760B (zh) 2021-12-22 2021-12-22 可移动载体的定位方法、装置和扫地机器人

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111578633.7A CN114403760B (zh) 2021-12-22 2021-12-22 可移动载体的定位方法、装置和扫地机器人

Publications (2)

Publication Number Publication Date
CN114403760A true CN114403760A (zh) 2022-04-29
CN114403760B CN114403760B (zh) 2022-12-02

Family

ID=81267742

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111578633.7A Active CN114403760B (zh) 2021-12-22 2021-12-22 可移动载体的定位方法、装置和扫地机器人

Country Status (1)

Country Link
CN (1) CN114403760B (zh)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1037129A1 (en) * 1999-03-05 2000-09-20 Nomadic Technologies, Inc. Method and apparatus for mobile robot motion control
US20080082208A1 (en) * 2006-09-29 2008-04-03 Samsung Electronics Co., Ltd. Method, apparatus, and medium for controlling mobile device based on image of real space including the mobile device
US20180216941A1 (en) * 2015-07-31 2018-08-02 Tianjin University Indoor mobile robot position and posture measurement system based on photoelectric scanning and measurement method
CN108710376A (zh) * 2018-06-15 2018-10-26 哈尔滨工业大学 基于多传感器融合的slam与避障的移动底盘
CN110216678A (zh) * 2019-06-25 2019-09-10 韦云智 一种机器人的室内定位导航的方法
CN110231030A (zh) * 2019-06-28 2019-09-13 苏州瑞久智能科技有限公司 基于陀螺仪的扫地机器人角度最优估算方法
CN111089595A (zh) * 2019-12-30 2020-05-01 珠海市一微半导体有限公司 一种机器人的检测数据融合方法和主控芯片及机器人

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1037129A1 (en) * 1999-03-05 2000-09-20 Nomadic Technologies, Inc. Method and apparatus for mobile robot motion control
US20080082208A1 (en) * 2006-09-29 2008-04-03 Samsung Electronics Co., Ltd. Method, apparatus, and medium for controlling mobile device based on image of real space including the mobile device
US20180216941A1 (en) * 2015-07-31 2018-08-02 Tianjin University Indoor mobile robot position and posture measurement system based on photoelectric scanning and measurement method
CN108710376A (zh) * 2018-06-15 2018-10-26 哈尔滨工业大学 基于多传感器融合的slam与避障的移动底盘
CN110216678A (zh) * 2019-06-25 2019-09-10 韦云智 一种机器人的室内定位导航的方法
CN110231030A (zh) * 2019-06-28 2019-09-13 苏州瑞久智能科技有限公司 基于陀螺仪的扫地机器人角度最优估算方法
CN111089595A (zh) * 2019-12-30 2020-05-01 珠海市一微半导体有限公司 一种机器人的检测数据融合方法和主控芯片及机器人

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
李世云等: "基于光流传感器的移动机器人定位方法", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *

Also Published As

Publication number Publication date
CN114403760B (zh) 2022-12-02

Similar Documents

Publication Publication Date Title
CN108638053B (zh) 一种机器人打滑的检测方法及其矫正方法
JP3770909B2 (ja) 目標と自走ユニットとの間隔が自走ユニットによって動的に検出される、自走ユニットの環境マップ内の目標の位置特定方法
EP1435554B1 (en) Method for compensating angular position error of robot cleaner
KR100772915B1 (ko) 이동 로봇에 장착된 자이로의 바이어스를 보정하는 장치 및방법
EP1868056B1 (en) Moving apparatus, method, and medium for compensating position of the moving apparatus
JP3018497B2 (ja) 旋回角速度センサのオフセット補正装置
US5999866A (en) Infrastructure independent position determining system
KR100772912B1 (ko) 절대 방위각을 이용한 로봇 및 이를 이용한 맵 작성 방법
CN109394095B (zh) 一种机器人运动地毯偏移的控制方法、芯片及清洁机器人
CN110553652A (zh) 机器人多传感器融合定位方法及其应用
CN102666032A (zh) 用于移动机器人的滑动检测装置和方法
US9802619B2 (en) Moving amount estimating apparatus, autonomous mobile body, and moving amount estimating method
KR20090009172A (ko) 이동 로봇의 위치 측정 방법 및 장치
KR20170088228A (ko) 다중로봇의 자기위치인식에 기반한 지도작성 시스템 및 그 방법
KR20090008924A (ko) 이동 로봇의 자세 측정 방법 및 상기 방법을 이용한 위치측정 방법 및 장치
JP2006194856A (ja) 移動物体の方向検出方法及びそのシステム
KR101115012B1 (ko) 로봇의 각속도 오차 보정 장치 및 방법
CN111637877B (zh) 机器人定位方法、装置、电子设备和非易失性存储介质
JP2001510890A (ja) 自律的移動ユニットの回転状態の検出のための方法及び自律的移動ユニット
CN114403760B (zh) 可移动载体的定位方法、装置和扫地机器人
KR20110081701A (ko) 캘리브레이션 장치
Madhavan et al. Terrain-aided localization of autonomous ground vehicles
KR101802650B1 (ko) 배관의 위치 추정 시스템 및 그 방법
CN112697153A (zh) 自主移动设备的定位方法、电子设备及存储介质
KR101255024B1 (ko) 초음파 센서를 이용한 상대적 위치 추정 시스템 및 방법

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant