[发明专利]一种直角弯下Mecanum轮式AGV导航定位方法及系统

摘要

本发明公开了一种直角弯下Mecanum轮式AGV导航定位方法，首先建立全局坐标系、AGV初始状态坐标系和激光雷达坐标系；根据激光雷达线段特征在全局坐标系下中建立对直角弯和AGV初始状态描述模型；通过AGV内部的编码器获取AGV转速数据，基于Mecanum轮式AGV的运动学模型，实现航位推算算法，计算在初始状态坐标系中AGV质心的坐标及姿态；最后计算在全局坐标系中对AGV外轮廓及转向状态的描述，实现定位。本发明提供的方法为工程上实现Mecanum轮式AGV在直角弯下的导航定位提供了一种解决方案，简单易行、且不需额外配置惯性传感器。

主权项

1.一种直角弯下Mecanum轮式AGV导航定位方法，其特征在于：包括以下步骤：S1：建立全局坐标系oxy、初始状态坐标系o′x′y′和激光雷达坐标系o″x″y″；在全局坐标系中，根据激光雷达线段特征建立直角弯和AGV初始状态描述模型；S2：通过AGV内部的编码器获取AGV转速数据并基于Mecanum轮式AGV运动学模型实现航位推算算法计算在初始状态坐标系o′x′y′中AGV质心的坐标(x′k,y′k,θ′k)；进而计算在全局坐标系oxy中AGV质心坐标和姿态(xk,yk,θk)；S3：计算在全局坐标系中AGV外轮廓及转向状态描述；所述直角弯和AGV初始状态描述模型的建立具体步骤如下：S11：在激光雷达坐标系o″x″y″中，将激光雷达距离数据(d1,d2…dm)通过线段特征提取，提取出线段(L1,L2,L3)；(d1,d2…dm)为激光雷达原始距离数据；Li表示为激光雷达在直角弯下距离图像的线段特征，Li的描述为直线的参数模型(θi,pi)，θi代表过原点做直线的垂线，垂线与x″轴的夹角，pi为原点到直线的距离；(θ1,p1)、(θ2,p2)、(θ3,p3)分别代表其对应线段的参数模型；S12：在全局坐标系oxy中，按照以下公式建立直角弯的描述模型：其中，F点的坐标为(w₁,‑w₂)，E点的坐标为(0,0)，w₁、w₂为通道的宽度，w₁为L₁,L₂之间的距离，w₂为F到L₂之间的距离；K₂表示线段L₂的斜率；b₂表示线段L₂的截距；(x₁₂，y₁₂)表示线段L₁的结束点；直角弯的描述模型F点的坐标为E点的坐标为(0,0)；E、F点表示在全局坐标下直角弯的拐点的坐标；S13：在全局坐标系oxy中，分别按照以下步骤来实现在全局坐标下中对AGV初始状态描述；若θs>0，即AGV朝右，则有：令M₂＝l₁cosθ_s；若θs<0，即AGV朝左，则有：令M₂＝l₁cosθ_s；其中，AGV的轴线与L1线段的延长线相交于H，夹角为θs，(x0,y0,θ0)表示AGV质心O′在oxy坐标系中的初始坐标；(θi,pi)表示直线Li的参数模型(θi,pi)，在直角弯下有L1,L2,L3三条线段，(θ1,p1)、(θ2,p2)、(θ3,p3)分别对应其参数模型；l1表示AGV长度的一半；所述在全局坐标系中AGV质心坐标和姿态的计算，具体步骤如下：S21：按照以下公式建立Mecanum轮式AGV的运动学模型；其中，r为Mecanum轮辊子半径，w1,w2,w3,w4为AGV四轮的转速；(vx,vy,w)表示在初始状态坐标系中，AGV质心的速度，分别对应x方向、y方向，w代表姿态角；S22：根据航位推算原理计算在t＝kT时刻，计算在初始状态坐标系o′x′y′中AGV质心的坐标(x′k,y′k,θ′k)的递推公式为：其中，(x′k,y′k,θ′k)表示在初始状态坐标系o′x′y′中AGV质心的坐标；(△xi,△yi,△θi)表示在第i个周期内，AGV质心的偏移量；S23：根据全局坐标系oxy与质心坐标系o′x′y′转换关系式计算在全局坐标系oxy中的质心坐标(xk,yk,θk)：其中，(x0,y0,θ0)为在全局坐标系oxy下质心的初始坐标；(xk,yk,θk)为全局坐标系oxy下AGV质心的坐标；所述在全局坐标系中AGV外轮廓及转向状态描述的具体步骤如下：S31：根据以下旋转关系计算AGV外轮廓ABCD在全局坐标系下的坐标：A点的坐标：(‑l2*cos(θk)+l1*sin(θk)+xk,l2*sin(θk)+l1*cos(θk)+yk,θk)B点的坐标：(l2*cos(θk)+l1*sin(θk)+xk,‑l2*sin(θk)+l1*cos(θk)+yk,θk)C点的坐标：(l2*cos(θk)‑l1*sin(θk)+xk,‑l2*sin(θk)‑l1*cos(θk)+yk,θk)D点的坐标：(‑l2*cos(θk)‑l1*sin(θk)+xk,l2*sin(θk)‑l1*cos(θk)+yk,θk)；其中，ABCD为AGV外轮廓坐标；(xk,yk,θk)为全局坐标系下质心坐标；S32：按照以下公式计算在全局坐标系中AGV转向状态：J1＝2l1*cos(θk)，J2＝‑2l1*sin(θk)；J3＝‑J2*(l2*sin(θk)‑l1*cos(θk)+yk)‑(l2*cos(θk)‑l1*sin(θk)+xk)*J1；其中，AGV在转向过程中发生碰撞的障碍点；外轮廓顶点A与L2边，侧边BC与拐点F，D点距离边L3；AGV外轮廓顶点A与L₂轴的距离为弯道拐点F距离轮廓BC的距离为disBC‑F；D点距离L₃轴为