AGV激光防撞传感器偏转角自动标定装置及标定方法与流程

本发明涉及自动化码头水平运输系统中自动搬运集装箱小车领域，更具体地说，涉及一种AGV激光防撞传感器偏转角自动标定装置及标定方法。

背景技术：

集装箱AGV(Automated Guided Vehicle，即“自动化导引运输车”)是自动化码头中一个重要组成部分。AGV与集装箱码垛吊车交互时，两者之间不可能总是同步，对此，往往将集装箱临时放置在缓冲支架上，使得另外一个设备(AGV或集装箱码垛吊车)可以提前完成该任务去执行下一条任务。由于AGV进缓冲支架时，AGV侧边与缓冲支架之间允许预留的缝隙仅仅16cm左右，当AGV车身距离反光板小于7cm时，为了避免AGV与集装箱缓冲支架发生碰撞，AGV必须停车。考虑AGV进入缓冲支架的过程中，AGV自身可能存在3cm左右的纠偏，AGV车长15m，依靠安装于AGV车头的激光防撞传感器探测AGV前方侧边的反射板，预估AGV车身跟支架反射板之间的距离，经过计算，要求获取激光防撞传感器的偏转角精度控制在0.15°以内。

目前，常见的解决办法是在AGV侧边安装多个超声波传感器来实现防撞。但是，超声波传感器也仅仅是探测AGV上安装有超声波的位置与集装箱缓冲支架的单点距离，而无法预估车身其他位置到集装箱缓冲支架的距离。

对于常见的激光防撞传感器的标定方法，可以利用AGV车身或AGV上一些定位基准作为参照物进行标定，但是，由于AGV是大型焊接结构件，要求标定精度误差控制在0.15°以内，难度非常大。

技术实现要素：

针对现有技术中存在的上述缺陷，本发明的目的是提供一种AGV激光防撞传感器偏转角自动标定装置及标定方法，能够自动计算激光防撞传感器的偏转角度，控制偏转角度误差在0.15度以内，从而减少人力成本，提高精度和效率。

为实现上述目的，本发明采用如下技术方案：

一方面，AGV激光防撞传感器偏转角自动标定装置，包括自动化导引运输车(以下称AGV车)，以及供AGV车行驶的车道，还包括前部激光防撞传感器、后部激光防撞传感器、前天线、后天线、磁订、反光板和处理器，磁订具有数个，分别设于车道的地面上，每个磁订内均设有坐标信息的编码，前部激光防撞传感器设于AGV车的前端，用以向外界发射激光，前天线也设于AGV车的前端，且设于前部激光防撞传感器的后方，用以探测磁订，后部激光防撞传感器设于AGV车的后端，用以向外界发射激光，后天线也设于AGV车的后端，且设于后部激光防撞传感器的前方，用以探测磁订，反光板具有数块，均通过相应的反光板安装支座均匀间隔设置于车道的一侧边，处理器分别与前部激光防撞传感器、后部激光防撞传感器、前天线和后天线数据相连，用以将接收的数据信息进行分析处理，并计算出前部激光防撞传感器和后部激光防撞传感器的偏转角标定角度。

所述的激光照射角度范围为-5°～185°，照射距离为80m。

所述的前部激光防撞传感器、后部激光防撞传感器、前天线和后天线均设于AGV车的中心线位置上。

所述的磁订的坐标误差为0.5mm以内。

所述的反光板与车道的中心线，以及磁订的连线均平行设置。

另一方面，AGV激光防撞传感器偏转角自动标定方法，包括以下步骤：

S1.根据激光的数据计算反光板与激光中心线的夹角，前部激光防撞传感器将按照-5°到185°范围内，每隔0.25°发出一束激光，当每束激光遇到反光板时，返回反光板与激光发射中心的距离，同时记录该角度，处理器根据反光板返回的激光束的距离和角度信息，初步拟合出反光板在激光直角坐标系中的直线方程，根据反光板反射回来的激光数据到初步拟合直线的距离关系，剔除部分干扰点，然后对剩余的数据点，再次进行直线拟合，根据直线拟合的斜率，计算反光板与激光中心线之间的夹角；

S2.根据前、后天线相对于磁订的横向偏差计算AGV的姿态角，AGV车上的前、后天线同时探测到有效磁订时，前、后天线将返回探测到磁订在天线坐标系中的坐标位置，从而处理器可以计算出前、后天线中心点相对于磁订连线的横向偏移量，根据前、后天线中心点之间的距离，通过三角关系，处理器计算出AGV车相对于磁订连线的夹角，即姿态角；

S3.计算激光相对于AGV的安装偏转角，由于反光板安装时，与磁订连线平行设置，将反光板与激光中心线的夹角减去AGV车的姿态角，最终求得激光相对于AGV车的安装偏转角。

所述的步骤S1中，前部激光防撞传感器具体标定如下：

S11.确定反光板的激光有效角度照射范围；

S12.每隔一固定角度获取反光板到激光中心的距离和角度；

S13.将反光板各个位置到激光中心的距离转换成激光坐标系中的坐标；

S14.对反光板的数据进行初步直线拟合；

S15.对初步拟合的反光板的干扰数据点进行过滤，保留有效点；

S16.对干扰剔除后的反光板的有效数据点进行重新直线拟合；

S17.获取反光板与激光中心线的夹角。

所述的步骤S15对初步拟合的反光板的干扰数据点进行过滤，保留有效点，具体如下：

S151.计算被拟合点到初步拟合直线的距离；

S152.计算距离的标准偏差；

S153.距离不大于两倍标准偏差；

S154.该距离为有效点，予以保留；

S155.该距离为干扰点，予以剔除。

所述的步骤S2中，计算AGV车当前姿态角具体如下：

S21.AGV车上的前、后天线同时探测到有效磁订；

S22.获取前天线探测到磁订在前天线坐标系中的坐标；

S23.获取后天线探测到磁订在后天线坐标系中的坐标；

S24.根据前、后天线坐标原点分别相对于前、后天线探测到磁订的横向偏差，计算AGV车的姿态角。

在上述的技术方案中，本发明所提供的AGV激光防撞传感器偏转角自动标定装置及标定方法，通过对反光板进行初步直线拟合的基础上，采用滤波的方法剔除部分干扰点后，再次进行直线拟合得到反光板与前、后部激光防撞传感器中心线的夹角。然后当AGV车前、后天线同时探测到磁订时，根据前、后天线相对磁订的横向偏差计算AGV车在地面全局坐标系中的姿态角，最后，再结合反光板与前、后部激光传感器中心线的夹角，自动计算前、后部激光防撞传感器的安装偏转角的精确值。实现对激光防撞传感器偏转角的精确自动标定，提高标定精度，减少人力成本，节约时间。

附图说明

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

图2是本发明的组成框图；

图3是本发明自动标定方法的流程示意图；

图4是本发明计算反光板与激光中心线夹角的流程示意图；

图5是本发明反光板干扰点滤波的流程示意图；

图6是本发明计算AGV车当前姿态角的流程示意图；

图7是本发明计算AGV车姿态角的三角关系示意图。

具体实施方式

下面结合附图和实施例进一步说明本发明的技术方案。

请结合图1和图2所示，本发明所提供的AGV激光防撞传感器偏转角自动标定装置，包括自动化导引运输车(以下称AGV车3)，以及供AGV车3行驶的车道10，还包括前部激光防撞传感器5、后部激光防撞传感器1、前天线4、后天线2、磁订6、反光板7和处理器9，磁订6具有数个，预先按照一定的规则分别埋设在车道10的地面上，并针对每个磁订6预先写入包含坐标信息的编码，前部激光防撞传感器5设于AGV车3的前端，用以向外界发射激光，每隔0.25°发射一束激光，前天线4也设于AGV车3的前端，且设于前部激光防撞传感器5的后方，用以探测磁订，获取探测到的磁订6在前天线4的坐标系中的坐标，并获取该磁订6在地面全局坐标系中坐标，后部激光防撞传感器1设于AGV车3的后端，用以向外界发射激光，每隔0.25°发射一束激光，待前部激光防撞传感器5标定完成后，将AGV车3掉头，再对后部激光防撞传感器1进行标定，后天线2也设于AGV车3的后端，且设于后部激光防撞传感器1的前方，用以探测磁订，获取探测到的磁订6在后天线2的坐标系中的坐标，并获取该磁订6在地面全局坐标系中坐标，反光板7具有数块，均通过反光板安装支座8均匀间隔设置于车道10的一侧边，同时确保与车道10的中心线平行，控制角度误差在0.1°以内，反光板7与磁订6的连线也平行设置，且间距控制在AGV车3一半的基础上再加0.15m以外的位置，处理器9分别与前部激光防撞传感器5、后部激光防撞传感器1、前天线4和后天线2数据相连，用以将接收的数据信息进行分析处理，并计算出前部激光防撞传感器5和后部激光防撞传感器1的偏转角标定角度。

较佳的，所述的激光照射角度范围为-5°～185°，照射距离为80m。

较佳的，所述的前部激光防撞传感器5、后部激光防撞传感器1、前天线4和后天线2均设于AGV车3的中心线位置上。

较佳的，所述的磁订6的误差控制在0.5mm以内。

请结合图3所示，本发明所提供的AGV激光防撞传感器偏转角自动标定方法，包括以下步骤：

S1.根据激光的数据计算反光板与激光中心线的夹角，前部激光防撞传感器将按照-5°到185°范围内，每隔0.25°发出一束激光，当每束激光遇到反光板时，返回反光板与激光发射中心的距离，同时记录该角度，处理器根据反光板返回的激光束的距离和角度信息，初步拟合出反光板在激光直角坐标系中的直线方程，根据反光板反射回来的激光数据到初步拟合直线的距离关系，剔除部分干扰点，然后对剩余的数据点，再次进行直线拟合，根据直线拟合的斜率，计算反光板与激光中心线之间的夹角；

S2.根据前、后天线相对于磁订的横向偏差计算AGV的姿态角，AGV车上的前、后天线同时探测到有效磁订时，前、后天线将返回探测到磁订在天线坐标系中的坐标位置，从而处理器可以计算出前、后天线中心点相对于磁订连线的横向偏移量，根据前、后天线中心点之间的距离，通过三角关系，处理器计算出AGV车相对于磁订连线的夹角，即姿态角；

S3.计算激光相对于AGV的安装偏转角，由于反光板安装时，与磁订连线平行设置，将反光板与激光中心线的夹角减去AGV车的姿态角，最终求得激光相对于AGV车的安装偏转角γ。

请结合图4所示，所述的步骤S1中，前部激光防撞传感器具体标定如下(后部激光防撞传感器的具体标定，将AGV车掉头，利用同样的方法进行标定)：

S11.确定反光板的激光有效角度照射范围，假设前部激光防撞传感器中心点到反光板的距离为D_y，反光板最远端点到AGV车头的距离为D_x，于是，可以求出前部激光防撞传感器允许探测到反光板的有效角度θ为：

本实施例中，反光板安装在AGV车的左侧，而前部激光防撞传感器是以右侧为零度，垂直向前为90°，所以，反光板的有效探测角度范围θ′满足：

θ′∈[180°-θ，180°]

S12.每隔一固定角度获取反光板到激光中心的距离和角度，根据反光板的激光有效角度探测范围，每个0.25°发射一束激光到反光板，每束激光照射到反光板后将返回前部激光防撞传感器，前部激光防撞传感器记录下这些距离和角度信息，这些距离信息用r_i表示，对应的角度信息用θ_i表示；

S13.将反光板各个位置到激光中心的距离转换成激光坐标系中的坐标，假设前部激光防撞传感器90°向外发射的方向为激光直角坐标系的x轴正方向，180°向外的方向为激光直角坐标系的y轴正方向，则上述S12中的距离r_i和角度θ_i的极坐标系可以转换成直角坐标系中对应的坐标(x_i，y_i)，即：

S14.对反光板的数据进行初步直线拟合，假设反光板的在防撞激光直角坐标系下的直线方程用y＝k₁x+b₁表示，其中，k₁表示斜率，b₁表示截距，这两个参数是待确定的。对于激光探测到的反光板的N组有效角度范围内的数据(x_i，y_i)(其中，i＝1，2，......，N，下同)，利用最小二乘法对数据把数据拟合为直线。

用最小二乘法估计参数时，要求观测值y_i的偏差的加权平方和为最小。对于等精度观测值的线性拟合，即：使下式的值最小即可：

对上式中的k₁和b₁分别求偏导得：

整理后，得到：

解上述方程，得到：

(其中，i＝1，2，……，N)

于是，可求得反光板在防撞激光直角坐标系中初步拟合的直线方程y＝k₁x+b₁中的斜率k₁和截距b₁的最佳估计值

S15.对初步拟合的反光板的干扰数据点进行过滤，保留有效点，首先将计算被拟合的数据点到初步拟合直线的距离；然后，计算这些距离的标准偏差；最后，将距离大于两倍标准偏差的数据点剔除，保留距离小于两倍标准偏差的数据点。

S16.对干扰剔除后的反光板的有效数据点进行重新直线拟合，根据类似于S14的方法，对剔除干扰后的有效数据点(x_j，y_j)(其中，j＝1，2，……，M)进行直线拟合，可求得反光板在防撞激光传感器直角坐标系中拟合的直线方程y＝k₂x+b₂中的斜率k₂和截距b₂的最佳估计值和

(其中，j＝1，2，……，M)

S17.获取反光板与激光中心线的夹角，假设反射板与防撞激光中心线(即x轴)夹角用α表示，于是，有：

请结合图5所示，对初步拟合的反光板的干扰数据点进行过滤，保留有效点，具体如下：

S151.计算被拟合点到初步拟合直线的距离，计算反光板上若干数据点在防撞激光坐标系中的坐标(x_i，y_i)到初步拟合直线的距离d_i。

S152.计算距离的标准偏差，被拟合点(x_i，y_i)到初步拟合直线的距离d_i的均值和标准偏差σ_d，于是有：

S153.距离不大于两倍标准偏差，根据距离d_i与其两倍的标准偏差σ_d比较，剔除“干扰”点，保留集中点。

S154.该点为有效点，予以保留，当d_i≤2σ_d时，保留第i个点的数据，其中i＝1，2，……，N

S155.该点为干扰点，予以剔除，当d_i＞2σ_d时，删除第i个点的数据，其中i＝1，2，……，N

假设删除部分干扰点后，剩余有效数据点的总数用M表示。

请结合图6所示，计算AGV车当前姿态角具体如下：

S21.AGV车上的前、后天线同时探测到有效磁订，AGV车上的前、后天线都经过预先埋设在地面里并写入准确坐标信息的磁订时，前、后天线都会发送一个探测到有效磁订的标识，启动AGV车在地面全局坐标系中姿态角的计算。

S22.获取前、天线探测到的磁订在前天线坐标系中的坐标，结合图7计算AGV车姿态角的三角关系示意图，假设前天线中心点为前天线坐标系的坐标原点，朝AGV车体外侧为前天线坐标系的x轴正方形，根据笛卡尔坐标系的原则，得到前天线坐标系的y轴正方向。前天线探测到有效磁订时，前天线可以获取到磁订在天线坐标系中的坐标，在前天线坐标系中，根据磁订中心点的坐标，利用直角三角形的关系，可以轻易求出磁订A的中心到前天线中心点O_F的距离和夹角∠AO_FB的大小。

S23.获取后天线探测到的磁订在后天线坐标系中的坐标，还是结合图7计算AGV车姿态角的三角关系示意图，假设后天线中心点为后天线坐标系的坐标原点，朝AGV车体外侧为后天线坐标系的x轴正方形，根据笛卡尔坐标系的原则，得到后天线坐标系的y轴正方向。后天线探测到有效磁订时，后天线可以获取到磁订在天线坐标系中的坐标，在后天线坐标系中，根据磁订中心点的坐标，利用直角三角形的关系，可以轻易求出磁订C的中心到后天线中心点O_R的距离和夹角∠CO_RB的大小。

S24.根据前、后天线坐标原点分别相对于前、后天线探测到磁订的横向偏差，计算AGV车的姿态角，结合图7计算AGV车姿态角的三角关系示意图，根据三角形的正弦定理有：

由于∠O_FBA＝∠O_RBC，且为前、后天线探测到的磁订的距离，为已知量。所以，可以求得：

∠O_FBA即为AGV与磁订中心线的夹角，记为β。

综上所述，本发明可以根据安装于AGV车前方的激光防撞传感器探测到集装箱缓冲支架侧边激光反光板的数据和AGV车当前的位置和姿态，自动计算激光防撞传感器的偏转角度，控制偏转角度误差在0.15度以内。从而减少人力成本，提高精度和效率。

本技术领域中的普通技术人员应当认识到，以上的实施例仅是用来说明本发明，而并非用作为对本发明的限定，只要在本发明的实质精神范围内，对以上所述实施例的变化、变型都将落在本发明的权利要求书范围内。