CN108896999B - 一种全闭环高精度导航系统及方法 - Google Patents

Abstract

本发明公开了一种全闭环高精度导航系统及方法，该系统包括激光跟踪仪、激光跟踪靶球及坐标转化控制器。该方法包括：激光跟踪仪对激光跟踪靶球进行跟踪，以获取得到激光跟踪靶球的坐标位置；坐标转化控制器根据激光跟踪靶球的坐标位置，从而确定出当前车体的坐标位置及车体方向，按照所需途经的站点和/或路线，对当前车体的坐标位置及车体方向进行数据处理后，将处理后得到的坐标结果和车体方向结果反馈至车体进行车体行驶位置和方向的修正。通过本发明来实现车体行驶路线的自动导航，不仅易于实施和安装，而且定位精度可高达0.015mm+0.005mm/m。本发明作为一种全闭环高精度导航系统及方法可广泛应用于移动机器人技术领域中。

Description

一种全闭环高精度导航系统及方法

技术领域

本发明涉及移动机器人技术领域，尤其涉及一种应用于自导航叉车、AGV(Automated Guided Vehicle，自动导向车)中的全闭环高精度导航系统及方法。

背景技术

自导航叉车、AGV是一种按照已经规划完成的地图(该地图包含路线及站点等信息)，在中央管理系统、调度系统或人工操作的指令下，自动选择路线行驶，且自动到达站点后，完成指令特定工作(如货物装卸，零部件安装，点送餐服务等)的无人自动行驶设备或移动机器人。

目前，市场上针对自导航叉车、AGV的导航方式主要包含有：磁导航、光学导航、视觉导航、超声波导航、惯性导航以及激光反射导航。其中，激光导航的定位精度相对较高，所以其受到更广泛的应用。所述激光导航又具体分为有反射板与无反射板两种，其中，有反射板方式的工作原理为：通过在行驶路径的周围安装位置精确的激光反射板后，通过激光扫描器发射激光束，同时采集由反射板反射的激光束，基于采集到的激光束，从而通过运算来确定其当前的位置和航向，并通过连续的三角几何运算来实现叉车、AGV的运行导引。由于有反射板的激光导航方式的导航精度相对较好，定位精度可以达到约±4mm，因此，目前多数激光导航采用的导航方式为有反射板方式。然而，市场在对自导航叉车、AGV的应用中，发现有些工作场所，因其场所环境条件受限，有反射板的激光导航的定位精度已无法满足该工作场所，因此由此可见，提供一种更高精度的自导航叉车/AGV导航定位方案是目前迫切需要解决的问题之一。

发明内容

为了解决上述技术问题，本发明的目的是提供一种应用于自导航叉车/AGV中的全闭环高精度导航系统及方法。

本发明所采用的第一技术方案是：一种全闭环高精度导航系统，该系统包括激光跟踪仪、激光跟踪靶球及坐标转化控制器；

所述激光跟踪仪，用于对激光跟踪靶球进行跟踪，以获取得到激光跟踪靶球的坐标位置；

所述激光跟踪靶球，其移动受控于靶球移动机构；

所述坐标转化控制器，用于根据激光跟踪靶球的坐标位置，从而确定出当前车体的坐标位置及车体方向，按照所需途经的站点和/或路线，对当前车体的坐标位置及车体方向进行数据处理后，将处理后得到的坐标结果和车体方向结果反馈至车体进行车体行驶位置和方向的修正。

进一步，所述根据激光跟踪靶球的坐标位置，从而确定出当前车体的车体方向，其具体包括：

获取激光跟踪靶球在规律性移动时的坐标，将获取得到的激光跟踪靶球的坐标拟合成对应的曲线，根据所述曲线，从而解析得出当前车体的车体方向。

进一步，所述根据激光跟踪靶球的坐标位置，从而确定出当前车体的车体方向，其具体包括：

获取激光跟踪靶球的第一坐标位置和第二坐标位置，其中，所述第一坐标位置指的是激光跟踪靶球受靶球移动机构进行位置切换移动前的坐标位置，所述第二坐标位置指的是激光跟踪靶球受靶球移动机构进行位置切换移动后的坐标位置；

根据第一坐标位置和第二坐标位置中的X轴坐标和Y轴坐标，计算出第一直线的第一斜率，其中，所述第一直线是由第一坐标位置和第二坐标位置中的X轴坐标和Y轴坐标确定得出；

根据计算得出的第一斜率，计算出第一直线的第一倾斜角度；

根据计算出的第一倾斜角度，确定得到当前车体的车体方向。

进一步，所述坐标转化控制器还用于根据激光跟踪靶球的坐标位置，计算得出绕Y轴的角度和/或绕X轴的角度；其中，所述X轴和Y轴指的是车体运行环境坐标系中的X轴和Y轴。

进一步，所述根据激光跟踪靶球的坐标位置，计算得出绕Y轴的角度，其具体包括：

获取激光跟踪靶球的第三坐标位置和第四坐标位置，其中，所述第三坐标位置指的是激光跟踪靶球在沿X轴移动第一固定距离L1前的坐标位置，所述第四坐标位置指的是激光跟踪靶球在沿X轴移动第一固定距离L1后的坐标位置；

根据第三坐标位置和第四坐标位置中的Z轴坐标以及第一固定距离L1，计算得出绕Y轴的角度；

所述绕Y轴的角度P的计算公式具体为：

其中，Z₃′表示为第三坐标位置中的Z轴坐标，z₄′表示为第四坐标位置中的Z轴坐标。

进一步，所述根据激光跟踪靶球的坐标位置，计算得出绕X轴的角度，其具体包括：

获取激光跟踪靶球的第五坐标位置和第六坐标位置，其中，所述第五坐标位置指的是激光跟踪靶球在沿Y轴移动第二固定距离L2前的坐标位置，所述第六坐标位置指的是激光跟踪靶球在沿Y轴移动第二固定距离L2后的坐标位置；

根据第五坐标位置和第六坐标位置中的Z轴坐标以及第二固定距离L2，计算得出绕X轴的角度；

所述绕X轴的角度W的计算公式具体为：

其中，z₅′表示为第五坐标位置中的Z轴坐标，z₆′表示为第六坐标位置中的Z轴坐标。

进一步，所述曲线为正弦曲线；所述根据所述曲线，从而解析得出当前车体的车体方向，其具体包括：

从正弦曲线中找出两个对应正峰值或负峰值的坐标位置；

根据找出的两个坐标位置中的X轴坐标和Y轴坐标，计算出第二直线的第二斜率，其中，所述第二直线是由找出的两个坐标位置中的X轴坐标和Y轴坐标确定得出；

根据计算得出的第二斜率，计算出第二直线的第二倾斜角度；

根据计算出的第二倾斜角度，确定得到当前车体的车体方向。

本发明所采用的第二技术方案是：一种全闭环高精度导航方法，包括以下步骤：

激光跟踪仪对激光跟踪靶球进行跟踪，以获取得到激光跟踪靶球的坐标位置；

坐标转化控制器根据激光跟踪靶球的坐标位置，从而确定出当前车体的坐标位置及车体方向，按照所需途经的站点和/或路线，对当前车体的坐标位置及车体方向进行数据处理后，将处理后得到的坐标结果和车体方向结果反馈至车体进行车体行驶位置和方向的修正。

进一步，还包括构建步骤；所述构建步骤具体包括：

利用激光跟踪仪和激光跟踪靶球来建立车体运行环境坐标系；

在车体运行环境坐标系下创建车体运行的站点、路线和/或地图。

进一步，还包括以下步骤：

根据激光跟踪靶球的坐标位置，计算得出绕Y轴的角度和/或绕X轴的角度；其中，所述X轴和Y轴指的是车体运行环境坐标系中的X轴和Y轴。

本发明系统及方法的有益效果是：本发明将激光跟踪仪、激光跟踪靶球及坐标转化控制器组合应用于自导航叉车和/或AGV中，通过激光跟踪仪对激光跟踪靶球进行跟踪，以获取得到激光跟踪靶球的坐标位置，然后坐标转化控制器便根据激光跟踪靶球的坐标位置，从而确定出当前车体的坐标位置及车体方向，按照所需途经的站点和/或路线，对当前车体的坐标位置及车体方向进行数据处理后，将处理后得到的坐标结果和车体方向结果反馈至车体进行车体行驶位置和方向的修正，以实现车体自动导航。可见，通过本发明来实现车体行驶路线的自动导航，其不仅易于实施和安装，无需在路面进行任何标识标记，对车体的运行路面要求低，而且定位精度可高达0.015mm+0.005mm/m，具有定位精度非常高的优点，基本可满足各种工作场所环境的精度需要，应用范围更广且商用价值更高。

附图说明

图1是本发明一种全闭环高精度导航系统的一具体实施例结构示意图；

图2是检测车体方向的第一具体实施例原理示意图；

图3是检测车体方向的第二具体实施例原理示意图；

图4是计算多自由度的一具体实施例原理示意图；

图5是本发明一种全闭环高精度导航方法的一具体实施例步骤流程示意图。

附图标记：

1、车体；2、靶球移动机构；3、激光跟踪靶球；4、激光跟踪仪；5、曲线；

301、第一坐标位置；302、第二坐标位置。

具体实施方式

下面结合附图和具体实施例对本发明做进一步的详细说明。对于以下实施例中的步骤编号，其仅为了便于阐述说明而设置，对步骤之间的顺序不做任何限定，实施例中的各步骤的执行顺序均可根据本领域技术人员的理解来进行适应性调整。

如图1所示，本发明提供一种全闭环高精度导航系统，应用于自导航叉车和/或AGV中，该系统包括激光跟踪仪4、激光跟踪靶球3及坐标转化控制器；

所述激光跟踪仪4，用于对激光跟踪靶球3进行跟踪，以获取得到激光跟踪靶球3的坐标位置；

所述激光跟踪靶球3，其移动受控于靶球移动机构2；

所述坐标转化控制器(调度系统)，用于根据激光跟踪靶球3的坐标位置，从而确定出当前车体1的坐标位置及车体方向，按照所需途经的站点和/或路线，对当前车体1的坐标位置及车体方向进行数据处理后，将处理后得到的坐标结果和车体方向结果反馈至车体1进行车体行驶位置和方向的修正，以实现车体1行驶位置和方向的调整控制，达到车体行驶自动导航的目的，令车体沿所需途径的站点/路线进行自动行驶。其中，所述激光跟踪仪4所获取得到的激光跟踪靶球3的坐标为(X,Y,Z)，X、Y、Z分别表示为车体运行环境坐标系中的X轴坐标、Y轴坐标和Z轴坐标，且此坐标系的原点为O；所述车体运行环境坐标系是利用激光跟踪仪4和激光跟踪靶球3来建立的，并且在车体运行环境坐标系下创建了车体运行的站点、路线和/或地图。

具体地，所述激光跟踪仪4以固定方式安装在场所中，而激光跟踪靶球3则安装在车体1的靶球移动机构2上，或者，两者位置互换方式，即将设置在靶球移动机构2上的激光跟踪靶球3安装在场所中，而激光跟踪仪4则安装在车体1。对于这两种安装方式，它们的实现原理相同，在实际应用中可根据实际需求来进行选择便可。优选地，在一实施例中(如图1所示)，将所述激光跟踪仪4以固定方式安装在场所中，而激光跟踪靶球3则安装在车体1的靶球移动机构2上。

在一优选实施例中，对于所述根据激光跟踪靶球3的坐标位置，从而确定出当前车体1的坐标位置，其可具体包括：

每获取得到一激光跟踪靶球3的坐标位置时，则直接将该激光跟踪靶球3的坐标位置作为当前车体1的坐标位置，也就是说，一激光跟踪靶球3的坐标位置表示为一车体坐标位置；

或，

获取得到至少两个激光跟踪靶球3的坐标位置时，对至少两个激光跟踪靶球3的坐标位置进行计算处理后，确定得出当前车体1的坐标位置；其中，所述的计算处理可采用各种数学公式进行处理，按照实际需求进行选取便可。

在一优选实施例中，对于所述根据激光跟踪靶球3的坐标位置，从而确定出当前车体1的车体方向，其可采用以下任一种优选实施方式来实现。

①、第一优选实施方式

S101、获取激光跟踪靶球3的第一坐标位置301和第二坐标位置302，其中，所述第一坐标位置301指的是激光跟踪靶球3受靶球移动机构2进行位置切换移动前的坐标位置，所述第二坐标位置302指的是激光跟踪靶球3受靶球移动机构2进行位置切换移动后的坐标位置；

S102、根据第一坐标位置301和第二坐标位置302中的X轴坐标和Y轴坐标，计算出第一直线的第一斜率，其中，所述第一直线是由第一坐标位置301和第二坐标位置302中的X轴坐标和Y轴坐标确定得出；

S103、根据计算得出的第一斜率k1，计算出第一直线的第一倾斜角度R1；

S104、根据计算出的第一倾斜角度R1，确定得到当前车体1的车体方向。

具体地，如图2所示，在本实施例中，对于车体方向1的检测，其可通过车体1上的靶球移动机构2对激光跟踪靶球3进行一次位置的切换，从而获取得到靶球位置切换前的坐标P1(x1,y1,z1)，即第一坐标位置301，以及获取得到位置切换后的坐标P2(x2,y2,z2)，即第二坐标位置302；

然后，根据P1和P2中的X轴坐标和Y轴坐标，计算出第一直线的第一斜率k1；具体地，利用以下公式来计算出第一斜率k1：

可见，所述第一直线是由(x1,y1)和(x2,y2)确定得出的；

接着，根据计算得出的第一斜率k1，计算出第一直线的第一倾斜角度R1；具体地，利用以下公式来计算出第一倾斜角R1：

R1＝tan^-1k1

可见，本实施例所计算出的倾斜角R1实质为第一直线与X轴的夹角，因此，利用计算出的倾斜角便能确认出直线与Y轴之间的夹角R，即确认得出当前车体1的车体方向。当然，若车体偏转角度R是利用与X轴之间的夹角来表示的话，那么上述计算得出的R1便可直接表示为车体偏转角度R。也就是说，对于车体偏转角度R，其实质为一绕Z轴的角度。

②、第二优选实施方式

在靶球移动机构2按照预设规律来移动激光跟踪靶球3的情况下，获取激光跟踪靶球3在规律性移动时的坐标，将获取得到的激光跟踪靶球3的坐标拟合成对应的曲线5，根据所述曲线5，从而解析得出当前车体1的车体方向；也就是说，所述的曲线5实质为靶球坐标轨迹曲线；

具体地，如图3所示，在本实施例中，对于车体方向1的检测，其可通过车体1上的靶球移动机构2规律性地激光跟踪靶球3，这样便能获取得到连续轨迹曲线，根据所述曲线5从而解析出车体偏转角度R；在这实施例中，所述车体偏转角度R，其同样也为绕Z轴的角度，即利用直线与Y轴/X轴之间的夹角来表示车体方向。

在一优选实施例中，所述曲线5为正弦曲线；此时，对于所述根据所述曲线5，从而解析得出当前车体1的车体方向，其可具体包括：

S201、从正弦曲线中找出两个对应正峰值或负峰值的坐标位置；

其中，对于所述两个对应正峰值或负峰值的坐标位置，其可为相邻两个对应正峰值或负峰值的坐标位置，也可不为相邻两个对应正峰值或负峰值的坐标位置；

S202、根据找出的两个坐标位置中的X轴坐标和Y轴坐标，计算出第二直线的第二斜率k2，其中，所述第二直线是由找出的两个坐标位置中的X轴坐标和Y轴坐标确定得出；

S203、根据计算得出的第二斜率k2，计算出第二直线的第二倾斜角度R2；

S204、根据计算出的第二倾斜角度R2，确定得到当前车体1的车体方向；

具体地，本实施例中的斜率、倾斜角的计算均可采用上述实施方式①的公式来计算，因此同样地，本实施例所计算出的倾斜角R2实质为第二直线与X轴的夹角，所以，利用计算出的倾斜角便能确认出直线与Y轴之间的夹角R，即确认得出当前车体1的车体方向。当然，若车体偏转角度R是利用直线与X轴之间的夹角来表示的话，那么上述计算得出的R2便可直接表示为车体偏转角度R。

在一优选实施例中，所述坐标转化控制器还用于根据激光跟踪靶球3的坐标位置，计算得出绕Y轴的角度和/或绕X轴的角度；其中，所述X轴和Y轴指的是车体运行环境坐标系中的X轴和Y轴。

在一优选实施例中，所述根据激光跟踪靶球3的坐标位置，计算得出绕Y轴的角度，其具体包括：

S301、获取激光跟踪靶球3的第三坐标位置(x₃ ^’,y₃’,z₃’)(即坐标P3)和第四坐标位置(x₄’,y₄’,z₄’)(即坐标P4)，其中，所述第三坐标位置指的是激光跟踪靶球3在沿X轴移动第一固定距离L1前的坐标位置，所述第四坐标位置指的是激光跟踪靶球3在沿X轴移动第一固定距离L1后的坐标位置；

S302、根据第三坐标位置和第四坐标位置中的Z轴坐标以及第一固定距离L1，计算得出绕Y轴的角度；

所述绕Y轴的角度P的计算公式具体为：

其中，z₃′表示为第三坐标位置中的Z轴坐标，z₄′表示为第四坐标位置中的Z轴坐标。

在一优选实施例中，所述根据激光跟踪靶球3的坐标位置，计算得出绕X轴的角度，其具体包括：

S401、获取激光跟踪靶球3的第五坐标位置(x₅’,y₅’,z₅’)(即坐标P5)和第六坐标位置(x₆’,y₆’,z₆’)(即坐标P6)，其中，所述第五坐标位置指的是激光跟踪靶球3在沿Y轴移动第二固定距离L2前的坐标位置，所述第六坐标位置指的是激光跟踪靶球3在沿Y轴移动第二固定距离L2后的坐标位置；

S402、根据第五坐标位置和第六坐标位置中的Z轴坐标以及第二固定距离L2，计算得出绕X轴的角度；

所述绕X轴的角度W的计算公式具体为：

其中，z₅′表示为第五坐标位置中的Z轴坐标，z₆′表示为第六坐标位置中的Z轴坐标。

在一优选实施例中，靶球移动机构2在直角坐标伺服滑台的协作下移动激光跟踪靶球3沿X轴/Y轴移动。由于采用了直角坐标伺服滑台来作为移动协作工具，因此，在激光跟踪靶球3沿X轴/Y轴移动时，其移动更加准确和稳定。

在一优选实施例中，如图4所示，当激光跟踪靶球3依次沿X轴、Y轴按固定距离L移动时，P4与P5两者坐标相同，并且L1与L2均为L，而此时，所述根据激光跟踪靶球3的坐标位置，计算得出绕Y轴的角度和绕X轴的角度，其具体包括：

S501、在激光跟踪靶球3依次沿X轴、Y轴按固定距离L移动时，获取得到激光跟踪靶球3的第三坐标位置(x₃’,y₃’,z₃’)(即坐标P3)、第四坐标位置(x₄’,y₄’,z₄’)(即坐标P4)/第五坐标位置(x₅’,y₅’,z₅’)(即坐标P5)、第六坐标位置(x₆’,y₆’,z₆’)(即坐标P6)；

其中，所述第三坐标位置指的是激光跟踪靶球3在沿X轴移动固定距离L前的坐标位置，所述第四坐标位置/第五坐标位置指的是激光跟踪靶球3在沿X轴移动固定距离L后的坐标位置，第六坐标位置指的是激光跟踪靶球3在沿Y轴移动固定距离L的坐标位置；

S502、根据第三坐标位置和第四坐标位置中的Z轴坐标以及固定距离L，计算得出绕Y轴的角度；在本实施例中，所述绕Y轴的角度的计算公式如上述步骤S302中的公式所示；

S503、根据第四坐标位置/第五坐标位置和第六坐标位置中的Z轴坐标以及固定距离L，计算得出绕X轴的角度；在本实施例中，所述绕X轴的角度的计算公式如上述步骤S402中的公式所示。

通过上述自由度P、W的计算，可以实现车体更多附加运动的控制(也就是说，所述坐标转化控制器还可具体用于根据计算得出的绕Y轴的角度和/或绕X轴的角度，来进行车体的运动控制)，功能更多样化，并且可令车体行驶的安全性、可靠性和稳定性大大提高；同时，当在车体上加装动力头来实现钻、铣等加工能力时，通过自由度P、W、R的计算，能够保证钻、铣的角度精度，从而提高加工精度；当在车体上加装刻印划线装置时，结合自由度P、W、R的计算，能够实现工厂布局的划线能力；当在车体上安装机器人时，通过P、W、R的计算，能够对机器人末端抓手坐标进行修正补偿。

上述系统实施例的内容均可适用于以下方法实施例中，并且达到相同的有益效果。

如图5所示，基于上述系统，本发明还提供一种全闭环高精度导航方法，应用于自导航叉车和/或AGV中，该方法包括以下步骤：

S601、激光跟踪仪4对激光跟踪靶球3进行跟踪，以获取得到激光跟踪靶球3的坐标位置；

S602、坐标转化控制器(调度系统)根据激光跟踪靶球3的坐标位置，从而确定出当前车体1的坐标位置及车体方向，按照所需途经的站点和/或路线，对当前车体1的坐标位置及车体方向进行数据处理后，将处理后得到的坐标结果和车体方向结果反馈至车体1进行车体行驶位置和方向的修正，以实现车体1行驶位置和方向的调整控制，达到车体行驶自动导航的目的，令车体沿所需途径的站点/路线进行自动行驶。其中，所述激光跟踪仪4所获取得到的激光跟踪靶球3的坐标为(X,Y,Z)，X、Y、Z分别表示为车体运行环境坐标系中的X轴坐标、Y轴坐标和Z轴坐标，且此坐标系的原点为O。

在一优选实施例中，对于所述根据激光跟踪靶球3的坐标位置，从而确定出当前车体1的坐标位置，其可具体包括：

每获取得到一激光跟踪靶球3的坐标位置时，则直接将该激光跟踪靶球3的坐标位置作为当前车体1的坐标位置，也就是说，一激光跟踪靶球3的坐标位置表示为一车体坐标位置；

或，

获取得到至少两个激光跟踪靶球3的坐标位置时，对至少两个激光跟踪靶球3的坐标位置进行计算处理后，确定得出当前车体1的坐标位置；其中，所述的计算处理可采用各种数学公式进行处理，按照实际需求进行选取便可。

在一优选实施例中，对于所述根据激光跟踪靶球3的坐标位置，从而确定出当前车体1的车体方向，其可采用以下任一种优选实施方式来实现：

①、第一优选实施方式

S701、获取激光跟踪靶球3的第一坐标位置301和第二坐标位置302，其中，所述第一坐标位置301指的是激光跟踪靶球3受靶球移动机构2进行位置切换移动前的坐标位置，所述第二坐标位置302指的是激光跟踪靶球3受靶球移动机构2进行位置切换移动后的坐标位置；

S702、根据第一坐标位置301和第二坐标位置302中的X轴坐标和Y轴坐标，计算出第一直线的第一斜率，其中，所述第一直线是由第一坐标位置301和第二坐标位置302中的X轴坐标和Y轴坐标确定得出；

S703、根据计算得出的第一斜率k1，计算出第一直线的第一倾斜角度R1；

S704、根据计算出的第一倾斜角度R1，确定得到当前车体1的车体方向。

②、第二优选实施方式

在靶球移动机构2按照预设规律来移动激光跟踪靶球3的情况下，获取激光跟踪靶球3在规律性移动时的坐标，将获取得到的激光跟踪靶球3的坐标拟合成对应的曲线5，根据所述曲线5，从而解析得出当前车体1的车体方向；也就是说，所述的曲线5实质为靶球坐标轨迹曲线。

在一优选实施例中，所述曲线5为正弦曲线；此时，对于所述根据所述曲线5，从而解析得出当前车体1的车体方向，其可具体包括：

S801、从正弦曲线中找出两个对应正峰值或负峰值的坐标位置；

其中，对于所述两个对应正峰值或负峰值的坐标位置，其可为相邻两个对应正峰值或负峰值的坐标位置，也可不为相邻两个对应正峰值或负峰值的坐标位置；

S802、根据找出的两个坐标位置中的X轴坐标和Y轴坐标，计算出第二直线的第二斜率k2，其中，所述第二直线是由找出的两个坐标位置中的X轴坐标和Y轴坐标确定得出；

S803、根据计算得出的第二斜率k2，计算出第二直线的第二倾斜角度R2；

S804、根据计算出的第二倾斜角度R2，确定得到当前车体1的车体方向。

在一优选实施例中，该方法还包括构建步骤S600；所述构建步骤S600具体包括：

S6001、利用激光跟踪仪4和激光跟踪靶球3来建立车体运行环境坐标系；

S6002、在车体运行环境坐标系下创建车体运行的站点、路线和/或地图。

在一优选实施例中，该方法还包括以下步骤：

S603、根据激光跟踪靶球3的坐标位置，计算得出绕Y轴的角度和/或绕X轴的角度；其中，所述X轴和Y轴指的是车体运行环境坐标系中的X轴和Y轴。

在一优选实施例中，所述根据激光跟踪靶球3的坐标位置，计算得出绕Y轴的角度，其具体包括：

S60311、获取激光跟踪靶球3的第三坐标位置(x₃’,y₃’,z₃’)(即坐标P3)和第四坐标位置(x₄’,y₄’,z₄’)(即坐标P4)，其中，所述第三坐标位置指的是激光跟踪靶球3在沿X轴移动第一固定距离L1前的坐标位置，所述第四坐标位置指的是激光跟踪靶球3在沿X轴移动第一固定距离L1后的坐标位置；

S60312、根据第三坐标位置和第四坐标位置中的Z轴坐标以及第一固定距离L1，计算得出绕Y轴的角度；

所述绕Y轴的角度P的计算公式具体为：

其中，z₃′表示为第三坐标位置中的Z轴坐标，Z₄′表示为第四坐标位置中的Z轴坐标。

在一优选实施例中，所述根据激光跟踪靶球3的坐标位置，计算得出绕X轴的角度，其具体包括：

S60321、获取激光跟踪靶球3的第五坐标位置(x₅’,y₅’,z₅’)(即坐标P5)和第六坐标位置(x₆’,y₆’,z₆’)(即坐标P6)，其中，所述第五坐标位置指的是激光跟踪靶球3在沿Y轴移动第二固定距离L2前的坐标位置，所述第六坐标位置指的是激光跟踪靶球3在沿Y轴移动第二固定距离L2后的坐标位置；

S60322、根据第五坐标位置和第六坐标位置中的Z轴坐标以及第二固定距离L2，计算得出绕X轴的角度；

所述绕X轴的角度W的计算公式具体为：

其中，Z₅′表示为第五坐标位置中的Z轴坐标，z₆′表示为第六坐标位置中的Z轴坐标。

在一优选实施例中，靶球移动机构2在直角坐标伺服滑台的协作下移动激光跟踪靶球3沿X轴/Y轴移动。由于采用了直角坐标伺服滑台来作为移动协作工具，因此，在激光跟踪靶球3沿X轴/Y轴移动时，其移动更加准确和稳定。

在一优选实施例中，如图4所示，当激光跟踪靶球3依次沿X轴、Y轴按固定距离L移动时，P4与P5两者坐标相同，并且L1与L2均为L，而此时，所述根据激光跟踪靶球3的坐标位置，计算得出绕Y轴的角度和绕X轴的角度，其具体包括：

S60331、在激光跟踪靶球3依次沿X轴、Y轴按固定距离L移动时，获取得到激光跟踪靶球3的第三坐标位置(x₃’,y₃’,z₃’)(即坐标P3)、第四坐标位置(x₄’,y₄’,z₄’)(即坐标P4)/第五坐标位置(x₅’,y₅’,z₅’)(即坐标P5)、第六坐标位置(x₆’,y₆’,z₆’)(即坐标P6)；

其中，所述第三坐标位置指的是激光跟踪靶球3在沿X轴移动固定距离L前的坐标位置，所述第四坐标位置/第五坐标位置指的是激光跟踪靶球3在沿X轴移动固定距离L后的坐标位置，第六坐标位置指的是激光跟踪靶球3在沿Y轴移动固定距离L的坐标位置；

S60332、根据第三坐标位置和第四坐标位置中的Z轴坐标以及固定距离L，计算得出绕Y轴的角度；

S60333、根据第四坐标位置/第五坐标位置和第六坐标位置中的Z轴坐标以及固定距离L，计算得出绕X轴的角度。

由上述可得，本发明的方法和系统提供了一种全闭环高精度导航方案，其具有的优点包括：

1、车体的位置坐标可通过激光跟踪仪对激光跟踪靶球进行跟踪从而实时获取，并且利用了所述坐标转化控制器获得的坐标进行数据处理后反馈至车体进行位置修正，因此，本发明实现了全闭环位置反馈，导航性能非常好，不仅位置更新速度快，而且定位精度高，最高可高达0.015mm+0.005mm/m；

2、应用本发明方案来实现车体自动导航，其对车体的运行路面要求低，并且无需在路面进行任何标识标记，易于实现和安装；

3、应用本发明方案来实现车体自动导航，相较于传统方案，可延伸得到6个自由度，分别为X、Y、Z、R、P、W(R：绕Z轴的角度，P：绕Y轴角度，W：绕X轴角度)，因此，可对车体实现更多的控制反馈，从而应付更多路况问题(如路面起伏不平的问题)，令车体行驶得更安全、稳定和可靠；

4、实施简便，位置可追溯，导航范围最大可以达到160米。

此外，对于本发明系统和方法，由于其应用于自导航叉车和/或AGV中，因此，上述实施例中的车体均可具体为自导航叉车车体或AGV车体；并且，由于所述自导航叉车和AGV均属于无人自动行驶设备或移动机器人，因此，对于本发明系统和方法，其可扩展适用于各种无人自动行驶设备或移动机器人中，对其精确定位，从而为各种无人自动行驶设备或移动机器人提供准确的自动导航，也就是说，上述实施例中的车体也可上位为移动对象主体。

以上是对本发明的较佳实施进行了具体说明，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做作出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。

Claims (8)

1.一种全闭环高精度导航系统，其特征在于，该系统包括激光跟踪仪、激光跟踪靶球及坐标转化控制器；

所述激光跟踪仪，用于对激光跟踪靶球进行跟踪，以获取得到激光跟踪靶球的坐标位置；

所述激光跟踪靶球，其移动受控于靶球移动机构；

所述坐标转化控制器，用于根据激光跟踪靶球的坐标位置，从而确定出当前车体的坐标位置及车体方向，按照所需途经的站点和/或路线，对当前车体的坐标位置及车体方向进行数据处理后，将处理后得到的坐标结果和车体方向结果反馈至车体进行车体行驶位置和方向的修正；

所述根据激光跟踪靶球的坐标位置，从而确定出当前车体的车体方向，其具体包括：

获取激光跟踪靶球的第一坐标位置和第二坐标位置，其中，所述第一坐标位置指的是激光跟踪靶球受靶球移动机构进行位置切换移动前的坐标位置，所述第二坐标位置指的是激光跟踪靶球受靶球移动机构进行位置切换移动后的坐标位置；

根据第一坐标位置和第二坐标位置中的X轴坐标和Y轴坐标，计算出第一直线的第一斜率，其中，所述第一直线是由第一坐标位置和第二坐标位置中的X轴坐标和Y轴坐标确定得出；

根据计算得出的第一斜率，计算出第一直线的第一倾斜角度；

根据计算出的第一倾斜角度，确定得到当前车体的车体方向。

2.根据权利要求1所述一种全闭环高精度导航系统，其特征在于，所述根据激光跟踪靶球的坐标位置，从而确定出当前车体的车体方向，其具体包括：

获取激光跟踪靶球在规律性移动时的坐标，将获取得到的激光跟踪靶球的坐标拟合成对应的曲线，根据所述曲线，从而解析得出当前车体的车体方向；

所述曲线为正弦曲线；所述根据所述曲线，从而解析得出当前车体的车体方向，其具体包括：

从正弦曲线中找出两个对应正峰值或负峰值的坐标位置；

根据找出的两个坐标位置中的X轴坐标和Y轴坐标，计算出第二直线的第二斜率，其中，所述第二直线是由找出的两个坐标位置中的X轴坐标和Y轴坐标确定得出；

根据计算得出的第二斜率，计算出第二直线的第二倾斜角度；

根据计算出的第二倾斜角度，确定得到当前车体的车体方向。

3.根据权利要求1-2任一项所述一种全闭环高精度导航系统，其特征在于，所述坐标转化控制器还用于根据激光跟踪靶球的坐标位置，计算得出绕Y轴的角度和/或绕X轴的角度；其中，所述X轴和Y轴指的是车体运行环境坐标系中的X轴和Y轴。

4.根据权利要求3所述一种全闭环高精度导航系统，其特征在于，所述根据激光跟踪靶球的坐标位置，计算得出绕Y轴的角度，其具体包括：

获取激光跟踪靶球的第三坐标位置和第四坐标位置，其中，所述第三坐标位置指的是激光跟踪靶球在沿X轴移动第一固定距离L1前的坐标位置，所述第四坐标位置指的是激光跟踪靶球在沿X轴移动第一固定距离L1后的坐标位置；

根据第三坐标位置和第四坐标位置中的Z轴坐标以及第一固定距离L1，计算得出绕Y轴的角度；

所述绕Y轴的角度P的计算公式具体为：

其中，z₃′表示为第三坐标位置中的Z轴坐标，z₄′表示为第四坐标位置中的Z轴坐标。

5.根据权利要求3所述一种全闭环高精度导航系统，其特征在于，所述根据激光跟踪靶球的坐标位置，计算得出绕X轴的角度，其具体包括：

获取激光跟踪靶球的第五坐标位置和第六坐标位置，其中，所述第五坐标位置指的是激光跟踪靶球在沿Y轴移动第二固定距离L2前的坐标位置，所述第六坐标位置指的是激光跟踪靶球在沿Y轴移动第二固定距离L2后的坐标位置；

根据第五坐标位置和第六坐标位置中的Z轴坐标以及第二固定距离L2，计算得出绕X轴的角度；

所述绕X轴的角度W的计算公式具体为：

其中，z₅′表示为第五坐标位置中的Z轴坐标，z₆′表示为第六坐标位置中的Z轴坐标。

6.一种全闭环高精度导航方法，其特征在于，包括以下步骤：

激光跟踪仪对激光跟踪靶球进行跟踪，以获取得到激光跟踪靶球的坐标位置；

坐标转化控制器根据激光跟踪靶球的坐标位置，从而确定出当前车体的坐标位置及车体方向，按照所需途经的站点和/或路线，对当前车体的坐标位置及车体方向进行数据处理后，将处理后得到的坐标结果和车体方向结果反馈至车体进行车体行驶位置和方向的修正；

所述根据激光跟踪靶球的坐标位置，从而确定出当前车体的车体方向，其具体包括：

获取激光跟踪靶球的第一坐标位置和第二坐标位置，其中，所述第一坐标位置指的是激光跟踪靶球受靶球移动机构进行位置切换移动前的坐标位置，所述第二坐标位置指的是激光跟踪靶球受靶球移动机构进行位置切换移动后的坐标位置；

根据第一坐标位置和第二坐标位置中的X轴坐标和Y轴坐标，计算出第一直线的第一斜率，其中，所述第一直线是由第一坐标位置和第二坐标位置中的X轴坐标和Y轴坐标确定得出；

根据计算得出的第一斜率，计算出第一直线的第一倾斜角度；

根据计算出的第一倾斜角度，确定得到当前车体的车体方向。

7.根据权利要求6所述一种全闭环高精度导航方法，其特征在于，还包括构建步骤；所述构建步骤具体包括：

利用激光跟踪仪和激光跟踪靶球来建立车体运行环境坐标系；

在车体运行环境坐标系下创建车体运行的站点、路线和/或地图。

8.根据权利要求6或7所述一种全闭环高精度导航方法，其特征在于，还包括以下步骤：

根据激光跟踪靶球的坐标位置，计算得出绕Y轴的角度和/或绕X轴的角度；其中，所述X轴和Y轴指的是车体运行环境坐标系中的X轴和Y轴。

Images