CN110221609A - 基于二维码的轨迹纠偏方法 - Google Patents

Abstract

本发明公开了基于二维码的轨迹纠偏方法，二维码沿Y轴间隔直线排列，轨迹纠偏方法包括：移动机器人运动至与任意一个二维码在X轴上平齐，以该二维码为初始二维码，读取初始二维码的坐标信息，分析移动机器人与初始二维码的位置关系，判断位置关系是否异常，若异常则根据位置关系从预设的纠偏数学模型库中获取对应的纠偏数学模型；根据纠偏数学模型和位置关系计算移动机器人的纠偏参数，移动机器人按照纠偏参数运动至与下一个二维码在X轴上平齐。本发明的纠偏过程简单灵活，施工成本小、难度低。

Description

基于二维码的轨迹纠偏方法

技术领域

本发明涉及导航控制技术领域，尤其涉及基于二维码的轨迹纠偏方法。

背景技术

移动机器人的导航方式很多，目前市面上使用的有磁条导航、磁钉与惯导导航、SLAM视觉导航等。其中磁条导航局限性太大，使用起来不够灵活；磁钉导航需要在地面嵌入磁钉，对地面造成破坏，施工难度大且不易维护；SLAM视觉导航成本太高，且技术还不是很成熟。

因此，如何设计算法简单灵活的轨迹纠偏方法是业界亟待解决的技术问题。

发明内容

为了解决现有导航方式存在局限性大、成本高、纠偏效果差的缺陷，本发明提出基于二维码的轨迹纠偏方法。

本发明采用的技术方案是，设计基于二维码的轨迹纠偏方法，二维码沿Y轴间隔直线排列，轨迹纠偏方法包括：移动机器人运动至与任意一个二维码在X轴上平齐，以该二维码为初始二维码，读取初始二维码的坐标信息，分析移动机器人与初始二维码的位置关系，判断位置关系是否异常，若异常则根据位置关系从预设的纠偏数学模型库中获取对应的纠偏数学模型；根据纠偏数学模型和位置关系计算移动机器人的纠偏参数，移动机器人按照纠偏参数运动至与下一个二维码在X轴上平齐。

优选的，判断位置关系是否异常，若正常则移动机器人按照当前状态继续运动至与下一个二维码在X轴上平齐。

优选的，分析移动机器人与初始二维码的位置关系包括：以初始二维码的中心为坐标系原点，计算移动机器人与初始二维码在X轴上的偏差e、移动机器人的行进方向和Y轴之间的航向角α，根据偏差e和航向角α的正负分析移动机器人与初始二维码的位置关系。

优选的，偏差e和航向角α的正负判断移动机器人与初始二维码的位置关系包括：当e≤0、α<0时为第一种，移动机器人偏左、航向角左偏；当e<0、α＝0时为第二种，移动机器人偏左、航向角正常；当e<0、α>0时为第三种，移动机器人偏左、航向角右偏；当e＝0、α＝0时为第四种，移动机器人正常、航向角正常；当e>0、α<0时为第五种，移动机器人偏右、航向角左偏；当e>0、α＝0时为第六种，移动机器人偏右、航向角正常；当e≥0、α>0时为第七种，移动机器人偏右、航向角右偏。

优选的，判断位置关系是否异常包括：当位置关系为第一种、第二种、第三种、第五种、第六种及第七种的任意一种时，则判断位置关系异常。

优选的，判断位置关系是否异常还包括：当位置关系为第四种时，则判断位置关系正常。

优选的，纠偏数学模型库中设有第一纠偏数学模型、第二纠偏数学模型和第三纠偏数学模型，位置关系为第三种或第五种时对应第一纠偏数学模型，位置关系为第一种或第七种时对应第三纠偏数学模型，位置关系为第二种或第六种时对应第二纠偏模型。

优选的，第一纠偏数学模型包括：m为初始二维码到下一个二维码的距离，L为移动机器人两侧驱动轮的中心距，v为移动机器人运动到初始二维码时的行进速度；当α<θ时，移动机器人按照当前状态继续运动至与下一个二维码在X轴上平齐；当α≥θ时，移动机器人的一侧驱动轮速度为pv、另一侧驱动轮速度为v，移动机器人运动到与下一个二维码在X轴上平齐后，两侧驱动轮速度均调整为v。

优选的，位置关系为第三种时从纠偏数学模型库中获取第一纠偏数学模型，当α≥θ时，移动机器人的纠偏参数为左侧驱动轮速度为v、右侧驱动轮速度为pv。

优选的，位置关系为第五种时从纠偏数学模型库中获取第一纠偏数学模型，当α≥θ时，移动机器人的纠偏参数为左侧驱动轮速度为pv、右侧驱动轮速度为v。

优选的，第二纠偏数学模型包括：m为初始二维码到下一个二维码的距离，L为移动机器人两侧驱动轮的中心距，v为移动机器人运动到初始二维码时的行进速度；移动机器人以一侧驱动轮速度为v、另一侧驱动轮速度为pv运动T时间后，移动机器人切换两轮的速度继续运动，移动机器人运动到与下一个二维码在X轴上平齐后，两侧驱动轮速度均调整为v。

优选的，位置关系为第二种时从纠偏数学模型库中获取第二纠偏数学模型，移动机器人的纠偏参数为移动机器人以左侧驱动轮速度为pv、右侧驱动轮速度为v运动T时间后，移动机器人再以左侧驱动轮速度为v、右侧驱动轮速度为pv继续运动。

优选的，位置关系为第六种时从纠偏数学模型库中获取第二纠偏数学模型，移动机器人的纠偏参数为移动机器人以左侧驱动轮速度为v、右侧驱动轮速度为pv运动T时间后，移动机器人再以左侧驱动轮速度为pv、右侧驱动轮速度为v继续运动。

优选的，第三纠偏数学模型包括： P₃＝P₂，m为初始二维码到下一个二维码的距离，L为移动机器人两侧驱动轮的中心距，v为移动机器人运动到初始二维码时的行进速度；移动机器人以一侧驱动轮速度为v、另一侧驱动轮速度为p₁v运动T₁时间后，移动机器人调整两轮的速度，以一侧驱动轮速度为v、另一侧驱动轮速度为p₂v运动T₂时间后，移动机器人再次调整两轮的速度，以一侧驱动轮速度为p₃v、另一侧驱动轮速度为v继续运动，移动机器人运动到与下一个二维码在X轴上平齐后，两侧驱动轮速度均调整为v。

优选的，位置关系为第一种时从纠偏数学模型库中获取第三纠偏数学模型，移动机器人的纠偏参数为移动机器人以右侧驱动轮速度为v、左侧驱动轮速度为p₁v运动T₁时间后，移动机器人调整两轮的速度，以右侧驱动轮速度为v、左侧驱动轮速度为p₂v运动T₂时间后，移动机器人再次调整两轮的速度，以右侧驱动轮速度为p₃v、左侧驱动轮速度为v继续运动。

优选的，位置关系为第七种时从纠偏数学模型库中获取第三纠偏数学模型，移动机器人的纠偏参数为移动机器人以右侧驱动轮速度为p₁v、左侧驱动轮速度为v运动T₁时间后，移动机器人调整两轮的速度，以右侧驱动轮速度为p₂v、左侧驱动轮速度为v运动T₂时间后，移动机器人再次调整两轮的速度，以右侧驱动轮速度为v、左侧驱动轮速度为p₃v继续运动。

与现有技术相比，本发明预先设置有纠偏数学模型库，根据移动机器人行进过程中扫描到的二维码信息分析当前移动机器人的位置关系，根据位置关系从纠偏数学模型库中调取对应的纠偏数学模型计算纠偏参数，纠偏过程简单灵活，二维码可粘贴或印制在地面上，施工成本小、难度低。

附图说明

下面结合实施例和附图对本发明进行详细说明，其中：

图1是本发明中轨迹纠偏方法的流程示意图；

图2是本发明中第一纠偏数学模型的示意图；

图3是本发明中第二纠偏数学模型的示意图；

图4是本发明中第三纠偏数学模型的示意图。

具体实施方式

如图1所示，本发明提出的轨迹纠偏方法基于二维码，二维码沿Y轴间隔直线排列，轨迹纠偏方法包括：移动机器人运动至与任意一个二维码在X轴上平齐，以该二维码为初始二维码，移动机器人具有左侧驱动轮和右侧驱动轮，左侧驱动轮和右侧驱动轮的速度可以单独控制，移动机器人上设置有摄像头，通过摄像头扫描初始二维码，读取初始二维码的坐标信息，分析移动机器人与初始二维码的位置关系，判断位置关系是否异常，若位置关系异常，则根据位置关系从预设的纠偏数学模型库中获取对应的纠偏数学模型；若位置关系正常，则移动机器人按照当前状态继续运动至与下一个二维码在X轴上平齐，移动机器人到达下一个二维码时，此二维码为初始二维码，重复上面的扫描分析动作。需要说明的是，移动机器人按照当前状态继续运动是保持移动机器人的行进方向不变、保持移动机器人两侧驱动轮的速度不变。

下面详细对轨迹纠偏方法中的细节进行说明，移动机器人的行进方向、移动机器人两侧驱动轮之间的中心距L和移动机器人的行进速度v是已知参数。

分析移动机器人与初始二维码的位置关系包括：以初始二维码的中心为坐标系原点，根据初始二维码的坐标信息计算移动机器人与初始二维码在X轴上的偏差e，移动机器人的行进方向和Y轴之间的航向角α，根据偏差e和航向角α的正负分析移动机器人与初始二维码的位置关系。位置关系有七种，当e≤0、α<0时为第一种，移动机器人偏左、航向角左偏；当e<0、α＝0时为第二种，移动机器人偏左、航向角正常；当e<0、α>0时为第三种，移动机器人偏左、航向角右偏；当e＝0、α＝0时为第四种，移动机器人正常、航向角正常；当e>0、α<0时为第五种，移动机器人偏右、航向角左偏；当e>0、α＝0时为第六种，移动机器人偏右、航向角正常；当e≥0、α>0时为第七种，移动机器人偏右、航向角右偏。

判断位置关系是否异常包括：当位置关系为第一种、第二种、第三种、第五种、第六种及第七种的任意一种时，则判断位置关系异常；当位置关系为第四种时，则判断位置关系正常。

纠偏数学模型库中设有第一纠偏数学模型、第二纠偏数学模型和第三纠偏数学模型，位置关系为第三种或第五种时对应第一纠偏数学模型，位置关系为第一种或第七种时对应第三纠偏数学模型，位置关系为第二种或第六种时对应第二纠偏模型。

如图2所示，第一纠偏数学模型包括：

上面的公式换算后，

其中，R为圆弧半径、θ为圆弧夹角、P为左右轮的转速比值、m为初始二维码到下一个二维码的距离、L为移动机器人两侧驱动轮的中心距、v为移动机器人运动到初始二维码时的行进速度；

当时，不做补偿动作，因为此时按照当前航向角即可到达靠近二维码的位置，做补偿处理会使机器人离下一个二维码更远，移动机器人按照当前状态继续运动至与下一个二维码在X轴上平齐；当α≥θ时，移动机器人的一侧驱动轮速度为pv、另一侧驱动轮速度为v，移动机器人运动到与下一个二维码在X轴上平齐后，两侧驱动轮速度均调整为v。

具体来说，位置关系为第三种时从纠偏数学模型库中获取第一纠偏数学模型，当α≥θ时，移动机器人的纠偏参数为左侧驱动轮速度为v、右侧驱动轮速度为pv。位置关系为第五种时从纠偏数学模型库中获取第一纠偏数学模型，当α≥θ时，移动机器人的纠偏参数为左侧驱动轮速度为pv、右侧驱动轮速度为v。

如图3所示，第二纠偏数学模型包括：

上面的公式换算后，

其中M为平分m和e的中间点，R1＝R2，R1为第一段圆弧半径，R2为第二段圆弧半径，θ1为第一段圆弧夹角，θ2为第二段圆弧夹角，P为左右轮的转速比值，T为小车在第一段运行的时间，m为初始二维码到下一个二维码的距离，L为移动机器人两侧驱动轮的中心距，v为移动机器人运动到初始二维码时的行进速度。

移动机器人以一侧驱动轮速度为v、另一侧驱动轮速度为pv运动T时间后，即移动机器人运动到M点后，移动机器人切换两轮的速度继续运动，移动机器人运动到与下一个二维码在X轴上平齐后，两侧驱动轮速度均调整为v。

具体来说，位置关系为第二种时从纠偏数学模型库中获取第二纠偏数学模型，移动机器人的纠偏参数为移动机器人以左侧驱动轮速度为pv、右侧驱动轮速度为v运动T时间后，移动机器人再以左侧驱动轮速度为v、右侧驱动轮速度为pv继续运动。

位置关系为第六种时从纠偏数学模型库中获取第二纠偏数学模型，移动机器人的纠偏参数为移动机器人以左侧驱动轮速度为v、右侧驱动轮速度为pv运动T时间后，移动机器人再以左侧驱动轮速度为pv、右侧驱动轮速度为v继续运动。

如图4所示，第三纠偏数学模型包括：

上面的公式换算后，P₃＝P₂，

其中，M、N为三等分点，即过M、N做二维码连线的垂线，垂足为P、Q，则P、Q三等分于二维码距离m，且在N点位置时，移动机器人的行进方向为正方向，R1为第一段圆弧半径，R2为第二段圆弧半径，R3为第三段圆弧半径，θ1为第一段圆弧夹角，θ2为第二段圆弧夹角，θ3为第三段圆弧夹角，P1为第一段左右轮的转速比值，P2为第二段左右轮的转速比值，T1为小车在第一段运行的时间，T2为小车在第二段运行的时间，m为初始二维码到下一个二维码的距离，L为移动机器人两侧驱动轮的中心距，v为移动机器人运动到初始二维码时的行进速度。

移动机器人以一侧驱动轮速度为v、另一侧驱动轮速度为p₁v运动T₁时间后，移动机器人调整两轮的速度，以一侧驱动轮速度为v、另一侧驱动轮速度为p₂v运动T₂时间后，移动机器人再次调整两轮的速度，以一侧驱动轮速度为p₃v、另一侧驱动轮速度为v继续运动，移动机器人运动到与下一个二维码在X轴上平齐后，两侧驱动轮速度均调整为v。

具体来说，位置关系为第一种时从纠偏数学模型库中获取第三纠偏数学模型，移动机器人的纠偏参数为移动机器人以右侧驱动轮速度为v、左侧驱动轮速度为p₁v运动T₁时间后，移动机器人调整两轮的速度，以右侧驱动轮速度为v、左侧驱动轮速度为p₂v运动T₂时间后，移动机器人再次调整两轮的速度，以右侧驱动轮速度为p₃v、左侧驱动轮速度为v继续运动。

位置关系为第七种时从纠偏数学模型库中获取第三纠偏数学模型，移动机器人的纠偏参数为移动机器人以右侧驱动轮速度为p₁v、左侧驱动轮速度为v运动T₁时间后，移动机器人调整两轮的速度，以右侧驱动轮速度为p₂v、左侧驱动轮速度为v运动T₂时间后，移动机器人再次调整两轮的速度，以右侧驱动轮速度为v、左侧驱动轮速度为p₃v继续运动。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (16)

1.一种轨迹纠偏方法，其特征在于，所述二维码沿Y轴间隔直线排列，所述轨迹纠偏方法包括：

移动机器人运动至与任一所述二维码在X轴上平齐，以该二维码为初始二维码，读取所述初始二维码的坐标信息，分析所述移动机器人与所述初始二维码的位置关系；

判断所述位置关系是否异常，若异常则根据所述位置关系从预设的纠偏数学模型库中获取对应的纠偏数学模型；

根据所述纠偏数学模型和所述位置关系计算所述移动机器人的纠偏参数，所述移动机器人按照所述纠偏参数运动至与下一个所述二维码在X轴上平齐。

2.如权利要求1所述的轨迹纠偏方法，其特征在于，所述判断位置关系是否异常，若正常则所述移动机器人按照当前状态继续运动至与下一个所述二维码在X轴上平齐。

3.如权利要求1所述的轨迹纠偏方法，其特征在于，所述分析移动机器人与所述初始二维码的位置关系包括：以初始二维码的中心为坐标系原点，计算移动机器人与所述初始二维码在X轴上的偏差e、移动机器人的行进方向和Y轴之间的航向角α，根据偏差e和航向角α的正负分析所述移动机器人与所述初始二维码的位置关系。

4.如权利要求3所述的轨迹纠偏方法，其特征在于，所述偏差e和航向角α的正负判断所述移动机器人与所述初始二维码的位置关系包括：当e≤0、α<0时为第一种，移动机器人偏左、航向角左偏；当e<0、α＝0时为第二种，移动机器人偏左、航向角正常；当e<0、α>0时为第三种，移动机器人偏左、航向角右偏；当e＝0、α＝0时为第四种，移动机器人正常、航向角正常；当e>0、α<0时为第五种，移动机器人偏右、航向角左偏；当e>0、α＝0时为第六种，移动机器人偏右、航向角正常；当e≥0、α>0时为第七种，移动机器人偏右、航向角右偏。

5.如权利要求4所述的轨迹纠偏方法，其特征在于，所述判断位置关系是否异常包括：当所述位置关系为第一种、第二种、第三种、第五种、第六种及第七种的任意一种时，则判断所述位置关系异常。

6.如权利要求4所述的轨迹纠偏方法，其特征在于，所述判断位置关系是否异常还包括：当所述位置关系为第四种时，则判断所述位置关系正常。

7.如权利要求4所述的轨迹纠偏方法，其特征在于，所述纠偏数学模型库中设有第一纠偏数学模型、第二纠偏数学模型和第三纠偏数学模型，所述位置关系为第三种或第五种时对应第一纠偏数学模型，所述位置关系为第一种或第七种时对应第三纠偏数学模型，所述位置关系为第二种或第六种时对应第二纠偏模型。

8.如权利要求7所述的轨迹纠偏方法，其特征在于，所述第一纠偏数学模型包括：m为所述初始二维码到下一个二维码的距离，L为所述移动机器人两侧驱动轮的中心距，v为所述移动机器人运动到初始二维码时的行进速度；当α<θ时，所述移动机器人按照当前状态继续运动至与下一个所述二维码在X轴上平齐；当α≥θ时，所述移动机器人的一侧驱动轮速度为pv、另一侧驱动轮速度为v，所述移动机器人运动到与下一个二维码在X轴上平齐后，两侧驱动轮速度均调整为v。

9.如权利要求8所述的轨迹纠偏方法，其特征在于，所述位置关系为第三种时从所述纠偏数学模型库中获取第一纠偏数学模型，当α≥θ时，所述移动机器人的纠偏参数为左侧驱动轮速度为v、右侧驱动轮速度为pv。

10.如权利要求8所述的轨迹纠偏方法，其特征在于，所述位置关系为第五种时从所述纠偏数学模型库中获取第一纠偏数学模型，当α≥θ时，所述移动机器人的纠偏参数为左侧驱动轮速度为pv、右侧驱动轮速度为v。

11.如权利要求7所述的轨迹纠偏方法，其特征在于，所述第二纠偏数学模型包括：m为所述初始二维码到下一个二维码的距离，L为所述移动机器人两侧驱动轮的中心距，v为所述移动机器人运动到初始二维码时的行进速度；所述移动机器人以一侧驱动轮速度为v、另一侧驱动轮速度为pv运动T时间后，所述移动机器人切换两轮的速度继续运动，所述移动机器人运动到与下一个二维码在X轴上平齐后，两侧驱动轮速度均调整为v。

12.如权利要求11所述的轨迹纠偏方法，其特征在于，所述位置关系为第二种时从所述纠偏数学模型库中获取第二纠偏数学模型，所述移动机器人的纠偏参数为移动机器人以左侧驱动轮速度为pv、右侧驱动轮速度为v运动T时间后，所述移动机器人再以左侧驱动轮速度为v、右侧驱动轮速度为pv继续运动。

13.如权利要求11所述的轨迹纠偏方法，其特征在于，所述位置关系为第六种时从所述纠偏数学模型库中获取第二纠偏数学模型，所述移动机器人的纠偏参数为移动机器人以左侧驱动轮速度为v、右侧驱动轮速度为pv运动T时间后，所述移动机器人再以左侧驱动轮速度为pv、右侧驱动轮速度为v继续运动。

14.如权利要求7所述的轨迹纠偏方法，其特征在于，所述第三纠偏数学模型包括： m为所述初始二维码到下一个二维码的距离，L为所述移动机器人两侧驱动轮的中心距，v为所述移动机器人运动到初始二维码时的行进速度；所述移动机器人以一侧驱动轮速度为v、另一侧驱动轮速度为p₁v运动T₁时间后，所述移动机器人调整两轮的速度，以一侧驱动轮速度为v、另一侧驱动轮速度为p₂v运动T₂时间后，所述移动机器人再次调整两轮的速度，以一侧驱动轮速度为p₃v、另一侧驱动轮速度为v继续运动，所述移动机器人运动到与下一个二维码在X轴上平齐后，两侧驱动轮速度均调整为v。

15.如权利要求14所述的轨迹纠偏方法，其特征在于，所述位置关系为第一种时从所述纠偏数学模型库中获取第三纠偏数学模型，所述移动机器人的纠偏参数为移动机器人以右侧驱动轮速度为v、左侧驱动轮速度为p₁v运动T₁时间后，所述移动机器人调整两轮的速度，以右侧驱动轮速度为v、左侧驱动轮速度为p₂v运动T₂时间后，所述移动机器人再次调整两轮的速度，以右侧驱动轮速度为p₃v、左侧驱动轮速度为v继续运动。

16.如权利要求14所述的轨迹纠偏方法，其特征在于，所述位置关系为第七种时从所述纠偏数学模型库中获取第三纠偏数学模型，所述移动机器人的纠偏参数为移动机器人以右侧驱动轮速度为p₁v、左侧驱动轮速度为v运动T₁时间后，所述移动机器人调整两轮的速度，以右侧驱动轮速度为p₂v、左侧驱动轮速度为v运动T₂时间后，所述移动机器人再次调整两轮的速度，以右侧驱动轮速度为v、左侧驱动轮速度为p₃v继续运动。