一种直角弯下Mecanum轮式AGV导航定位方法及系统的制作方法
【专利摘要】本发明公开了一种直角弯下Mecanum轮式AGV导航定位方法，首先建立全局坐标系、AGV初始状态坐标系和激光雷达坐标系；根据激光雷达线段特征在全局坐标系下中建立对直角弯和AGV初始状态描述模型；通过AGV内部的编码器获取AGV转速数据，基于Mecanum轮式AGV的运动学模型，实现航位推算算法，计算在初始状态坐标系中AGV质心的坐标及姿态；最后计算在全局坐标系中对AGV外轮廓及转向状态的描述，实现定位。本发明提供的方法为工程上实现Mecanum轮式AGV在直角弯下的导航定位提供了一种解决方案，简单易行、且不需额外配置惯性传感器。
【专利说明】
-种直角弯下Mecanum轮式AGV导航定位方法及系统
技术领域
[0001] 本发明设及轮式移动机器人自主导航领域，特别设及一种实现Mecanum轮式AGV导 航定位方法，适用室内直角弯场景下Mecan皿轮式AGV的定位W及运动状态的描述。
【背景技术】
[0002] 随着机器人技术的不断进步，移动机器人的应用领域越来越广泛，其重要发展方 向是全自主化，即自主导航。自主导航首先要解决第一大问题，即"where am 1(我在哪 里)"，是关于自主移动机器人的定位问题，也是移动机器人实现自主导航的一个首要的问 题，最重要的一项功能之一。
[0003] 定位问题可W描述成机器人通过传感器感知环境，建立合理的数学模型来描述移 动机器人的工作环境，并确定自身在工作环境中的精确位置，W及运动状态。机器人的定位 方式取决于所采用的传感器，常见的定位传感器有惯性传感器、摄像机、激光雷达、超声波、 红外线、巧螺仪等。
[0004] 常见的定位系统有GPS全球定位系统，但是GPS只能用在室外，而且在直角弯运样 一个局部场景下并不能描述AGV的位姿信息。其次，比较常见的还有惯导定位系统，基于惯 性传感器实现航位推算的方法。比如速度或加速度计、电子罗盘、巧螺仪等惯性传感器实现 机器人的位姿的推算。由于惯性传感器需要额外配置，所W成本比较高，而且容易受到磁场 干扰，对推算精度造成较大的影响。传统的航位推算是基于内部传感器，并不能描述AGV的 初始状态。而且仅仅只能得到坐标信息，并不能反映 AGV轮廓、姿态信息，W及AGV与环境之 间的关系。所W在直角弯，运样一个局部的场景下并不适用。
[0005] 因此，急需一种适用于Mecanum轮式AGV，在直角弯局部场景下，成本低、切实可行 的导航定位方法。
【发明内容】
[0006] 有鉴于此，为了解决上诉问题，本发明在充分分析直角弯特性的情况下，利用激光 雷达建立对直角弯场景W及AGV初始状态的描述。然后基于Mecanum轮式移动机构运动学模 型，利用AGV内部的编码器实现在直角弯场景之下AGV的航位推算。并在此基础之上，描述 AGV外轮廓坐标信息W及与环境之间的关系，实现全面的定位。
[0007] 本发明的目的是通过W下技术方案来实现的：
[000引本发明提供的一种直角弯下Mecanum轮式AGV导航定位方法，包括W下步骤：
[0009] Sl:建立全局坐标系〇巧、初始状态坐标系0' X' y'和激光雷达坐标系o"x"y";根据 激光雷达线段特征在全局坐标系下中建立对直角弯和AGV初始状态描述模型；
[0010] S2:通过AGV内部的编码器获取AGV转速数据;并基于Mecanum轮式AGV运动学模型 实现航位推算算法计算在初始状态坐标系c/x//中AGV质屯、的坐标^/1^，/1<，0/〇;及计算 在全局坐标系中AGV质屯、坐标和姿态(xk，yk, 9k);
[001 U S3:计算在全局坐标系中AGV夕帷廓及转向状态描述。
[0012] 进一步，所述直角弯和AGV初始状态描述模型的建立具体步骤如下：
[0013] S11:在激光雷达坐标系o"x"y"中，将激光雷达距离数据(dl，d2...dm)转换成线段 (, Ls, Ls);
[0014] 其中，（dl，d2...dm)为激光雷达原始距离数据;Li表示为激光雷达在直角弯下距离 图像的线段特征，Li的描述为直线的参数模型（0i，pi)，0i代表过原点做直线的垂线，垂线与 X"轴的夹角，Pi为原点的直线的距离；
[001引 Li，L2山代表在直角弯下的立条线段，（目1，P1)、（目2，P2)、（目3，P3)分别代表对应线段 的参数模型；
[0016] S12:在全局坐标系oxy中，按照W下公式建立直角弯的描述模型：
[0017]
[001引其中，F点的坐标为(W1，-W2)，E点的坐标为(0,0)，*1、*2为通道的宽度，*1为1^1山之 间的距离，W2为巧IjL込间的距离瓜表示线段L2的斜率;b康示线段L2的截距；（X12，yi2)表示 线段^的结束点；直角弯的描述模型F点的坐标夫
E点的坐标为 (0,0);
[0019] E、F点表示在全局坐标下直角弯的拐点的坐标。
[0020] S13:在全局坐标系oxy中，分别按照W下步骤来实现在全局坐标下中对直角弯和 AGV初始状态描述；
[0021] 若目s〉0,即AGV朝右，则有：
[0022]
[0023]
[0024]
[0025]
[0026]
[0027]其中，AGV的轴线与^线段的延长线相交于H，夹角为目s，（xo，y日，目日)表示AGV质屯、〇/ 在oxy坐标系中的初始坐标；
[002引（目i，pi)表示直线以的参数模型（目i，pi);在直角弯表现为1^1山山立条线段；
[0029] h表示AGV长度的一半；
[0030] 进一步，所述在全局坐标系中AGV质屯、的坐标W及姿态具体步骤如下：
[0031] S21:按照W下公式建立Mecan皿轮式AGV的运动学模型；
[0032]
[00削其中，r为Mecanum轮漉子半径，'\￥1,讯2，讯3，讯4为46￥四轮的转速;讯1、'\￥2、讯3、讯4为按照固 定周期T通过编码器的采集值；
[0034] (Vx, Vy, W)表示在初始位置坐标系中，AGV质屯、的速度，分别对应X方向、y方向，W代 表姿态角；
[00巧]S22:根据航位推算原理计算在t = kT时刻，计算在初始状态坐标系〇/中AGV质 屯、的坐标(x'k，y'k，目'k)为：
[0036]
[0037] 其中，^'1^，7'1<，目'0表示在初始状态坐标系〇'义'7'中46￥质屯、的坐标。
[003引（A Xi, A yi，A目i)表示在第i个周期内，AGV质屯、的偏移量。
[0039] S23:根据全局坐标系oxy与质屯、坐标系c/ /转换关系式计算在全局坐标系oxy 中的质屯、坐标(xk，yk，9k):
[0040]
[004。 其中，（xo，yo, 00)为在全局坐标系oxy下质屯、的初始坐标。
[00创 （xk，yk,目k)为全局坐标系O巧下AGV质屯、的坐标。
[0043] 进一步，所述在全局坐标系中AGV外轮廓及转向状态描述的具体步骤如下：
[0044] S31:根据W下旋转关系计算AGV外轮廓ABCD在全局坐标系下的坐标：
[0045] A点的坐标：（_l2*cos(目k)+li*sin(目k)+xk, l2*sin(目k)+li*cos(目k)+yk,白k)
[0046] B点的坐标：（l2*cos(目k)+li*sin(目k)+xk,-l2*sin(目k)+li*cos(目k)+yk,白k)
[0047] C点的坐标：（l2*cos(目k)-li*sin(目k)+xk,-l2*sin(目k)-li*cos(目k)+yk,白k)
[004引 D点的坐标：（_l2*cos(目k)-li*sin(目k)+xk, l2*sin(目k)-li*cos(目k)+yk,目k);
[0049] 其中，ABCD为AGV外轮廓坐标；（a,yk，目k)为全局坐标系下质屯、坐标；
[0050] S32:按照W下公式计算在全局坐标系中AGV转向状态：
[0化1 ]
[0052] ji = 2h*cos(目k)，j2 = -2h*sin(目k);
[0053] J3 = -J2*(l2*sin(目k)-li*cos (目k)+yk)-(l2*cos (目k)-li*sin (目k)+xk)*Ji;
[0054] 其中，AGV在转向过程中发生碰撞的障碍点；外轮廓顶点A与L2边，侧边BC与拐点F， D点距离边L3;
[005引 AGV外轮廓顶点A与L2轴的距离为王:；
[0056] 弯道拐点F距离轮廓BC的距离为disBC-F;
[0057] D点距离L3轴为泌如-為。
[005引本发明还提供了一种直角弯下Mecanum轮式AGV导航定位系统，包括坐标系建立模 块、通道模型建立模块、AGV初始状态描述模型建立模块、数据采集模块、全局质屯、坐标计算 模块、全局外轮廓计算模块和全局转向状态描述计算模块；
[0化9]所述坐标系建立模块，用于建立全局坐标系〇巧、初始状态坐标系〇/ /和激光雷 达坐标系〇"x"y";
[0060] 所述通道模型建立模块，用于根据激光雷达线段特征在全局坐标系下中建立直角 弯模型；
[0061] 所述AGV初始状态描述模型建立模块，用于根据激光雷达线段特征在全局坐标系 下中建立AGV初始状态描述模型；
[0062] 所述数据采集模块，用于通过AGV内部的编码器获取AGV转速数据；
[0063] 所述全局质屯、坐标计算模块，用于基于Mecanum轮式AGV运动学模型实现航位推算 算法计算在全局坐标系oxy中AGV质屯、的坐标(Xk，yk，0k);
[0064] 所述全局外轮廓计算模块，用于计算在全局坐标系中AGV外轮廓；
[0065] 所述全局转向状态描述计算模块，用于计算在全局坐标系中的转向状态描述。
[0066] 进一步，所述直角弯和AGV初始状态描述模型的建立具体步骤如下：
[0067] S11:在激光雷达坐标系o"x"y"中，将激光雷达距离数据(di，d2...dm)转换成线段 (, Ls, Ls);
[0068] 其中，Li表示为激光雷达在直角弯下距离图像的线段特征，Li的描述为直线的参数 模型（0i，Pi)，01代表过原点做直线的垂线，垂线与X"轴的夹角，P功原点的直线的距离；
[0069] S12:在全局坐标系O巧中，按照W下公式计算W2为F到L2之间的距离：
[0070]
[0071] 其中，F点的坐标为(W1，-W2)，E点的坐标为(0,0)，巧1、巧2为通道的宽度，巧1为1^1，1^2之 间的距离W2为巧化2之间的距离;K2表示线段L2的斜率;b2表示线段L2的截距；（X12，yi2)表示 线段^的结束点；直角弯的描述模型F点的坐标为
3点的坐标为 (0,0);
[0072] E、F点表示在全局坐标下直角弯的拐点的坐标。
[0073] S13:在全局坐标系oxy中，分别按照W下步骤来实现在全局坐标下中对直角弯和 AGV初始状态描述；
[0074] 若目s〉0,即AGV朝右，则有：
[0075]
[0076]
[0077]
[007引
[0079]
[0080] 其中，AGV的轴线与^线段的延长线相交于H，夹角为0s，AGV质屯、〇/在oxy坐标系中 的初始坐标为(xo，yo，0〇);
[OOW](目1，Pi)表示直线以的参数模型（目1，Pi);
[0082] h表示AGV长度的一半；
[0083] 进一步，所述在全局坐标系中AGV质屯、的坐标W及姿态具体步骤如下：
[0084] S21:按照W下公式建立Mecan皿轮式AGV的运动学模型；
[0085]
[0086] 其中，r为Mecanum轮漉子半径，'\￥1,讯2，讯3，讯4为46￥四轮的转速;讯1、'\￥2、讯3、讯4为按照固 定周期T通过编码器的采集值；
[0087] (vx，vy，w)表示在初始位置坐标系中，AGV质屯、的速度。
[0088] S22:根据航位推算原理计算在t = kT时刻，计算在初始状态坐标系〇/中AGV质 屯、的坐标(x'k，y'k，目'k)为：
[0089]
[0090] 其中，（X' k，y' k,目'k)表示在初始状态坐标系0' X' y'中AGV质屯、的坐标。
[00W] ( A Xi, Ayi, A 0i)表示在第i个周期内，AGV质屯、的偏移量。
[0092] S23:根据全局坐标系〇巧与质屯、坐标系c/ /转换关系式计算在全局坐标系oxy 中的质屯、坐标(xk，yk，9k):
[0093]
[0094] 其中，（xo，yo,目0)为在全局坐标系〇巧下质屯、的初始坐标，由S13计算可得。
[00巧](Xk，yk,目k)为全局坐标系O巧下AGV质屯、的坐标。
[0096] 进一步，所述在全局坐标系中AGV外轮廓及转向状态描述的具体步骤如下：
[0097] S31:根据W下旋转关系计算AGV外轮廓ABCD在全局坐标系下的坐标：
[009引 A点的坐标：（-l2*cos(目k)+li*sin(目k)+xk, l2*sin(目k)+li*cos(目k)+yk,白k)
[0099] B点的坐标：（l2*cos(目k)+li*sin(目k)+xk，-l2*sin(目k)+li*cos(目k)+yk,白k)
[0100] C点的坐标：（l2*cos(目k)-li*sin(目k)+xk，-l2*sin(目k)-li*cos(目k)+yk,白k)
[0101 ] D点的坐标：（-l2*cos(目k)-li*sin(目k)+xk, l2*sin(目k)-li*cos(目k)+yk,目k);
[010^ 其中，ABCD为AGV外轮廓坐标；（a,yk，目k)为全局坐标系下质屯、坐标；
[0103] S32:按照W下公式计算在全局坐标系中AGV转向状态：
[0104]
[0105] ji = 2h*cos(目k)，j2 = -2h*sin(目k);
[0106] j3 = -j2*(l2*sin(0k)-li*cos(0k)+yk)-(l2*cos(0k)-li*sin(0k)+xk)*ji;
[0107] 其中，AGV在转向过程中发生碰撞的障碍点；外轮廓顶点A与L2边，侧边BC与拐点F， D点距离边L3;
[010引 AGV外轮廓顶点A与L2轴的距离为凉為-心；
[0109] 弯道拐点F距离轮廓BC的距离为disBC-F;
[0110] D点距离L3轴为觀O一A。
[0111] 由于采用了上述技术方案，本发明具有如下的优点：
[0112] 本发明提供的方法为工程上实现Mecanum轮式AGV在直角弯下的导航定位提供了 一种解决方案，简单易行、且不需额外配置惯性传感器。基于激光雷达建立了对直角弯场景 W及AGV初始姿态的描述模型。并通过模型实现基于Mecanum轮式AGV运动学模型实现航位 推算，建立了在整个弯道场景下对AGV的定位、外轮廓的描述W及AGV转向时与环境之间的 关系。
[0113] 本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并 且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可 W从本发明的实践中得到教导。本发明的目标和其他优点可W通过下面的说明书来实现和 获得。
【附图说明】
[0114] 本发明的【附图说明】如下。
[0115] 图1为算法流程图。
[0116] 图2为S个坐标系的建立。
[0117] 图3为基于激光雷达线段特征建立弯道描述模型。
[0118] 图4为基于激光雷达线段特征建立AGV初始状态描述模型。
[0119] 图5为建立对外轮廓及转向状态的描述。
【具体实施方式】
[0120] 下面结合附图和实施例对本发明作进一步说明。
[0121] 实施例1
[0122] 如图1-5所示，其中，图1为算法流程图。图2为S个坐标系的建立。W直角弯拐点E 为坐标原点，通道方向为X轴建立oxy坐标系。WAGV质屯、为坐标原点，轴线为纵坐标轴，建立 0' X' y'坐标系。W激光雷达为坐标原点，建立激光雷达o"x"y"坐标系。图3为基于激光雷达 线段特征建立弯道描述模型。直角弯场景下，在〇"x"y"坐标系中，经过线段分割和直线拟合 算法，可W得到转换成线段山山山）心为直线的参数模型（01瓜）。图4为基于激光雷达线 段特征建立AGV初始状态描述模型。在O巧全局坐标系中，AGV轴线与^线段的延长线相交于 H，夹角为目S，设AGV质屯、〇/在〇巧坐标系中的初始坐标为(XO，yo，目日）。图5为建立对外轮廓及 转向状态的描述。AGV的外轮廓为ABCD，在全局坐标系下质屯、坐标为(祉，yk，目O，夕帷廓顶点 A与L2轴的距离为^叫心，弯道拐点F距离轮廓BC的距离为disBG-F，D点距离L3轴为心如-V。
[0123] 本实施例提供的直角弯下Mecanum轮式AGV导航定位方法，利用激光雷达建立对直 角弯场景W及AGV初始状态的描述，然后基于Mecanum轮式移动机构运动学模型，利用AGV内 部的编码器实现在直角弯场景之下AGV的航位推算;通过AGV外轮廓坐标信息W及与环境之 间的关系实现全面的定位。
[0124] 本实施例提供的定位方法中存在=个坐标系，如图1所示，W直角弯拐点E为坐标 原点，通道方向为X轴建立oxy坐标系。该坐标系称为全局坐标系，属于绝对坐标系，用于实 现全局的定位。WAGV质屯、为坐标原点，轴线为纵坐标轴，建立〇/坐标系，称为初始状态 坐标系。该坐标系属于绝对坐标系，用于实现航位推算。W激光雷达为坐标原点，建立激光 雷达〇"x"y"坐标系，该坐标系属于局部坐标系，用于建立对直角弯场景W及AGV初始姿态的 描述。
[01剧如图2所示，AGV长为2h，宽为212,狭窄直角弯场景宽为Wi XW2,单位cmec/坐 标系与oxy坐标系的夹角为0s，o"x"y"相对于o'x'y'坐标系向上平移了h。
[0126] 具体步骤如下：
[0127] (1)基于激光雷达线段特征建立对直角弯和AGV初始状态描述模型
[012引在直角弯场景下，如图3所示，在o"x"y"坐标系中，经过线段分割和直线拟合算法， 可W将激光雷达距离数据(dl，d2...dm)通过线段特征提取，提取线段特征化1，12，1^3)。以为 直线的参数模型（0i，Pi)，其中01代表过原点做直线的垂线，垂线与X"轴的夹角，Pi为原点的 直线的距离。其斜率一截距模型为化I，bi)。线段的起始、结束坐标为(Xil，yil)、（Xin，yin)。
[0129] 如图3所示，在oxy坐标系中，F点的坐标为(wi,-W2)，E点的坐标为(0,0) eWi、W2为通 道的宽度。*1为^，L2之间的距离，W2为F到L2之间的距离，则有：
[0130]
[0131] 所W，可得到在全局坐标系中F点的坐标；i巧点的坐标 为(〇,〇)。
[0132] 如图4所示，在oxy全局坐标系中，AGV轴线与b线段的延长线相交于H，夹角为0S，设 AGV质屯、〇/在oxy坐标系中的初始坐标为(XO，yo，目0)。
[0133] 罢目。>0 .目n AGV迦右.加巧由祈志.麵巧.
[0134]
[0135]
[0136]
[0137]
[013 引
[0139] 综上所述:即可在全局坐标下中建立了对直角弯W及AGV初始状态的描述。
[0140] (2)在全局坐标系中基于运动学模型的航位推算
[0141] Mecaruim轮式AGV的运动学模型
[0142]
[014；3 ] 其中r为Mecanum轮漉子半径，Wi, W2，W3，W4为AGV四轮的转速，基于AGV内部的编码 器可W实时获取。W某个固定的周期T通过编码器采集巧1、巧2、巧3、巧4的值。在* = 4刊寸刻，采集 的转速的值为wi(k)，W2(k)，W3(k)，W4(k)。'屯.,丫、九這W/为AGV在0' X' y'坐标系下的位
[0145] 根据航位推算原理可得在t = kT时刻，在〇/坐标系下，AGV质屯、的坐标(X^ k，y V目'k)为： 移量，因为T足够小，所W假定在周期T内，在初始状态坐标下兩.,.,.、欠线为恒定值， 则有
[0144]
[0146]
[0147] 由此递推关系模型可实时获得在〇/坐标系中，AGV质屯、的坐标W及姿态。由前 面已知，在坐标系〇巧下，质屯、的初始坐标为(XO，y日，目日），根据〇巧与c/ /坐标系转换关系 式可得，小车在oxy坐标系下质屯、的坐标(Xk，yk，0k)为：
[014 引
[0149] 由此可实时地获得得在全局坐标系中，AGV导航过程中，质屯、的坐标W及姿态。
[0150] (3)在全局坐标系中对AGV外轮廓及转向状态的描述
[0151] 在直角弯运样局部场景下，要保证通过性，实现质屯、的导航定位是不够的，需要建 立对整个外轮廓W及外轮廓与环境关系的描述。
[0152] 如图5所示，AGV的外轮廓为ABCD,在全局坐标系下质屯、坐标为(xk，yk,目k)，由上一 节推算可得。则基于旋转关系可得：
[0153] A点的坐标：（-l2*cos(目k)+li*sin(目k)+xk, l2*sin(目k)+li*cos(目k)+yk,白k)
[0154] B点的坐标：（l2*cos(目k)+li*sin(目k)+xk，-l2*sin(目k)+li*cos(目k)+yk,白k)
[015引 C点的坐标：（l2*cos(目k)-li*sin(目k)+xk，-l2*sin(目k)-li*cos(目k)+yk,白k)
[0156] D点的坐标：（-l2*cos(目k)-li*sin(目k)+xk, l2*sin(目k)-li*cos(目k)+yk,白k)
[0157] AGV在转向过程中，发生碰撞的地方有立处，即外轮廓顶点A与L2边，侦赃BC与拐点 F，D点距离边L3。令AGV外轮廓顶点A与L2轴的距离为(心'，k，弯道拐点F距离轮廓BC的距离为 diSBC-F，D点距离L3轴为^如-￡，，贝贿：
[015引令ji = 2h*cos(目k)，j2 = -2h*sin(目k);
[0159] j3 = -j2*(l2*sin(0k)-li*cos(0k)+yk)-(l2*cos(0k)-li*sin(0k)+xk)*ji
[0160]
[0161] 由此，可实现在全局坐标系中对AGV外轮廓及转向状态的描述，实现自主导航定 位。
[0162] 实施例2
[0163] 本实施例提供的直角弯下Mecanum轮式AGV导航定位方法，具体步骤如下：
[0164] 第一步:基于激光雷达线段特征建立对直角弯和AGV初始状态描述；
[01化]如图3所示，在直角弯场景下，在激光雷达坐标系o"x"y"中，经过线段特征提取后 将传感器数据提取成线段信息化1，12，1^3)。以为直线的参数模型（01，91)，其斜率一截距模型 为化i，bi)。线段的起始、结束坐标为(xii,yii)、（xin，yin)。
[0166] (1)基于激光雷达线段特征建立对直角弯的描述
[0167] 则可求得通道模型：
[016 引
[0169] (2)基于激光雷达线段特征建立对AGV初始状态的描述
[0170] 如图4所示，设AGV质屯、〇/在oxy坐标系中的初始坐标为(XO,yo, 0〇)，贝可求得AGV初 始状态：
[0171] 若qi〉0,则有：
[0172：
[0173:
[0174:
[0175：
[0176：
[0177]第二步:在全局坐标系中基于运动学模型的航位推算
[0178] W某个固定的周期T通过编码器采集*1、*2、*3、*4的值。在* = 1^'时刻，采集的转速 的值为Wi化），W2化），W3化），W4化），Mecanum轮漉子半径为r，则有在c/ /坐标系中AGV质屯、 坐标为：
[0179]
[0180]
[0181]
[0182]
[0183] 其中(xo，yo，0〇)由第一步获得。
[0184] 第S步:全局坐标系中对AGV外轮廓及转向状态的描述
[01化]如图5所示，由第一步可得通道模型*1、*2、6^坐标，46￥初始状态^〇,7〇，0〇)。由第 二步可得在全局坐标系下质屯、坐标为(xk，yk，0k)，所W基于坐标轴之间的转换可得在全局 坐标系中：
[0186] A点的坐标：（_l2*cos(目k)+li*sin(目k)+xk, l2*sin(目k)+li*cos(目k)+yk,白k)
[0187] B点的坐标：（l2*cos(目k)+li*sin(目k)+xk,-l2*sin(目k)+li*cos(目k)+yk,白k)
[018引 C点的坐标：（l2*cos(目k)-li*sin(目k)+xk,-l2*sin(目k)-li*cos(目k)+yk,白k)
[0189] D点的坐标：（_l2*cos(目k)-li*sin(目k)+xk, l2*sin(目k)-li*cos(目k)+yk,白k)
[0190] 进一步可得，AGV外轮廓顶点A与L2轴的距离弯道拐点F距离轮廓BC的距离 diSBC-F，D点距离L3轴4'?-与
[0191 ] ji = 2h*cos(目k)，j2 = -2h*sin(目k)，
[0192] J3 = -J2*(l2*sin(目k)-li*cos (目k)+yk)-(l2*cos (目k)-li*sin (目k)+xk)*Ji
[0193]
[0194] 最后说明的是，W上实施例仅用W说明本发明的技术方案而非限制，尽管参照较 佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可W对本发明的技 术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明 的权利要求范围当中。
【主权项】
1. 一种直角弯下Mecanum轮式AGV导航定位方法，其特征在于:包括W下步骤： Sl:建立全局坐标系oxy、初始状态坐标系〇/ /和激光雷达坐标系o"x"y";根据激光 雷达线段特征在全局坐标系下中建立对直角弯和AGV初始状态描述模型； S2:通过AGV内部的编码器获取AGV转速数据并基于Mecanum轮式AGV运动学模型实现航 位推算算法计算在初始状态坐标系〇//中AGV质屯、的坐标k，/k，0/0 ;进而计算在全 局坐标系〇巧中AGV质屯、坐标和姿态(xk,yk,目k); S3:计算在全局坐标系中AGV外轮廓及转向状态描述。2. 如权利要求1所述的直角弯下Mecanum轮式AGV导航定位方法，其特征在于:所述直角 弯和AGV初始状态描述模型的建立具体步骤如下： Sl 1:在激光雷达坐标系o"x"y"中，将激光雷达距离数据(di，Cb . . . dm)通过线段特征提 取，提取出线段化1，L2，L3); (dl，d2. . .dm)为激光雷达原始距离数据;Li表示为激光雷达在直角弯下距离图像的线段 特征，Li的描述为直线的参数模型（01，P1)，01代表过原点做直线的垂线，垂线与X"轴的夹 角，Pi为原点的直线的距离； 1^1 ,L2,L3代表在直角弯下的S条线段，（目l,pi)、（目2 ,P2)、（目3 ,P3)分别代表其对应线段的 参数模型； S12:在全局坐标系oxy中，按照W下公式建立直角弯的描述模型：其中，F点的坐标为(Wi，-W2)，E点的坐标为(0，0)，Wi、W2为通道的宽度，*1为1^1，L2之间的 距离，W2为巧化2之间的距离瓜表示线段L2的斜率;b康示线段L2的截距；（X12，yi2)表示线段 ^的结束点；直角弯的描述模型F点的坐标为I'E点的坐标为（0， 0); E、F点表示在全局坐标下直角弯的拐点的坐标； S13:在全局坐标系O巧中，分别按照W下步骤来实现在全局坐标下中对AGV初始状态描 述； 若目s〉0,即AGV朝右，则有：其中，AGV的轴线与^线段的延长线相交于H，夹角为0s，（xo,yo, 0〇)表示AGV质屯、〇/在oxy 坐标系中的初始坐标； (目i，Pi)表不直线以的参数模型（目i，Pi)，在直角弯下有1^1，L2，L3S条线段，（目i,pi)、（目2， P2)、（ 03，P3)分别对应其参数模型； h表示AGV长度的一半。3.如权利要求1所述的直角弯下Mecanum轮式AGV导航定位方法，其特征在于:所述在全 局坐标系中AGV质屯、的坐标W及姿态具体步骤如下： S21:按照W下公式建立Mecanum轮式AGV的运动学模型；其中，r为Mecanum轮漉子半径，机,'\￥2，*3，*4为46￥四轮的转速;*1、'\￥2、*3、*4为按照固定周 期T通过编码器的采集值； (Vx, Vy, W)表示在初始位置坐标系中，AGV质屯、的速度，分别对应X方向、y方向，W代表姿 态角； S22:根据航位推算原理计算在t = kT时刻，计算在初始状态坐标系〇/中AGV质屯、的 坐标(x'k，y'k，0'k)的递推公式为：其中，（X' k，y' k，目'k)表示在初始状态坐标系0' X' y'中AGV质屯、的坐标； (A Xi, Ayi, A 0i)表示在第i个周期内，AGV质屯、的偏移量； S23:根据全局坐标系oxy与质屯、坐标系c/ /转换关系式计算在全局坐标系oxy中的 质屯、坐标(xk，yk,目k):其中，（xo，y日，目日)为在全局坐标系0巧下质屯、的初始坐标； (xk，yk,目k)为全局坐标系0巧下AGV质屯、的坐标。4. 如权利要求1所述的直角弯下Mecanum轮式AGV导航定位方法，其特征在于:所述在全 局坐标系中AGV外轮廓及转向状态描述的具体步骤如下： S31:根据W下旋转关系计算AGV外轮廓ABCD在全局坐标系下的坐标： A点的坐标：（-l2*cos(目k)+li*sin(目k)+xk, l2*sin(目k)+li*cos(目k)+yk,白k) B点的坐标：（l2*cos(目k)+li*sin(目k)+xk，-l2*sin(目k)+li*cos(目k)+yk,白k) C点的坐标：（l2*cos(目k)-li*sin(目k)+xk，-l2*sin(目k)-li*cos(目k)+yk,白k) D点的坐标：（-l2*cos(目k)-li*sin(目k)+xk, l2*sin(目k)-li*cos(目k)+yk,目k); 其中，ABCD为AGV外轮廓坐标；（a,yk，目k)为全局坐标系下质屯、坐标； S32:按照W下公式计算在全局坐标系中AGV转向状态：Ji = 2h*cos(目k)，j2 = -2h*sin(目k); J3 = -J2*(l2*sin (目 k)-li*cos (目 k)+yk)-(l2*cos (目 k)-li*sin (目 k)+xk)*Ji; 其中，AGV在转向过程中发生碰撞的障碍点;夕帷廓顶点A与L2边，侦脚BC与拐点F，D点距 离边L3; AGV外轮廓顶点A与L2轴的距离为^?j-V; 弯道拐点F距离轮廓BC的距离为diSBC-F; D点距离L3轴为《S。-4。5. -种直角弯下Mecanum轮式AGV导航定位系统，其特征在于:包括坐标系建立模块、通 道模型建立模块、AGV初始状态描述模型建立模块、数据采集模块、全局质屯、坐标计算模块、 全局外轮廓计算模块和全局转向状态描述计算模块； 所述坐标系建立模块，用于建立全局坐标系oxy、初始状态坐标系〇/ /和激光雷达坐 标系 o"x"y"; 所述通道模型建立模块，用于根据激光雷达线段特征在全局坐标系下中建立直角弯模 型； 所述AGV初始状态描述模型建立模块，用于根据激光雷达线段特征在全局坐标系下中 建立AGV初始状态描述模型； 所述数据采集模块，用于通过AGV内部的编码器获取AGV四个轮子的转速； 所述全局质屯、坐标计算模块，用于基于Mecanum轮式AGV运动学模型实现航位推算算法 计算在全局坐标系oxy中AGV质屯、的坐标(xk，yk，9k); 所述全局外轮廓计算模块，用于计算在全局坐标系中AGV外轮廓； 所述全局转向状态描述计算模块，用于计算在全局坐标系中的转向状态描述。6. 如权利要求5所述的直角弯下Mecanum轮式AGV导航定位系统，其特征在于:所述直角 弯和AGV初始状态描述模型的建立具体步骤如下： Sl 1:在激光雷达坐标系o"x"y"中，将激光雷达距离数据(di，Cb . . . dm)通过线段特征提 取，提取出线段化1，L2，L3); 其中，Li表示为激光雷达在直角弯下距离图像的线段特征，Li的描述为直线的参数模型 (01，口1)，01代表过原点做直线的垂线，垂线与义"轴的夹角，口1为原点的直线的距离，（01，口1)、 (目2，P2 )、（目3，P3 )分别为b，L2，L3的参数模型； S12:在全局坐标系oxy中，按照W下公式建立直角弯的描述模型：其中，F点的坐标为(Wl，-W2 )，E点的坐标为(0，0 )，Wl、W2为通道的宽度，Wl为b，L2之间的 距离，W2为巧化2之间的距离瓜表示线段L2的斜率;b康示线段L2的截距；（X12，yi2 )表示线段 b的结束点； 直角弯的描述模型F点的坐标戈'E点的坐标为(0,0); S13:在全局坐标系0巧中，分别按照W下步骤来实现在全局坐标下中对直角弯和AGV初 始状态描述； 若目s〉0,即AGV朝右，则有：其中，AGV的轴线与^线段的延长线相交于H，夹角为0S，（xo,yo, 0〇)表示AGV质屯、〇/在oxy 坐标系中的初始坐标； (目i,Pi)表不直线以的参数模型（目i,Pi); h表示AGV长度的一半。7.如权利要求5所述的直角弯下Mecanum轮式AGV导航定位系统，其特征在于:所述在全 局坐标系中AGV质屯、的坐标W及姿态具体步骤如下： S21:按照W下公式建立Mecanum轮式AGV的运动学模型；其中，r为Mecanum轮漉子半径，机,'\￥2，*3，*4为46￥四轮的转速;*1、'\￥2、*3、*4为按照固定周 期T通过编码器的采集值； (VxVy,，）w表示在初始位置坐标系中，AGV质屯、的速度； S22:根据航位推算原理计算在t = kT时刻，计算在初始状态坐标系〇/中AGV质屯、的 坐标(厶，7'1<，0'〇的递推公式为：其中，（X' k，y' k，目'k)表示在初始状态坐标系0' X' y'中AGV质屯、的坐标； (A Xi, Ayi, A 0i)表示在第i个周期内，AGV质屯、的偏移量； S23:根据全局坐标系oxy与质屯、坐标系c/ /转换关系式计算在全局坐标系oxy中的 质屯、坐标(xk，yk,目k):其中，（xo,y日,目日)为在全局坐标系oxy下质屯、的初始坐标； (Xk，yk,目k)为全局坐标系0巧下AGV质屯、的坐标。8.如权利要求5所述的直角弯下Mecanum轮式AGV导航定位系统，其特征在于:所述在全 局坐标系中AGV外轮廓及转向状态描述的具体步骤如下： S31:根据W下旋转关系计算AGV外轮廓ABCD在全局坐标系下的坐标： A点的坐标：（_l2*cos(目k)+li*sin(目k)+xk, l2*sin(目k)+li*cos(目k)+yk,白k) B点的坐标：（l2*cos(目k)+li*sin(目k)+xk,-l2*sin(目k)+li*cos(目k)+yk,白k) C点的坐标：（l2*cos(目k)-li*sin(目k)+xk,-l2*sin(目k)-li*cos(目k)+yk,白k) D点的坐标：（_l2*cos(目k)-li*sin(目k)+xk, l2*sin(目k)-li*cos(目k)+yk,目k); 其中，ABCD为AGV外轮廓坐标；（a,yk，目k)为全局坐标系下质屯、坐标； S32:按照W下公式计算在全局坐标系中AGV转向状态：Ji = 2h*cos(目k)，j2 = -2h*sin(目k); j3 = -j2*(l2*sin(0k)-li*cos(0k)+yk)-(l2*cos(0k)-li*sin(0k)+xk)*ji; 其中，AGV在转向过程中发生碰撞的障碍点;外轮廓顶点A与L2边，侦脚BC与拐点F，D点距 离边L3; AGV外轮廓顶点A与L2轴的距离为心S； 弯道拐点F距离轮廓BC的距离为diSBC-F; 〇点距离13轴为^*'如--王,。
【文档编号】G01C21/20GK105987697SQ201610266652
【公开日】2016年10月5日
【申请日】2016年4月26日
【发明人】赵敏, 孙棣华, 熊星
【申请人】重庆大学