一种基于PSD测距的AGV精确定位方法与流程

本发明涉及一种基于psd测距的agv精确定位方法，涉及机械工程、电子工程领域中的检测、定位、调姿等。

背景技术：

agv是柔性制造系统的重要组成部分，在生产过程自动化和智能化方面有着缺一不可的重要作用，尤其是在离散制造型企业，完成多线路间不定点多个工位的即时自动搬运，实现制造业的自动化，进行加工、装配等，所以agv定位精度的高低，直接影响产品的加工和装配精度，影响产品的特性。

目前agv导航定位方式有：惯性定位、视觉定位、激光定位、gps定位、光电定位等，定位精度较低，无法满足精密制造、装配等领域对agv定位精度的要求。定位精度最高为±5mm，为对agv的粗定位。完全依靠agv粗定位，存在定位误差较大的问题，达不到加工和装配指标的情况，无法满足某些对定位精度要求较高的使用要求。

技术实现要素：

本发明解决的技术问题为：克服现有技术不足，提供一种基于psd测距的agv精确定位方法，用于解决现有技术中agv定位精度低的问题。

本发明解决的技术方案为：一种基于psd测距的agv精确定位方法，其特征在于步骤如下：

(1)在自动导引运输车agv(automatedguidedvehicle)底盘安装两个位置敏感检测器psd(positionsensitivedetector)，并分别建立agv坐标系和psd坐标系；

(2)在需要对agv进行精确定位的工位处，安装两个激光发射仪；

(3)采用视觉导引方式，使agv的中心相对于步骤(2)需要对agv进行精确定位的工位中心的定位精度达到±10mm以内，完成agv粗定位；

(4)在步骤(3)agv粗定位完成后，步骤(2)的两个激光发射仪发出的激光束，分别照射在步骤(1)所述的两个psd的光敏面上形成两个光点，agv实时采集两个光点分别在两个psd坐标系o1-x1y1、o2-x2y2下的坐标(x1,y1)，(x2,y2)；

(5)根据步骤(4)获得的光点的坐标(x1,y1)，(x2,y2)，确定agv的当前位姿，包括当前工位坐标系相对于agv坐标系的偏斜角度、工位坐标系原点相对于agv坐标系的原点在x方向和y方向的偏移距离；

(6)将步骤(5)获得的agv的当前位姿与目标位姿对比，目标位姿为：agv当前偏斜角度为0°、横向和纵向的偏移距离也均为0mm，通过基于麦克纳姆轮的agv的全向运动，实时agv位姿的调整；

(7)agv位姿调整过程中，实时解算agv的当前位姿，直至定位精度达到±0.3mm，精确定位完成。

所述的步骤(1)中的psd为二维psd，光敏面为其重要组成部分，外部的两个激光发射仪发射的两个激光束能够分别照射至两个二维psd的光敏面上形成两个光点，两个二维psd能够分别测得自己光敏面上的光点的x坐标和y坐标。agv底盘所在的平面平行于承载agv的地面，psd的光敏面与agv底盘所在平面平行，朝向地面安装，激光束垂直于承载agv的地面，则激光束垂直于光敏面。agv运动过程中，两个psd之间的相对位置保持不变。坐标系建立如下：两个psd的坐标系o1-x1y1、o2-x2y2与agv的坐标系oa-xaya均为平面直角坐标系，两个psd的坐标系所在的平面均与agv的坐标系所在的平面平行，agv的坐标系原点位于底盘中心,agv的坐标系和两个psd坐标系的y轴正方向指向agv的前进方向，3个坐标系的x轴在一条直线上，指向agv前进方向的右侧，且两个psd坐标系的坐标原点分别为两个psd光敏面的中心，且以agv坐标系的y轴为对称轴，对称分布。

所述步骤(2)中的激光发射仪安装在需要对agv进行精确定位的工位处，以地面为坐标平面，以该工位的中心为坐标原点，垂直工位向右为x轴，建立工位坐标系og-xgyg，工位坐标系为平面直角坐标系。安装激光发射仪时，使两个激光发射仪的激光发射方向均垂直地面向上且激光束的反向延长线与工位坐标系的x轴相交，两个交点以工位坐标系的y轴为对称轴，对称分布，且交点与工位坐标系的y轴的距离与psd坐标系的坐标原点与agv坐标系的y轴的距离相等。两个激光束分别垂直照射在两个psd的光敏面上，各形成一个激光点，且两激光束中心点间的距离与两个psd光敏面的中心距相同。

所述步骤(3)中的视觉导引方式为：需要对agv进行精确定位的工位处设置粗定位标识，粗定位标识为与工位坐标系的x轴平行，且与agv的理想定位位置处停车时，朝向粗定位标识的车体端面之间距离为400mm。在agv停到工位后agv坐标系的x轴与工位坐标系og-xgyg的该条直线形成的夹角为agv当前偏斜角度，agv的前方、后方、左侧和右侧四个方向的中心位置处各安装一套视觉定位系统；根据运动指令，agv定位方向的视觉定位系统能够拍摄到agv停止线，且停止线与agv端面之间的距离在395mm到405mm之间，且停止线与agv坐标系的x轴或y轴(与车头方向有关)之间夹角在-0.5°～0.5°之间时粗定位完成。

所述步骤(5)中需要对agv进行精确定位工位处对agv的车头方向有要求，车头方向必须固定，以agv的理论车头方向与工位坐标系的y轴平行为例进行说明，agv的当前位姿确定过程如下：

1)将psd坐标系下的两个光点坐标(x1,y1)，(x2,y2)转换到agv坐标系下，分别为(x1-a,y1)，(x2+a,y2)：

其中，a为每个psd坐标系的坐标原点与agv坐标系的坐标原点之间的距离；

2)利用agv坐标系下的光点坐标值(x1-a,y1)，(x2+a,y2)，计算agv当前的位姿，即计算agv当前偏斜角度、agv坐标系原点相对于工位坐标系的原点在x方向和y方向的偏移距离；

agv的当前偏斜角度为：α＝arctan(k)

其中k为直线的斜率，且x2-x1+2a≠0

工位坐标系原点相对于agv坐标系的原点在x方向偏移距离，即agv的横向偏移距离为：

工位坐标系原点相对于agv坐标系的原点在y方向偏移距离，即agv的纵向偏移距离为：

所述步骤(6)和(7)中的位姿调整过程即为确定agv的旋转方向和运动方向并执行相应动作的过程，agv位姿调整过程中，agv的运动速度在0.2mm/s到1mm/s之间，旋转速度在0.3°/s到1°/s之间，防止位姿调整过程中的超调震荡。精确定位过程中工位坐标系原点相对于agv坐标系的原点在x方向和y方向的偏移距离x和y满足:x²+y²≤0.3²时，则停止agv的运动，精确定位完成,定位精度达到±0.3mm。

本发明中将agv的运动方向和agv车头方向之间的夹角记为agv的偏航角度，偏航角度取值范围为[0°,360°)。agv向前方即agv车头方向运动时，偏航角度为0°。

计算agv偏航角度的步骤如下：

1)若x＝0，则agv需沿agv坐标系的y方向运动，即agv应向前方或后方运动。

若此时y>0，则agv的偏航角度为β＝0°，即agv应向前运动；

若此时y<0，则agv的偏航角度为β＝180°，即agv应向后运动；

若此时y＝0，则满足，x²+y²≤0.3²agv精确定位完成，无需再调整。

2)若x>0，则agv的偏航角度为

3)若x<0，则agv的偏航角度为

计算agv旋转方向的步骤如下：

4)若k>0，则agv应逆时针旋转；

5)若k<0，则agv应顺时针旋转；

6)若k＝0,则agv无需旋转。

对agv进行精确定位的过程为确定agv偏航角度和agv旋转方向的复合运动过程。

步骤(1)所述的一种基于psd测距的agv精确定位方法，其特征在于：所述agv为基于麦克纳姆轮的agv，基于麦克纳姆轮的agv的全向运动包括：向前、向后直行，向左、向右横移，0回转半径原地旋转及上述运动的组合。

本发明与现有技术相比的优点在于：

(1)本发明中采用以激光发射仪为基准点，光束竖直向上发射至psd的光敏面的安装方式，使激光点的坐标与agv的位姿一一对应，保证agv位姿的精确性；

(2)本发明将麦克纳姆轮agv的全向运动和二维psd精确定位相结合，使agv的定位精度优于±0.3mm，解决了agv定位精度不高，无法满足高定位精度要求的问题。

(3)本发明中可根据实际需要，通过在需要对agv进行精确定位的多个工位处分别安装激光发射仪，可实现多线路间不定点多个工位的精确定位，提高自动加工或装配等流水线的布置和更改的灵活性。

(4)本发明中通过对agv进行精确定位使得生产现场的精确定位操作自动完成，大大减小了定位调整的时间和难度，有效提高了生产效率和自动化水平。

(5)本发明中，通过对agv定位精度的提高，直接提高了产品的加工和装配精度，提高了产品本身的性能，增加了产品的附加价值。

附图说明

图1本发明实施例所述的psd坐标系和agv坐标系的关系示意图；

图2本发明实施例所述的工位坐标系示意图；

图3本发明实施所述的agv粗定位位置示意图；

图4本发明实施所述的agv坐标系与工位坐标系的关系示意图；

图5本发明实施所述的agv精确定位方法流程图。

具体实施方式

本发明的基本思路为：提出一种基于psd测距的agv精确定位方法如下：(1)采用视觉导引方式，使agv的定位精度达到±10mm以内；(2)agv粗定位完成后，利用psd测距，测得两个基准点坐标，从而根据直线方程确定agv当前横向和纵向的偏移距离和偏斜角度，得到agv的当前位姿；(3)agv当前姿态和目标姿态作为输入，建立agv的运动方程，计算agv的偏航角度、旋转角度；(4)通过麦克纳姆轮agv的全向运动(向前、向后直行，向左、向右横移，任意角度斜行，0回转半径原地旋转及上述运动的组合)，实现agv的姿态调整；(5)agv姿态调整过程中，实时解算agv的位姿，直至定位精度达到±0.3mm为止。本发明的定位精度高，计算简单，工程实现容易。

下面结合附图和具体实施例对本发明做进一步详细描述，

(1)在自动导引运输车agv(automatedguidedvehicle)底盘安装两个二维位置敏感检测器psd(positionsensitivedetector)，光敏面为psd的重要组成部分，外部的两个激光发射仪发射的两个激光束能够分别照射至两个二维psd的光敏面上形成两个光点。agv又名无人搬运车，指装备有自动导引装置，能够沿预定的导引路径自动行驶，具有安全保护以及各种移载功能的无人驾驶运输车。agv能够将货物或物料自动从起始点运送到目的地，并且其行驶路径可以根据仓储货位要求、生产工艺流程等改变而灵活改变。agv一般由车体、蓄电和充电装置、驱动装置、自动导引装置、车上控制器、安全保护装置、移载装置等组成。agv种类和形式多种多样。常常根据agv自动行驶过程中的导航方式将agv分为：电磁感应导引式agv、激光导引式agv、视觉导引式agv等。psd是一种能测量光点在探测器光敏面上连续位置的光学探测器。该psd的位置信号数据与光点在探测器上的形状无关，只与光点的能量中心有关。它分为一维psd和二维psd。一维psd探测出一个入射光点在它的光敏面上的一个维度的位置。二维psd探测出一个入射光点在其光敏面上的两个维度(x和y)的位置。本发明中agv底盘所在的平面平行于承载agv的地面，psd的光敏面与agv底盘所在平面平行且朝向地面，激光束垂直于承载agv的地面，则激光束垂直于光敏面。agv运动过程中，两个psd之间的相对位置保持不变。坐标系建立如下：两个psd的坐标系o1-x1y1、o2-x2y2与agv的坐标系oa-xaya均为平面直角坐标系，如图1所示为psd坐标系和agv坐标系的关系示意图，两个psd的坐标系所在的平面均与agv的坐标系所在的平面平行，agv的坐标系原点位于底盘中心,agv的坐标系和两个psd坐标系的y轴正方向指向agv的前进方向，3个坐标系的x轴在一条直线上，指向agv前进方向的右侧，且两个psd坐标系的坐标原点分别为两个psd光敏面的中心，且以agv坐标系的y轴为对称轴，对称分布，便于后续坐标转换和姿态解算。

(2)在需要对agv进行精确定位的工位处，安装两个激光发射仪作为精确定位的基准。其中需要对agv进行精确定位的工位处是指：在自动装配等自动化流水线中，agv用于运载零部件到相关工位处，由机械手自动抓取零部件进行自动装配，机械手对零部件的定位精度有较高要求以保证最终装配精度，而运载过程中零部件和agv的相对位置固定，所以在高精度装配工位处要求对agv进行精确定位。以地面为坐标平面，以该工位的中心为坐标原点，垂直工位向右为x轴，建立工位坐标系og-xgyg，如图2所示，工位坐标系为平面直角坐标系。安装激光发射仪时，使两个激光发射仪的激光发射方向均垂直地面向上，激光发射仪发出的光束垂直照射在psd的光敏面上，保证两个激光点的坐标与agv的位姿一一对应，保证agv位姿的精确测量；且激光束的反向延长线与工位坐标系的x轴相交，两个交点以工位坐标系的y轴为对称轴，对称分布，且交点与工位坐标系的y轴的距离与psd坐标系的坐标原点与agv坐标系的y轴的距离相等。

(3)采用视觉导引方式，使agv的中心相对于步骤(2)需要对agv进行精确定位的工位中心的定位精度达到±10mm以内，完成agv粗定位。视觉导引方式为：需要对agv进行精确定位的工位处设置粗定位标识，粗定位标识为与工位坐标系的x轴平行，且与agv的理想定位位置(agv定位停车无误差时，agv的定位位置)处停车时，朝向粗定位标识的车体端面之间距离为400mm，如图3所示。当前工位坐标系相对于agv坐标系的偏斜角度是指：在agv停到工位后agv坐标系的x轴与工位坐标系og-xgyg的x轴形成的夹角为agv当前偏斜角度，agv的前方、后方、左侧和右侧四个方向的中心位置处各安装一套视觉定位系统；根据运动指令，agv定位方向的视觉定位系统能够拍摄到agv停止线，且停止线与agv端面之间的距离在395mm到405mm之间，且停止线与agv坐标系的x轴或y轴(与车头方向有关)之间夹角在-0.5°～0.5°之间时粗定位完成，使agv相对于需要对agv进行精确定位的工位中心的的定位精度达到±10mm以内，是指：以需要对agv进行精确定位的工位中心为圆点，半径为10mm画一个圆，agv的中心落入该圆内，则满足定位精度，以保证激光发射仪发出的光束能照射在psd25mm×25mm的光敏面上。

(4)在步骤(3)agv粗定位完成后，步骤(2)的两个激光发射仪发出的激光束，分别照射在步骤(1)所述的两个psd的光敏面上形成两个光点，优选方案为：agv实时判断照射在光敏面上的光点个数，若光点数小于2，则精确定位无法执行，agv向agv调度系统反馈错误信息。此时需要agv调度系统重新对agv下发粗定位的指令，进行agv位姿调整，或人工干预，检查并修复激光发射仪、psd、agv的线路等，使粗定位完成后两个psd光敏面内各可检测到一个光点。若agv检测到的光点数为2，则记录两个光点分别在两个psd坐标系o1-x1y1、o2-x2y2下的坐标(x1,y1)，(x2,y2)；

(5)根据步骤(4)获得的光点的坐标(x1,y1)，(x2,y2)，确定agv的当前位姿，包括当前agv坐标系相对于工位坐标系的偏斜角度、agv坐标系原点相对于工位坐标系的原点在x方向和y方向的偏移距离，此时agv坐标系与工位坐标系的关系如图4所示。需要对agv进行精确定位工位处对agv的车头方向有要求，以agv的理论车头方向(agv停车定位无误差时，车头的方向)与工位坐标系的y轴平行为例进行说明，agv的当前位姿确定过程如下：

1)将psd坐标系下的两个光点坐标(x1,y1)，(x2,y2)转换到agv坐标系下，分别为(x1-a,y1)，(x2+a,y2)：

其中，a为每个psd坐标系的坐标原点与agv坐标系的坐标原点之间的距离；

2)利用agv坐标系下的光点坐标值(x1-a,y1)，(x2+a,y2)，计算agv当前的位姿，即计算agv当前偏斜角度、agv坐标系原点相对于工位坐标系的原点在x方向和y方向的偏移距离；

agv的当前偏斜角度为：α＝arctan(k)

其中k为直线的斜率，且x2-x1+2a≠0。

由于本发明选取的psd的光敏面的尺寸为25mm×25mm，故x1、x2、y1、y2允许的范围为[-12.5,12.5](单位为mm)，而a由安装距离确定，本发明中a为100(单位为mm)，因此计算k时不存在分母为0的情况；

工位坐标系原点相对于agv坐标系的原点在x方向偏移距离，即agv的横向偏移距离为：

工位坐标系原点相对于agv坐标系的原点在y方向偏移距离，即agv的纵向偏移距离为：

(6)将步骤(5)获得的agv的当前位姿与目标位姿对比，目标位姿为：agv当前偏斜角度为0°、横向和纵向的偏移距离也均为0mm，通过基于麦克纳姆轮的agv的全向运动，确定agv的旋转方向和偏航角度并执行相应动作，实时调整agv的姿态。基于麦克纳姆轮的agv的全向运动包括向前、向后直行，向左、向右横移，0回转半径原地旋转及上述运动的组合。精调过程中，agv的运动速度较低，为0.2mm/s到1mm/s之间，旋转速度在0.3°/s到1°/s之间，防止位姿调整过程中的超调震荡，处理流程如图5所示。精确定位过程中agv坐标系原点相对于工位坐标系的原点在x方向和y方向的偏移距离x和y满足:x²+y²≤0.3²时，则停止agv的运动，精确定位完成，定位精度达到±0.3mm。如图4所示α为agv当前偏斜角度，即工位坐标系的x轴与agv坐标系的x轴之间的夹角。

本发明中将agv的运动方向和agv车头方向之间的夹角记为agv的偏航角度，偏航角度取值范围为[0°,360°)。agv向前方即agv车头方向运动时，偏航角度为0°。

计算agv偏航角度的步骤如下：

1)若x＝0，则agv需沿agv坐标系的y方向运动，即agv应向前方或后方运动。

若此时y>0，则agv的偏航角度为β＝0°，即agv应向前运动；

若此时y<0，则agv的偏航角度为β＝180°，即agv应向后运动；

若此时y＝0，则满足，x²+y²≤0.3²agv精确定位完成，无需再调整。

2)若x>0，则agv的偏航角度为

3)若x<0，则agv的偏航角度为

计算agv旋转方向的步骤如下：

7)若k>0，则agv应逆时针旋转；

8)若k<0，则agv应顺时针旋转；

9)若k＝0,则agv无需旋转。

对agv的位姿调整为上述运动的复合运动。

(7)agv位姿调整过程中，实时解算agv的当前位姿，并重复：检测当前位姿、位姿符合性判断和位姿调整的过程，直至定位精度达到±0.3mm，精确定位完成。

本发明将麦克纳姆轮agv的全向运动和二维psd精确定位相结合，使agv的精确定位自动完成，且定位精度优于±0.3mm，解决了agv定位精度不高，无法满足高定位精度要求的问题，大大减小了定位调整的时间和难度，有效提高了生产效率和自动化水平，提高了产品的加工和装配精度，提高了产品本身的性能，增加了产品的附加价值。通过在需要对agv进行精确定位的多个工位处分别安装激光发射仪，可实现多线路间不定点多个工位的精确定位，提高自动加工或装配等流水线的布置和更改的灵活。