CN110879591A - 一种复杂地貌下agv车定位导航系统接收单元及调控方法 - Google Patents

Abstract

本发明涉及一种复杂地貌下AGV车定位导航系统接收单元，信号处理器及若干可调向接收器均安装在AGV车上，若干发射站对应与若干可调向接收器连接，若干可调向接收器均连接至信号处理器，信号处理器连接至PC机，若干可调向接收器均连接至PC机。本发明还涉及一种复杂地貌下AGV车定位导航系统接收单元的调控方法，包括1)初始角度标定；2)调整角度计算。本发明设计科学合理，利用电机的绝对定位功能构建可调向接收器的信号接收角度和车体坐标系之间的旋转关系，通过计算确定每个发射站与每个可调向接收器之间的位置关系，可调向接收器能够始终用其有效的接收面正对发射站，最大程度地解决因为接收器朝向不适导致的“断光”问题，保证测量的连续性。

Description

一种复杂地貌下AGV车定位导航系统接收单元及调控方法

技术领域

本发明属于工业现场AGV车辆位姿测量技术领域，涉及空间发射站动态位姿测量及结合伺服电机的实时调向系统，特别涉及一种复杂地貌下AGV车定位导航系统接收单元及调控方法。

背景技术

工作空间测量定位系统(workshop Measuring Positioning System)即wMPS系统是一种基于光电扫描的大尺寸三维坐标测量系统，由发射站、接收器、信号处理器以及计算机这几部分构成。系统在空间中通过合理的发射站布局建立大范围测量场，具有坐标测量精度高、测量空间大以及布局灵活等特点，在边长20m的测量范围内单点静态测量精度优于0.3mm。目前使用的wMPS球型接收器能够固定在磁力球座上，可以将其固定在刚体表面，通过建立静止状态下接收器和刚体相对位置的约束关系，通过动态测量所得的接收器坐标来实时建立刚体的位姿，这就是用wMPS系统进行AGV车定位导航实验的原理。

AGV车的实时动态位姿测量需要安装在车体上的接收器能够随着车体移动持续接收发射站发出的激光信号。这对发射站的布局以及接收器的布置朝向均有要求，并且当测量范围扩大或者发射站布局受到限制时，在不增加发射站数量节约测量成本的情况下，接收器的朝向更为重要。考虑到实验现场可能出现的恶劣复杂情况，发射站发出的激光信号容易被遮挡，这种“断光”现象会导致测量数据的不连续。为了解决这种情况，一种类似于后方交会的wMPS系统新位姿解算方式被提出，并得以实现。这种方法在解算传感器坐标的时候，加入了传感器之间的位置约束补充优化方程，减少了必要的发射站数量，每个接收器只需要在同一时间接收到单个发射站的激光信号就可以计算出坐标。这种方法节约了测量成本，不易产生“断光”，提高了wMPS系统动态测量的性能。同时这种测量方法提供了一种优化思路：定位过程中只需要实时确定接收器和发射站之间的位置关系，并构建出接收器的有效接收面范围，使接收器的接收面能够一直接收到单个发射站的激光信号。这种方法比起传统的交会测量，省去了苛刻的交会条件，令计算难度和实现难度均能降低。为了进一步优化这种新型测量方式的连续性，现设计一种用于wMPS系统的接收器自动跟踪瞄准模块，并利用wMPS系统的位姿测量功能设计其实时控制方法，让接收器能够在不改变自身坐标的前提下旋转以改变接收方向，满足复杂地貌下AGV车导航的需求。

这种模块包括一种新型的可调向接收器和跟踪瞄准适应系数算法设计，目的是利用wMPS系统的位姿测量功能，利用伺服电机的绝对定位功能构建可调向接收器的信号接收角度和车体坐标系之间的旋转关系，通过计算确定每个发射站与每个接收器之间的位置关系。利用接收器自动跟踪瞄准模块能使动态位姿测量过程中，接收器能够始终用其有效的接收面正对发射站，最大程度地解决因为接收器朝向不适导致的“断光”问题，保证测量的连续性。

发明内容

本发明的目的在于克服现有技术的不足，提供一种复杂地貌下AGV车定位导航系统接收单元及调控方法，利用接收器自动跟踪瞄准模块能使动态位姿测量过程中，接收器能够始终用其有效的接收面正对发射站，最大程度地解决因为接收器朝向不适导致的“断光”问题，保证测量的连续性。

本发明解决其技术问题是通过以下技术方案实现的：

一种复杂地貌下AGV车定位导航系统接收单元，其特征在于：包括AGV车、PC机、信号处理器及若干发射站、可调向接收器，所述信号处理器及若干可调向接收器均安装在所述AGV车上，所述若干发射站对应与若干可调向接收器连接，所述若干可调向接收器均连接至所述信号处理器，所述信号处理器连接至所述PC机，所述若干可调向接收器均连接至所述PC机；

所述可调向接收器包括测控电路板、光敏元件、水平角调控电机、俯仰角调控电机、水平轴及垂直轴，所述测控电路板上同轴安装所述光敏元件，所述光敏元件的信号线连接口处连接有信号线，所述信号线连接至所述信号处理器，所述水平轴焊接安装在所述测控电路板背部，所述水平轴端部连接所述俯仰角调控电机；所述垂直轴一端固定安装支架环，所述支架环与所述测控电路板同轴并固定在所述水平轴上，所述垂直轴的另一端连接所述水平角调控电机，所述水平角调控电机及俯仰角调控电机均连接至所述PC机。

而且，所述可调向接收器的个数为4个，分别位于AGV车的四角位置处。

而且，所述发射站的个数为8个，分别对应4个可调向接收器的水平角调控电机及俯仰角调控电机。

一种复杂地貌下AGV车定位导航系统接收单元调控方法，其特征在于：所述方法的步骤为：

1)初始角度标定：调节水平角调控电机及俯仰角调控电机在零位上，保证可调向接收器的起始绝对量；通过可调向接收器获得AGV车的坐标位置，将四个可调向接收器拟合出的圆的圆心作为AGV车的坐标系原点，x轴朝向AGV车右侧两个可调向接收器的连线中点，右前方的可调向接收器作为xOy平面的一点，由此可获得四个可调向接收器的坐标；可调向接收器的光敏元件接收发射站信号，并标定左、右极限接收角度来确定该有效接收范围的平分线，该平分线为可调向接收器正对发射站的方向，AGV车的位姿x，y，z，rx，ry，rz代表的含义是世界坐标系先经过平移[x，y，z]再经过三次坐标轴的旋转得到车体坐标系；

2)调整角度计算：向量

用来确定当AGV车移动时，在任意车体位姿的情况下，水平角调控电机及俯仰角调控电机为使得可调向接收器正对任意发射站需要进行的调整方向和幅度；在车体位姿变化后，计算出新的从可调向接收器到发射站的位移向量

将

进行单位化并拆分成同样的水平角和俯仰角，

和

进行对比确定当前水平角调控电机及俯仰角调控电机从零位出发进行旋转的角度，来使可调向接收器能够正对发射站；

AGV车体位姿变化为新的x，y，z，rx，ry，rz时，可调向接收器在AGV车体系下的坐标[x_s，y_s，z_s]保持不变，发射站在场地系下的坐标[x_l，y_l，z_l]也保持不变，则发射站在新的车体坐标系下坐标为

[x_aln，y_aln，z_aln]^T＝R′([x_l，y_l，z_l]^T-T′)

其中R′T′由新的车体位姿确定。

由此可计算AGV车移动后，车体坐标系下从可调向接收器[x_s，y_s，z_s]到发射站[x_aln，y_aln，z_aln]的移动向量

该向量和

均拆分成水平角和俯仰角，将两种水平角和俯仰角进行对比，就是此时水平角调控电机及俯仰角调控电机从零位出发需要调整的角度量。

本发明的优点和有益效果为：

本发明设计科学合理，利用电机的绝对定位功能构建可调向接收器的信号接收角度和车体坐标系之间的旋转关系，通过计算确定每个发射站与每个可调向接收器之间的位置关系，可调向接收器能够始终用其有效的接收面正对发射站，最大程度地解决因为接收器朝向不适导致的“断光”问题，保证测量的连续性。

附图说明

图1为本发明的结构示意图；

图2为本发明可调向接收器的结构示意图；

图3为可调向接收器有效接收方向定义图；

图4为自动跟踪瞄准系统的角度计算示意图；

图5为水平方向的有效接收方向标定示意图。

附图标记说明

1-发射站、2-AGV车、3-PC机、4-可调向接收器、5-信号处理器、6-信号线、7-信号线连接口、8-测控电路板、9-水平轴、10-光敏元件、11-水平角调控电机、12-垂直轴、13-俯仰角调控电机、14-支架环、15-左极限接收角度、16-右极限接收角度、17-有效接收方向、18-AGV车体坐标系。

具体实施方式

下面通过具体实施例对本发明作进一步详述，以下实施例只是描述性的，不是限定性的，不能以此限定本发明的保护范围。

如图1所示，一种复杂地貌下AGV车定位导航系统接收单元，其创新之处在于：包括AGV车2、PC机3、信号处理器5及8个发射站1、4个可调向接收器4，所述信号处理器及4个可调向接收器均安装在所述AGV车上，所述8个发射站对应与4个可调向接收器连接，所述4个可调向接收器均连接至所述信号处理器，所述信号处理器连接至所述PC机，所述4个可调向接收器均连接至所述PC机；

如图2所示，可调向接收器包括测控电路板8、光敏元件10、水平角调控电机11、俯仰角调控电机13、水平轴9及垂直轴12，所述测控电路板上同轴安装所述光敏元件，所述光敏元件的信号线连接口7处连接有信号线6，所述信号线连接至所述信号处理器，所述水平轴焊接安装在所述测控电路板背部，所述水平轴端部连接所述俯仰角调控电机；所述垂直轴一端固定安装支架环14，所述支架环与所述测控电路板同轴并固定在所述水平轴上，所述垂直轴的另一端连接所述水平角调控电机，所述水平角调控电机及俯仰角调控电机均连接至所述PC机。

可调向接收器的朝向是光敏元件的光信号接收面所在平面的法线方向，由于可调向接收器的结构上是双轴控制，可以将该方向拆分成水平方向和俯仰方向。以水平方向为例说明，如图3所示，使用一种基于伺服电机的匀速旋转功能的标定方法，在接收器围绕伺服电机中轴旋转一圈的过程中记录是否接收到激光信号的时间，用是否能够接收到激光信号的变化时间节点在旋转周期之中占比确定接收器的有效接受角度范围，其中左极限接收角度15和右极限接受角度16的平分线即可认为是可调向接收器4的有效接收方向17，作为评定是否正对发射站的基准。俯仰方向的有效接收方向确定方法类似，当接收单元安装在AGV车身上时，以车身为水平面则当有效接收方向和车身平行时，认为此时俯仰角为0；水平方向取值范围为[0,2π]，俯仰方向取值范围为[-π/2,π/2]，两种方向结合起来能够覆盖空间内任何方向，能够用来表示当前接收器在三维空间内的有效接收方向。

如图4所示，在测量场地下利用wMPS系统自身的坐标测量功能，可以构建出可调向接收器在世界坐标系下的坐标；发射站的布局情况经过事先标定，可以确定发射站的仪器坐标系到世界坐标系的旋转平移关系；事先构建可调向接收器的有效接收方向和AGV车体坐标系18的相对关系，就能通过车的位姿得到当前可调向接收器的有效接收方向，结合以上信息就能构建出可调向接收器和发射站之间的相互位置关系，通过计算确定伺服电机需要的定位角度使可调向接收器的有效接收方向能够指向需要指向的发射站。

一种复杂地貌下AGV车定位导航系统接收单元调控方法，其创新之处在于：所述方法的步骤为：

1)初始角度标定：调节水平角调控电机及俯仰角调控电机在零位上，保证可调向接收器的起始绝对量；通过可调向接收器获得AGV车的坐标位置，将四个可调向接收器拟合出的圆的圆心作为AGV车的坐标系原点，x轴朝向AGV车右侧两个可调向接收器的连线中点，右前方的可调向接收器作为xOy平面的一点，由此可获得四个可调向接收器的坐标；可调向接收器的光敏元件接收发射站信号，并标定左、右极限接收角度来确定该有效接收范围的平分线，该平分线为可调向接收器正对发射站的方向，AGV车的位姿x，y，z，rx，ry，rz代表的含义是世界坐标系先经过平移[x，y，z]再经过三次坐标轴的旋转得到车体坐标系；

本实施例中以水平方向的有效接收方向标定为例进行说明：

底部伺服电机从零位出发匀速旋转三周，其中前两次逆时针旋转，第三次再次旋转到零位时立刻反转一周，最后停在零位。则相对来说如果认为电机不动的话，可以认为是发射站的入射光信号围绕电机中心进行旋转，前两次顺时针旋转，第三次逆时针旋转。通过wMPS测量软件的测量日志功能确定在这三周的时间内，从无法接收到对应发射站信号到能够接收到信号的时间节点或者是从能够接收到信号到无法接收到信号的时间节点这些信息，判断出旋转周期和可调向接收器的两个极限接受角度，从而计算出伺服电机零位对应接收器有效接收方向和发射站连线之间的夹角关系。

本实施例中以起点能够接收到信号的情况为例进行说明，如图5所示：

电机从起点开始旋转后第一次变成无法接受到信号的时间点设为t1，第一次从无法接收到能够接收的时间点为t2。第二轮周期以上两种时间点为t3、t4。则电机的旋转一周的周期T为T＝(t3+t4-t1-t2)/2

旋转一周中，可接收到信号的角度范围大小为

∝＝2π(t3-t2)/T

第三周电机反转，以上两种时间点为t5、t6，则电机零位对应的接收器有效接收范围与可接收到信号的角度范围的俯视右边界夹角为

∝_r＝π(t5-t4)/T

我们认为从接收器到标定发射站的连线方向为可接受到信号的角度范围的平分线方向。通过以上步骤标定出伺服电机零位状态下接收器的有效接收方向和从接收器到发射站连接线方向的夹角θ_z。

θ_z＝∝_r-∝/2

其中θ_z为正表示伺服电机零位状态下接收器的有效接收方向在从接收器到发射站连接线方向的逆时针方向的位置。标定过程如果是从无法接收到信号的一侧开始也同理于以上步骤，不做赘述。俯仰角的标定方法与上述过程类似，只要先标定出θ_z，令底部伺服电机旋转到θ_z的位置，此时接收器的有效接收方向在xOy平面的投影经过发射站的投影。侧部伺服电机进行类似的三圈旋转，通过以上步骤用类似的方法标定出侧部伺服电机位于零位时，接收元件所处的俯仰方向上的有效接收方向θ_r。根据上述定义，θ_r取值范围为[-π/2，π/2]，定义当θ_r为π/2时，接受元件方向朝向垂直向上，和车身垂直。

标定时车体坐标系下从接收器到发射站的位移向量为[x，y，z]，则通过[x，y]确定的向量水平角θ₁和z确定的θ₂分别加上标定出的零位对应有效接收方向和发射站方向夹角θ_z和θ_r，就可以确定电机处于零位时接收器有效接收方向在车体坐标系下的方向向量，具体计算过程如下：

标定时已知使用的接收器在车体系下坐标为[x_s，y_s，z_s]，发射站在场地系下坐标为[x_l，y_l，z_l]，AGV车的位姿x，y，z，rx，ry，rz，对应的旋转平移矩阵为RT。则发射站在车体坐标系下的坐标[x_aly_al，z_al]为

[x_al，y_al，z_al]^T＝R([x_l，y_l，z_l]^T-T)

将从[x_s，y_s，z_s]到[x_al，y_al，z_al]的向量对应的水平方向和俯仰方向的旋转角θ₁和θ₂求出，令

则

则伺服电机位于零位的时候，车体坐标系下接收器有效接收方向的方向向量

满足

2)调整角度计算：向量

用来确定当AGV车移动时，在任意车体位姿的情况下，水平角调控电机及俯仰角调控电机为使得可调向接收器正对任意发射站需要进行的调整方向和幅度；在车体位姿变化后，计算出新的从可调向接收器到发射站的位移向量

将

进行单位化并拆分成同样的水平角和俯仰角，

和

进行对比确定当前水平角调控电机及俯仰角调控电机从零位出发进行旋转的角度，来使可调向接收器能够正对发射站；

AGV车体位姿变化为新的x，y，z，rx，ry，rz时，可调向接收器在AGV车体系下的坐标[x_s，y_s，z_s]保持不变，发射站在场地系下的坐标[x_l，y_l，z_l]也保持不变，则发射站在新的车体坐标系下坐标为[x_aln，y_aln，z_aln]^T＝R′([x_l，y_l，z_l]^T-T′)

其中R′T′由新的车体位姿确定。

由此可计算AGV车移动后，车体坐标系下从可调向接收器[x_s，y_s，z_s]到发射站[x_aln，y_aln，z_aln]的移动向量

该向量和

均拆分成水平角和俯仰角，将两种水平角和俯仰角进行对比，就是此时水平角调控电机及俯仰角调控电机从零位出发需要调整的角度量。

尽管为说明目的公开了本发明的实施例和附图，但是本领域的技术人员可以理解：在不脱离本发明及所附权利要求的精神和范围内，各种替换、变化和修改都是可能的，因此，本发明的范围不局限于实施例和附图所公开的内容。

Claims (4)

1.一种复杂地貌下AGV车定位导航系统接收单元，其特征在于：包括AGV车、PC机、信号处理器及若干发射站、可调向接收器，所述信号处理器及若干可调向接收器均安装在所述AGV车上，所述若干发射站对应与若干可调向接收器连接，所述若干可调向接收器均连接至所述信号处理器，所述信号处理器连接至所述PC机，所述若干可调向接收器均连接至所述PC机；

所述可调向接收器包括测控电路板、光敏元件、水平角调控电机、俯仰角调控电机、水平轴及垂直轴，所述测控电路板上同轴安装所述光敏元件，所述光敏元件的信号线连接口处连接有信号线，所述信号线连接至所述信号处理器，所述水平轴焊接安装在所述测控电路板背部，所述水平轴端部连接所述俯仰角调控电机；所述垂直轴一端固定安装支架环，所述支架环与所述测控电路板同轴并固定在所述水平轴上，所述垂直轴的另一端连接所述水平角调控电机，所述水平角调控电机及俯仰角调控电机均连接至所述PC机。

2.根据权利要求1所述的复杂地貌下AGV车定位导航系统接收单元，其特征在于：所述可调向接收器的个数为4个，分别位于AGV车的四角位置处。

3.根据权利要求1所述的复杂地貌下AGV车定位导航系统接收单元，其特征在于：所述发射站的个数为8个，分别对应4个可调向接收器的水平角调控电机及俯仰角调控电机。

4.根据权利要求1所述的复杂地貌下AGV车定位导航系统接收单元调控方法，其特征在于：所述方法的步骤为：

1)初始角度标定：调节水平角调控电机及俯仰角调控电机在零位上，保证可调向接收器的起始绝对量；通过可调向接收器获得AGV车的坐标位置，将四个可调向接收器拟合出的圆的圆心作为AGV车的坐标系原点，x轴朝向AGV车右侧两个可调向接收器的连线中点，右前方的可调向接收器作为xOy平面的一点，由此可获得四个可调向接收器的坐标；可调向接收器的光敏元件接收发射站信号，并标定左、右极限接收角度来确定该有效接收范围的平分线，该平分线为可调向接收器正对发射站的方向，AGV车的位姿x，y，z，rx，ry，rz代表的含义是世界坐标系先经过平移[x，y，z]再经过三次坐标轴的旋转得到车体坐标系；

2)调整角度计算：向量

用来确定当AGV车移动时，在任意车体位姿的情况下，水平角调控电机及俯仰角调控电机为使得可调向接收器正对任意发射站需要进行的调整方向和幅度；在车体位姿变化后，计算出新的从可调向接收器到发射站的位移向量

将

进行单位化并拆分成同样的水平角和俯仰角，

和

进行对比确定当前水平角调控电机及俯仰角调控电机从零位出发进行旋转的角度，来使可调向接收器能够正对发射站；

AGV车体位姿变化为新的x，y，z，rx，ry，rz时，可调向接收器在AGV车体系下的坐标[x_s，y_s，z_s]保持不变，发射站在场地系下的坐标[x_l，y_l，z_l]也保持不变，则发射站在新的车体坐标系下坐标为

[x_aln，y_aln，z_aln]T＝R′([x_l，y_l，z_l]^T-T′)

其中R′T′由新的车体位姿确定。

由此可计算AGV车移动后，车体坐标系下从可调向接收器[x_s，y_s，z_s]到发射站[x_aln，y_aln，z_aln]的移动向量

该向量和

均拆分成水平角和俯仰角，将两种水平角和俯仰角进行对比，就是此时水平角调控电机及俯仰角调控电机从零位出发需要调整的角度量。

Images