技术领域
[0001] 本
发明属于
移动机器人导航
定位技术领域，具体为一种基于直角弯道特征的全方位移动机器人位姿校准方法。
背景技术
[0002] 全方位移动机器人，也即AGV，以其高自动化程度、灵活性好、空间利用率高等诸多优点迅速在物流行业及其他行业中得到广泛的应用，并成为制造业系统和自动化仓储系统重要的物流输送设备。
[0003] 智能车的位姿估计，简称为定位，是ACV自动控制及导航中不可缺少的重要环节。在一般环境中，移动机器人的位姿通常使用三维向量来表示，即相对于全局坐标
位置的横向、纵向平移分量和代表其朝向的旋转角度分量。精确的位姿估计对于移动机器人的自动地图生成、路径规划和控制、目标检测和
跟踪等具有重要意义。
[0004] 常见的定位技术主要有基于测距法和惯性导航的相对定位，以及GPS定位、路标定位和地图匹配等绝对定位技术。现有定位方法多存在技术成本与
精度之间的矛盾，以航位推算为主的相对定位技术方法简单、技术成本低，但存在累积误差问题；绝对定位技术没有累积误差，但该类技术存在技术复杂、方法移植性差、技术成本高等问题。
[0005] 由于单独的相对定位和绝对定位技术都存在一定的不足之处，在一些定位精度要求高的复杂环境中，定位结果往往不够理想。实际应用中，多以两种方法相结合的方式，以绝对位姿信息对相对位姿信息进行的校准更新，消除定位累积误差，从而提高整体位姿估算的精准度。
[0006] 现有的位姿校准方法中，技术较为成熟的主要有基于GPS数据融合、导航信标和标签、Landmarks等，但GPS数据多用于室外环境，室内
信号易受遮挡干扰，且定位精度多以米为量级，室内精确定位精度不够；基于信标和标签的定位方法定位精度高，实时性好，但该类定位方法需要在
传感器检测环境范围内放置大量特征明显的人工路标或标签，工作量较大，在仓储室等环境复杂的场景中路标易损坏，设备维护成本高；其次，基于地图匹配的位姿校准方法在室内环境下研究的也比较多，比较典型的有
激光雷达SLAM、视觉SLAM技术等，但该类方法
算法复杂度较高、计算量大，且在仓库通道这种相似度较高的环境下，地图识别与匹配的过程易产生误差；此外，在一些基于环境特征的位姿校准方法中，比较有代表性的有基于柱状特征和室内直角特征的技术，该类方法灵活性高，但前者在空旷的室内环境中效果较好，在一些环境复杂的场合效果不理想；而基于室内直角特征的方法中，虽然在结构化比较明显的室内环境下有很好的适用性，但该类方法在提取角点信息的过程中易受传感器噪声或环境干扰因素的影响，精度易受影响。
[0007] 因此，急需一种适用于全向移动机器人，在自动化仓储通道环境下，适用性好、切实可行的位姿校准方法。
发明内容
[0008] 有鉴于此，为了解决上述问题，本发明提供一种基于直角弯道特征的全方位移动机器人位姿校准方法，该方法在充分分析典型仓库通道环境的情况下，利用激光雷达数据对直角弯道处的环境特征进行描述，并以此特征对全方位小车在通道环境下进行全局定位与位姿更新，消除相对定位中的累积误差，提高机器人的整体定位精度。
[0009] 本发明的目的是通过以下技术方案来实现的：本发明提供的一种基于直角弯道特征的全方位移动机器人位姿校准方法，该方法包括以下步骤：
[0010] S1.建立直角弯模型，包括L型直角弯模型、I型直角弯模型、T型直角弯模型；
[0011] 其中，所述L型直角弯模型包括由两面墙面抽象拟合出的呈垂直相交状态的两线段和一由货包堆垛抽象拟合的第一线段，由两面墙面抽象拟合出的呈垂直相交状态的两线段的其中一线段与由货包堆垛抽象拟合的第一线段平线；
[0012] 所述T型直角弯模型包括由一面墙面抽象拟合出的第二线段和由两货包堆垛抽象拟合的呈平行状态的两线段，由两货包堆垛抽象拟合的呈平行状态的两线段与由一面墙面抽象拟合出的第二线段垂直但不相交；
[0013] 所述I型直角弯模型包括由一面墙面抽像拟合出的第三线段和由两货包堆垛抽象拟合的呈平行状态的两线段，由一面墙面抽像拟合出的第三线段与由两货包堆垛抽象拟合的呈平行状态的两线段垂直且相交；或所述I型直角弯模型包括由一货包堆垛抽像拟合出的第四线段和由两墙面抽象拟合的呈平行状态的两线段，由一货包堆垛抽像拟合出的第四线段与由两墙面抽象拟合的呈平行状态的两线段垂直且相交；
[0014] S2.将直角弯模型进行标准化，然后建立直角弯处全局参考点集；
[0015] S3.建立小车初始状态
坐标系，并计算AGV小车在全局坐标系中的初步位姿估计；
[0016] S4.对直角弯特征进行识别；
[0017] S5.基于直角弯特征对AGV小车进行全局定位和位姿更新。
[0018] 优选地，所述L型直角弯模型为：
[0019]
[0020] 所述T型直角弯模型为：
[0021]
[0022] 所述I型直角弯模型为：
[0023]
[0024] 在二维直角坐标系下线段Li＝(pi,θi,start,end,len)，其中，start_q和end_q分别表示小车前方
墙壁拟合线段的起、始端点坐标，start_(q+1)小车左侧通道内壁拟合线段的起点坐标，end_(q-1)表示小车右侧通道内壁拟合线段的始点坐标，pi表示从原点到直线Li的距离，θi表示过原点与Li的垂线与x轴正方向的夹角，start和end分别表示线段Li的起始端点坐标，len表示线段Li的长度，δdmin和δdmax表示两点之间欧式距离的最小
阈值和最大阈值，δdmax用于判断相邻两条线段首尾是否相交，δdmin用于判断相邻两条线段首位间的距离是否大于一定阈值。
[0025] 优选地，在所述步骤S2中，对L型直角弯模型特征标准化的方法为：
[0026] 将由两面墙面抽象拟合出的呈垂直相交状态的两线段及由两面墙面抽象拟合出的呈垂直相交状态的两线段中的一条线段作为固定特征；
[0027] 对T型直角弯模型特征标准化方法为：
[0028] 首先分别求出由两货包堆垛抽象拟合的呈平行状态的两线段延长线与第二线段的交点坐标；
[0029] 然后求两交点坐标的算术平均值，将该算术平均值作为关键点Key_P；
[0030] 并将关键点Key_P和第二线段作为定位的关键特征；
[0031] 对I型直角弯模型特征标准化的方法为：
[0032] 首先分别求出由两货包堆垛抽象拟合的呈平行状态的两线段延与第三线段的交点坐标；
[0033] 然后求两交点坐标的算术平均值，将该算术平均值作为关键点Key_P'；
[0034] 并将关键点Key_P'和第三线段作为定位的关键特征；
[0035] 或对I型直角弯模型特征标准化的方法为：
[0036] 首先分别求出由两墙面抽象拟合的呈平行状态的两线段延与第四线段的交点坐标；
[0037] 然后求两交点坐标的算术平均值，将该算术平均值作为关键点Key_P”；
[0038] 并将关键点Key_P”和第四线段作为定位的关键特征。
[0039] 优选地，在所述步骤S2中，在建立全局坐标系时；以仓库入口抽象拟合的点为原点，以垂直于货包堆垛抽象拟合的线段的方向为y轴，以平行于货包堆垛抽象拟合的线段的方向为x轴建立全局坐标系xoy；并在全局坐标系下建立直角弯处定位关键点的全局参考点集P＝{P1,P2,...,Pn}，其中Pi＝(xi,yi,θri)，i∈[1,n]，(xi,yi)表示标准化后的直角弯两条相交直线交点在全局坐标系xoy下的坐标，θri表示以墙面抽象拟合的直线与全局坐标系xoy的x轴的夹角。
[0040] 优选地，所述步骤S3具体包括以下子步骤：
[0041] S31.以AGV小车的几何中心为原点建立小车的初始状态的
车身坐标系x′o′y′，建立全方位移动小车
运动学模型：
[0042]
[0043] 其中，(w1,w2,w3,w4)表示AGV小车的四个轮子的转速，r为
车轮半径，m、n分别表示车身宽度、长度的一半，(vx,vy,w)表示质心在车身初始坐标系下分别沿x轴、y轴、车头方向的移动速度；
[0044] S32.根据航位推算原理计算在t＝kT时刻，在车身坐标系x′o′y′下，计算小车质心坐标(x′k,y′k,θ′k)，θk'表示小车车头方向与小车质心坐标系的y正方向的夹角，T表示航位推算
采样周期，k表示正整数；
[0045] S33.根据全局坐标系xoy与小车的车身坐标系x′o′y′转换关系式计算小车在全局坐标系xoy下质心坐标(xk,yk,θk)，θk表示小车车头方向与全局坐标系的y正方向的夹角。
[0046] 优选地，所述步骤S4具体包括以下子步骤：
[0047] S41.首先，人工遥控AGV小车在通道环境中进行初步路径的学习，生成直角弯初步识别的全局路径关键点地图；在AGV小车进行后续自主导航的过程中，根据先验地图，采用位姿跟踪方法实时判别AGV小车是否进入直角弯初步识别的全局路径关键点地图的邻域范围内，其邻域范围定义为以关键点为中心，半径为r1的圆形区域；
[0048] S42.基于激光雷达的线段特征提取，利用split_merge算法对激光雷达扫描数据进行线段特征提取得到线段集合L'；对环境中提取的线段进行无效数据的剔除和线段再次合并得到新的线段集合L”；
[0049] S43.利用直角弯模型对直角弯附近一
帧激光雷达中的线段集合L”进行模型判别并对其进行分类。
[0050] 优选地，所述步骤S5具体包括以下子步骤：
[0051] S51.参考坐标关联；当AGV小车完成直角弯特征的识别后，需要将此时AGV小车位置处直角弯特征与预设的全局参考坐标点进行关联匹配，获得正确关联参考点；
[0052] S52.基于线段特征的AGV位姿解算，利用激光雷达扫描数据中提取的标准化直角弯模型线段和关键特征，对激光雷达进行位姿解算得到全局坐标系xoy下激光雷达的位姿o″(xo,yo,θo)；
[0053] S53.AGV位姿更新，通过激光雷达与AGV小车坐标系转换求出AGV小车在全局坐标系xoy下的位置坐标及
姿态角(xr,yr,θr)，利用直角弯的全局定位结果对AGV小车的相对定位结果进行更新。
[0054] 由于采用了上述技术方案，本发明具有如下的优点：
[0055] 本发明在充分分析典型仓库通道环境的情况下，利用激光雷达数据对直角弯道处的环境特征进行描述，并以此特征对全方位小车在通道环境下进行全局定位与位姿更新，消除相对定位中的累积误差，提高机器人的整体定位精度。
[0056] 本发明的其他优点、目标和特征在某种程度上将在随后的
说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。
附图说明
[0057] 为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步的详细描述：
[0058] 图1为典型自动化仓库应用环境；
[0059] 图2为L型弯示意图；
[0060] 图3为T型弯示意图；
[0061] 图4为I型弯示意图；
[0062] 图5为L型弯标准化；
[0063] 图6为T型弯标准化；
[0064] 图7为I型弯标准化；
[0065] 图8为参考坐标点集示意图；
[0066] 图9为全方位移动小车车身坐标系；
[0067] 图10为基于关键点判别的直角弯初步定位示意图；
[0068] 图11为IEPF算法分割原理；
[0069] 图12为split_merge算法提取的通道线段示意图；
[0070] 图13为基于线段特征的AGV定位原理图；
具体实施方式
[0072] 以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。
[0073] 请参阅图1至图14。需要说明的是，本
实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。
[0074] 本实施例的基于直角弯道特征的全方位移动机器人位姿校准方法，包括如下步骤：
[0075] Step 1：建立直角弯模型。
[0076] 仓库通道环境多由立体货架及墙面构成，由于货包的摆放方式一般比较整齐规则，在通道内部形成比较典型的结构化几何特征，在通道转弯处，形成比较明显的由线段组成的直角弯特征，如图1所示。
[0077] 通过环境分析可知，仓库通道环境中存在的典型直角弯主要有以下几类：
[0078] Step 11：建立L型直角弯模型
[0079] 图2所示为所示为L型直角弯示意图，该场景下的通道由两面墙面和一面货包堆垛组成，典型的L型直角弯主要由三条线段(L1,L2,L3)组合而成，L型直角弯模型包括由两面墙面抽象拟合出的呈垂直相交状态的两线段和一由货包堆垛抽象拟合的第一线段，由两面墙面抽象拟合出的呈垂直相交状态的两线段的其中一线段与由货包堆垛抽象拟合的第一线段平线。在二维直角坐标系下线段Li＝(pi,θi,start,end,len)，其中pi表示从原点到直线Li的距离，θi表示过原点与Li的垂线与x轴正方向的夹角，start和end分别表示线段Li的起始端点坐标，len表示线段Li的长度，基于以上线段参数和实际环境中通道组合特性，该场景下的L型直角弯的模型描述为：
[0080]
[0081]
[0082] 上式中，式(1)是对L型直角弯模型轮廓构成的粗略描述，式(2)是对满足粗略轮廓的模型作进一步精确描述，主要用于判断L型直角弯模型中两面墙面抽象拟合出的呈垂直相交状态的两线段构成的一个直角特征；num_L表示从激光雷达一帧扫描数据中提取的线段的总条数，len_min表示组成L型弯线段的最短长度，ε表示用于判断两条线段间夹角的最小邻域，δdmin和δdmax表示两点之间欧式距离的最小值和最大阈值，δdmax用于判断相邻两条线段首尾是否相交，δdmin用于判断相邻两条线段首位间的距离是否大于一定阈值，式(2)中“||”表示数学运算符“或”。
[0083] Step 12：建立T型直角弯模型
[0084] 图3所示为T型直角弯模型示意图，该场景下的通道由一面墙面和两面货包堆垛组成。该场景下的弯道抽象模型也由三条线段组成，T型直角弯模型包括由一面墙面抽象拟合出的第二线段和由两货包堆垛抽象拟合的呈平行状态的两线段，由两货包堆垛抽象拟合的呈平行状态的两线段与由一面墙面抽象拟合出的第二线段垂直但不相交。结合线段参数和实际环境中通道组合特性，该场景下T型直角弯道的模型描述为：
[0085]
[0086]
[0087] 其中，式4主要是对墙面拟合出的线段进行识别，其中的参数含义与T型直角弯中的参数描述一致。
[0088] Step 13：I型直角弯模型
[0089] 图4所示为I型通道尽头属于一种特殊类型的直角弯，由于其具备与典型直角弯相似的线段组合特性，本实敦例将其归属于用于全局定位的典型直角弯一类。根据构成该类直角弯周围环境的不同，该类型的直角弯主要分为两种类型，I型直角弯模型包括由一面墙面抽像拟合出的第三线段和由两货包堆垛抽象拟合的呈平行状态的两线段，由一面墙面抽像拟合出的第三线段与由两货包堆垛抽象拟合的呈平行状态的两线段垂直且相交；或所述I型直角弯模型包括由一货包堆垛抽像拟合出的第四线段和由两墙面抽象拟合的呈平行状态的两线段，由一货包堆垛抽像拟合出的第四线段与由两墙面抽象拟合的呈平行状态的两线段垂直且相交。该类型的直角弯模型描述为：
[0090]
[0091]
[0092] Step 2：直角弯特征标准化及建立全局参考点集
[0093] 为了使基于线段特征的定位方法在不同模型中具有通用性，首先将经过模型识别分类的直角弯进行标准化，即提取出用于AGV小车定位的相同特征；然后建立直角弯处全局参考点集。
[0094] Step 21：直角弯特征标准化
[0095] Step 211：L型直角弯标准化
[0096] 如图5所示，(a)(b)分别为实际直角弯模型及表转化后的直角弯模型，该类型的直角弯以两面相交的墙面拟合的直线L2和L3的交点Key_P及其中的一条拟合直线作为定位所用的固定特征，实际应用中，一般以与车身横坐标轴方向斜率最接近的一条直线作为定位用的关键直线特征。
[0097] Step 212：T型直角弯标准化
[0098] 如图6所示，该场景中直角弯特征标准化过程中，首先分别求出由两货包堆垛抽象拟合的呈平行状态的两线段延长线与第二线段L2的交点坐标P23和P21，然后由P23和P21算术平均值求出关键点Key_P，并将求出的关键点Key_P和墙面抽象拟合出的第二线段L2作为定位的关键特征，其中：
[0099]
[0100] Step 213：I型直角弯标准化
[0101] 如图7所示，该场景中直角弯特征标准化过程中，首先分别求出由两货包堆垛抽象拟合的呈平行状态的两线段延长线与第二线段L2的交点坐标P23和P21，然后由P23和P21算术平均值求出关键点Key_P，并将求出的关键点Key_P和墙面抽象拟合出的第二线段L2作为定位的关键特征，其中：
[0102]
[0103] Step 22：建立全局参考点集
[0104] 在标准化仓库环境下，货架的空间位置摆放一般与仓库中的其中一面墙面呈垂直或平行状态，基于此现状，本实施例在建立全局坐标系时，以仓库入口抽象拟合的点为原点，以垂直于货包堆垛抽象拟合的线段的方向为y轴，以平行于货包堆垛抽象拟合的线段的方向为x轴建立全局坐标系xoy；并在全局坐标系下建立直角弯处定位关键点的全局参考点集P＝{P1,P2,...,Pn}，其中Pi＝(xi,yi,θri)，i∈[1,n]，(xi,yi)表示标准化后的直角弯两条相交直线交点在全局坐标系xoy下的坐标，θri表示以墙面抽象拟合的直线与全局坐标系xoy的x轴的夹角。如图8所示，将参考点集按顺序存储于
数据库中。
[0105] Step 3：建立小车初始状态坐标系，利用车身内部的
编码器获取小车运动参数，结合全向移动
机器人运动学模型计算小车在全局坐标系中的初步位姿估计。
[0106] 如图9所示，以AGV小车的几何中心为原点建立小车的初始状态车身坐标系x′o′y′，其中y轴方向与车头方向平行，图中激光雷达坐标系x″o″y″为车身坐标系x′o′y′沿车头正方向
水平偏移距离n。
[0107] Step 31:建立全方位移动小车运动学模型
[0108]
[0109] 上式中，(w1,w2,w3,w4)表示根据车轮编码器数据计算四个轮子的转速，r为车轮半径，m、n分别表示车身宽度、长度的一半，(vx,vy,w)表示质心在车身初始坐标系下分别沿x轴、y轴、车头方向的移动速度。
[0110] Step 32：根据航位推算原理计算在t＝kT时刻，在车身坐标系x′o′y′坐标系下，AGV质心的坐标(x′k,y′k,θ′k)为：
[0111]
[0112] (Δxi，Δyi，Δθi)表示在第i个周期内，AGV小车质心的偏移量。因为T足够小，所以假定在周期T内，在初始状态坐标下(Δxi,Δyi,Δθi)为恒定值，则有：
[0113]
[0114] Step 33：根据全局坐标系xoy与车身坐标系x′o′y′坐标系转换关系式计算小车在全局坐标系xoy下质心坐标(xk,yk,θk)：
[0115]
[0116] 其中，(x0,y0,θ0)为在全局坐标系xoy下质心的初始坐标，θ为小车车头方向与全局坐标系xoy的y正方向的夹角，(x′k,y′k,θ′k)表示车身坐标系x′o′y′坐标系下小车质心坐标。
[0117] Step 4：直角弯特征识别
[0118] 基于直角弯特征对AGV小车进行全局定位的过程中，准确的识别出直角弯特征是进行全局定位的第一步，本实施例对直角弯识别的过程主要分为初步定位和精确判定两大步骤。
[0119] Step 41：直角弯初步定位
[0120] 首先，人工遥控AGV小车在通道环境中进行初步路径的学习。当AGV小车行进至直角弯附近时，操作中控电脑记录下该位置处基于航位推算得到的小车位置(xi,yi)，并将该定位结果作为AGV小车运动至直角弯附近时的初步判别关键点，将其按顺序存储于数据库，生成直角弯初步识别的全局路径关键点地图。
[0121] 在AGV小车进行后续自主导航的过程中，采用位姿跟踪方法实时判别AGV小车是否进入全局路径关键点地图的邻域范围内，其邻域范围定义为以关键点为中心，半径为r1的圆形区域，具体示意图如图10所示。
[0122] Step 42：直角弯精确判定
[0123] 对直角弯进行初步定位判定后，利用前面的直角弯判定模型对初步定位邻域范围内的环境做进一步识别，判定该邻域范围内是否有符合模型的直角弯特征，以及属于哪一类直角弯特征。该过程中主要包括两个环节：基于激光雷达的线段特征提取和直角弯模型判定。
[0124] Step 421：基于激光雷达的线段特征提取
[0125] 本实施例利用split_merge算法对激光雷达扫描数据进行线段特征提取，该算法主要分为分割-拟合-合并三个步骤。
[0126] 分割阶段：把不属于同一条直线的点集进行分割，采用IEPF聚类方法，通过对直角弯附近激光雷达一帧扫描的点
云数据进行分割，得到多个属于不同直线的点集集合D＝(D1,D2,...,Dn)。
[0127] 如图11所示，对某一区域中的激光数据点Di，首先选取该区域中的首尾两点为端点拟合直线li，同时设定点到直线的距离阈值dthd，对首尾两点间的所有点进行遍历，计算出点到直线的距离di，并找出点到直线li的最大距离dmax，若dmax＜dthd，则判定该区域中所有点在同一条直线上，否则，以该区域中到首位点拟合直线距离最大的点Pmax为分界点，将该区域中的点集分割成两个点集{Pj(xj,yj)|j＝1,2,...,i}和{Pj(xj,yj)|j＝i+1,i+2,...,n}，然后分别在每个点集中重复进行上述分割步骤，依次类推，直到所有的点集满足分割条件为止，最后得到多个属于不同直线的点集集合D＝(D1,D2,...,Dn)。
[0128] 拟合阶段：对分割后的点集集合D＝(D1,D2,...,Dn)分别进行最小二乘的线段拟合，得到每个集合的线段表示L＝(L1,L2,...,Ln)，i∈[1,n]，Li＝(pi,θi,start,end,len)，其中pi表示从原点到直线Li的距离，θi表示过原点与Li的垂线与x轴正方向的夹角，start和end分别表示线段Li的起始端点坐标，len表示线段Li的长度。
[0129] 合并阶段：对线段集合L＝(L1,L2,...,Ln)中满足一定阈值范围的线段进行合并，防止同一条线段被分割成多段，造成线段过拟合，相邻两条直线Li,Lj的合并准则为：|pi-pj|＜δp，并且|θi-θj|＜δθ，其中，δp和δθ分别为线段参数的合并阈值，对线段集合进行合并处理得到新的线段集合L′＝(L1,L2,...,Lm)。
[0130] 通过分割-拟合-合并三个步骤即可完成对一帧激光雷达环境扫扫描数据的准确提取，但在由货包堆垛组成的实际通道环境中，由于货包与货包间的摆放会存在一定的间隙，从激光雷达一帧环境扫描数据中会提取出多条间断的共线线段，如图12所示。
[0131] 为了使实际场景中的基于线段特征对直角弯的描述符合前面的典型直角弯模型，在此需要对环境中提取的线段进行无效数据的剔除和线段再次合并。
[0132] 无效数据剔除：主要是对堆垛货包间夹缝中拟合的线段进行剔除，在激光雷达坐标系下，对任意线段Li＝(pi,θi,start,end,len)，有以下两个剔除原则为：1)对满足||θi|-90°|＜thdθ的线段中，若pi＜thdp，对该线段剔除；若pi＞thdp且len_i＜dmin，剔除；2)对满足||θi|-180°|＜thdθ或者||θi|-0°|＜thdθ的线段，若len_i＜dmin，剔除，；其中，参数thdθ为线段极径角与90°之间的判别阈值，thdp为线段极径长度需要满足的阈值，dmin为最小线段长度阈值。
[0133] 再次合并：对剔除无效数据后的线段采用split_merge算法的线段合并方法进行再次合并最终得到新的线段集合L″＝(L1,L2,...,Lk)。
[0134] Step 422：基于线段特征的模型判定
[0135] 利用典型直角弯模型对直角弯附近一帧激光雷达中的线段集合L″＝(L1,L2,...,Lk)进行模型判别并对其进行分类。
[0136] Step 5：基于直角弯特征的全局定位与位姿更新
[0137] Step 51：参考坐标关联
[0138] 当AGV小车完成直角弯特征的识别后，需要将该位置处直角弯特征与预设的全局参考坐标点进行关联匹配。
[0139] 本实施例采用“最近邻”法对参考坐标进行关联匹配，匹配过程中，结合AGV小车航位推算结果估算出直角弯标准化模型中参考关键点坐标Pi′(Xi′,Yi′)，利用“最近邻”法在参考坐标点集中搜索点Pi′，当搜索到某个参考点Pj与点Pi′之间的欧式距离最小时，则认为Pj是该直角弯处的正确关联参考点。
[0140] Step 52：基于线段特征的AGV位姿解算
[0141] 如图13所示，存在两个笛卡尔坐标系，全局坐标系xoy和激光雷达坐标系x′o′y′，已知全局坐标系下两条相交直线L1＝(k1,b1,p1,θ1)，L2＝(k2,b2,p2,θ2)，直线的交点坐标A(xA,yA)，其中ki,bi分别代表直线的斜率和截距，pi,θi分别为直线相应的极径和极角；在激光雷达坐标系下测量得到直线参数分别为L1′＝(k1′,b1′,p1′,θ1′)，L2′＝(k2′,b2′,p2′,θ2′)，直线交点坐标A′(x′A,y′A)，设全局坐标系下激光雷达坐标系原点的位姿为o″(xo″,yo″,θo″)，其中θo表示激光雷达在全局坐标系中的姿态朝向角，图中表示为雷达坐标系x轴正方向与全局坐标系x轴正方向夹角，结合图13及上述条件可得：
[0142]
[0143]
[0144] 将式14带入式13便可求出全局坐标系下激光雷达的位姿o″(xo,yo,θo)。
[0145] Step 53：AGV位姿更新
[0146] 本文中，激光雷达与AGV车体属于刚体连接状态，如图9所示，同理，通过激光雷达与AGV小车坐标系转换可求出AGV在全局坐标系下的位置坐标及姿态角。
[0147] 图9中，利用直角弯特征得到的激光雷达在全局坐标系下的位姿为o″(xo,yo,θo)，通过坐标转换得到AGV小车在全局坐标系xoy下的位置坐标及姿态角(xr,yr,θr)：
[0148]
[0149] 最后，利用直角弯的全局定位结果对AGV小车的相对定位结果进行更新，消除相对定位结果中的累积误差，完成AGV小车的全局位姿校准。
[0150] 本实施例的基于直角弯道特征的全方位移动机器人位姿校准方法在充分利用激光雷达采集的环境信息的情况下，结合组成典型直角弯道的结构化线性化特点，提出了基于直角弯特征的全向移动机器人位姿校准方法，此方法为工程上实现全向移动机器人通道内精确定位提供了一种解决方案，该方案易于实现、适应性强，能够解决通道内移动机器人定位误差问题。
[0151] 最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行
修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的保护范围当中。
