全方位移动机器人狭窄直角弯下转向运动规划方法及系统的制作方法
【专利摘要】本发明公开了全方位移动机器人狭窄直角弯下转向运动规划方法，首先初始化机器人参数；实时获得机器人在全局坐标系中的质心坐标和约束量；根据机器人的质心坐标和约束量来判断机器人当前所属转向状态；然后根据转向状态来确定控制策略和计算控制指令；判断控制量是否超出控制量的上下限，若超过，则控制指令等于其最大或最小值；最后向机器人发送控制指令，驱动机器人运行；本发明提供的运动规划方法，通过充分分析狭窄直角弯约束受限特性的情况下，结合小车全方位移动以及指令控制这种特殊的方式，采用基于规则式的狭窄直角弯转向运动规划方法。此方法为工程上实现全方位机器人在狭窄直角弯下的自主转向提供了一种解决方案，简单易行、具有良好的适应性，能较好的解决机器人在狭窄限制下的转向运动规划问题。
【专利说明】
全方位移动机器人狭窄直角弯下转向运动规划方法及系统
技术领域
[0001] 本发明涉及轮式移动机器人的运动规划领域，特别涉及一种全方位移动机器人在 狭窄直角弯下转向运动规划方法。
【背景技术】
[0002] 全方位移动机器人在平面空间上具有前后，左右，自旋三个自由度，可同时、独立 地进行前后、左右和原地旋转三个维度上的运动，在不改变自身位姿的情况下向任意方向 移动。由于该机器人可以结合多种移动方式，特别是在空间需求较大的转向场景下，灵活性 更强。目前在应用现场比较典型的就是狭窄直角弯。
[0003] 全方位移动机器人在操控方式上分三个维度上的指令式控制，ahead指令代表沿 着轴线方向前后行进，数值为正代表前进，负代表后退。move指令代表沿着左右横移，数值 为正代表左移，负代表右移。turn指令代表原地旋转，数值为正代表左旋，负代表右旋。指令 数值的大小代表各个方向上的速度。机器人最后行进的一个轨迹是三个维度移动方式的一 个叠加。
[0004] 如何利用全方位移动机器人全方位移动的特性在狭窄直角弯受限的情况自主规 划完成转向，是移动机器人自主导航的关键。通过查阅论文与专利，在运动规划上常见的路 径规划方法有人工势场法、Dubins路径规划方法、遗传算法、PRM法等。
[0005]此类方法规划出的轨线往往是基于全局出发，主要侧重于算法的和鲁棒性。并不 考虑对象的轮廓以及运动特性，无法保证路径的真实可行性。而狭窄直角弯属于局部的场 景，狭窄是最典型的特点，即空间受限较大，将小车抽象成质点不能反映出轮廓与环境的信 息，所以并不合适。而且机器人具备全方位移动功能，在转向过程中可以采用原地旋转、侧 移等多种组合策略，而传统的路径规划并不考虑其运动特性，无法体现全方位移动的优势。 所以常规的运动规划方法并不能解决在狭窄直角弯限制下全方位移动机器人的转向规划 问题。
[0006] 因此，急需一种当空间受到限制的情况，切实可行、结合全方位移动特性的转向运 动规划方法。
【发明内容】
[0007] 本发明的目的就是提供一种当空间受到限制的情况，切实可行、结合全方位移动 特性的转向运动规划方法。该运动规划方法，适用于狭窄直角弯限制下全方位移动机器人 根据环境信息自主规划完成转向。
[0008] 本发明的目的是通过这样的技术方案实现的：
[0009] 本发明提供的全方位移动机器人狭窄直角弯下转向运动规划方法，包括以下步 骤：
[0010]第一步:参数初始化；
[0011]第二步:实时获得机器人在全局坐标系中的质心坐标和约束量；
[0012] 第三步:根据机器人的质心坐标和约束量来判断机器人当前所属转向状态；
[0013] 第四步:根据转向状态来确定控制策略和计算控制指令；
[0014] 第五步:根据机器人控制指令范围，判断控制指令是否超出上下限，若超过，则控 制指令等于其最大或最小值；
[0015] 第六步：向机器人发送控制指令，驱动机器人运行；
[0016] 第七步:返回到第二步。
[0017] 进一步，所述参数包括机器人的规格hXl2、通道的宽度W1XW2、待整定的参数cU， d2，ki，k2，k3，k4，k5，k6，k7，k8、角度补偿值 ang_theta 和距离补偿值 dis_theta;
[0018]其中，ll、l2表不机器人的规格，li为机器人长度的一半，12为机器人宽度的一半； W1、W2表示直角弯场景的宽度;dl表示机器人从状态1切换到状态2的阈值;d2表示机器人从 状态2切换到状态3的阈值;ki，k 2，k3，k4，k5，k6，k7，k 8表示在各个状态下控制指令计算公式中 待整定的参数。
[0019] 进一步，所述约束量包括#4-i2、disB〇F和.?-& ;其中Xij表示机器人外轮廓顶 点A与X轴的距离;diSBC-F表示弯道拐点F距离轮廓BC的距离表示机器人外轮廓顶点D 点距离y轴的距离。
[0020] 进一步，所述转向状态包括第一状态、第二状态和第三状态；
[0021] 所述第一状态、第二状态和第三状态按照以下方式来确定：
[0022] ，属于第一状态；
[0023] 若((成<名）am/ (泣~―4 <d2))〇r((成Vi2为真，则属于第 三状态，否则属于第二状态。
[0024] 进一步，所述控制策略和计算控制指令是按照以下步骤来确定的：
[0025] 1:若机器人处于第一状态，则按照以下步骤来计算控制指令：
[0027] 其中，ahead表示行进方向控制指令;turn表示旋转控制指令;move表示左右横移 控制指令；
[0028] 返回到第三步；
[0029] 2:若机器人处于第二状态，则按照以下步骤来计算比例量、距离偏差、角度偏差：
[0031]其中，ratio表示比例量，反应机器人外轮廓A、D距离直角弯距离的比例；
[0032] dis_deviation 表不距离偏差；
[0033] ang_deviation 表示角度偏差；
[0034]计算角度补偿量，距离补偿量；
[0035]若(也Vi2 < 4)和（θ>θ2)为真，或者，则turnoffset = ang_ theta；
[0036]若〇^d-& < iiJancKQSQi)，贝lJaheadoffset = dis_theta;
[0037] 其中，turnoffset表示角度补偿量；
[0038] ang_theta表示角度补偿值；
[0039] aheadoffset表不距离补偿量；
[0040] dis_theta表示距离补偿值；
[0041]按照以下公式计算控制指令：
[0043] 返回到第三步；
[0044] 3:若机器人处于第三状态，则判断diSBC-F是否等于d 2,若等于则重新回到第三步， 重新判断状态；
[0045]若不等于d2,则按照以下公式计算控制指令：
[0047]返回到第四步，机器人继续横移。
[0048] 本发明还提供了全方位移动机器人狭窄直角弯下转向运动规划系统，包括初始模 块、实时参数采集模块、转向状态判断模块、控制命令生成模块、控制量判断模块和控制指 令发送模块；
[0049] 所述初始模块，用于对参数进行初始化；
[0050] 所述实时参数采集模块，用于实时获得机器人在全局坐标系中的质心坐标和约束 量；
[0051] 所述转向状态判断模块，用于根据机器人的质心坐标和约束量来判断机器人当前 所属转向状态；
[0052] 所述控制命令生成模块，用于根据转向状态来确定控制策略和计算控制指令； [0053]所述控制量判断模块，用于判断控制量是否超出控制量的上下限，若超过，则控制 指令等于其最大或最小值；
[0054] 所述控制指令发送模块，用于向机器人发送控制指令，驱动机器人运行。
[0055] 进一步，所述转向状态判断模块中的转向状态包括第一状态、第二状态和第三状 态；
[0056]所述第一状态、第二状态和第三状态按照以下方式来划分：
[0057] 若冰2 & < <，属于第一状态；
[0058] 若((沿·^-& <4)嫌i (也!)-&i2为真，则属于第 三状态，否则属于第二状态。
[0059] 进一步，所述控制命令生成模块中的控制策略和计算控制指令是按照以下步骤来 确定的：
[0060] 1:若机器人处于第一状态，则按照以下步骤来计算控制指令：
[0062] 其中，ahead表示行进方向控制指令;turn表示旋转控制指令;move表示左右横移 控制指令；
[0063] 返回到第三步；
[0064] 2:若机器人处于第二状态，则按照以下步骤来计算比例量、距离偏差、角度偏差：
[0066]其中，ratio表示比例量，反应机器人外轮廓A、D距离直角弯距离的比例；
[0067] dis_deviation 表不距离偏差；
[0068] ang_deviation 表示角度偏差；
[0069]计算角度补偿量，距离补偿量；
[0070] 若(也Vi2 < 4)和（θ>θ2)为真，或者(成，则turnoffset = ang_ theta；
[0071] 若，贝lJaheadoffset = dis_theta;
[0072] 其中，turnoffset表示角度补偿量；
[0073] ang_theta表示角度补偿值；
[0074] aheadoffset表不距离补偿量；
[0075] dis_theta表示距离补偿值；
[0076]按照以下公式计算控制指令：
[0078] 返回到第三步；
[0079] 3:若机器人处于第三状态，则判断diSBC-F是否等于d 2,若等于则重新回到第三步， 重新判断状态；
[0080]若不等于d2，则按照以下公式计算控制指令：
[0082]返回到第四步，机器人继续横移。
[0083]由于采用了上述技术方案，本发明具有如下的优点：
[0084]本发明提供的全方位移动机器人在狭窄直角弯下转向运动规划方法，首先通过充 分分析狭窄直角弯约束受限特性的情况下，提出了描述机器人转向受限的约束量。然后考 虑机器人全方位移动的特性，根据约束量，将机器人转向的过程划分为3个状态。最后，结合 机器人全方位移动以及指令控制这种特殊的方式，针对每个状态提出了基于规则式的规划 及控制策略。此方法为工程上实现全方位机器人在狭窄直角弯下的自主转向提供了一种解 决方案，简单易行、具有良好的适应性，能较好的解决机器人在狭窄限制下的转向运动规划 问题。
[0085]本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并 且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可 以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书和权利要 求书来实现和获得。
【附图说明】
[0086]本发明的【附图说明】如下。
[0087]图1为本发明的轮式移动机器人轮廓图。
[0088] 图2为本发明的狭窄直角弯场景图。
[0089] 图3为本发明的狭窄直角弯坐标系的建立。
[0090] 图4为本发明的机器人初始时刻。
[0091] 图5为本发明的状态1结束条件。
[0092]图6为本发明的状态2结束条件。
[0093]图7为本发明的状态3结束条件。
[0094]图8为本发明的全方位移动机器人狭窄直角弯下转向运动规划方法流程图。
【具体实施方式】
[0095]下面结合附图和实施例对本发明作进一步说明。
[0096] 实施例1
[0097] 如图所示，图1为轮式移动机器人轮廓图，轮式移动机器人轮廓，长为，宽为212， 单位cm〇
[0098] 图2为狭窄直角弯场景图，狭窄直角弯场景宽为奶Xw2，E、F为两个拐点，机器人轮 廓为AB⑶，弯道边为Li、L2、L3，单位cm。
[0099]图3为狭窄直角弯坐标系的建立，在狭窄直角弯下以拐点E为坐标原点，L2*x轴建 立oxy坐标系。机器人外轮廓顶点A与X轴的距离为，弯道拐点F距离轮廓BC的距离为 disbc-f，D点距离y轴为，单位为cm。质心Μ在坐标系中的位置为(x，y，0)，其中Θ代表机 器人的轴线与y轴的夹角，代表姿态角。Θ为正，代表机器人朝右，反之，向左，单位为度。 [0?00]图4为机器人初始时刻，机器人初始时刻的质心位置坐标为(XQ，yo, θ〇)，θ〇为姿态。
[0101] 图5为状态1结束条件，该时刻为状态1进入状态2的临界时刻，当F与ΑΒ的距离大于 di时，即进入状态2。
[0102] 图6为状态2结束条件，该时刻为状态1进入状态2的临界时刻，当机器人前方或者 后方受限时即进入状态3。
[0103] 图7为状态3结束条件，当F与BC距离小于山时，机器人结束状态3,重新进入状态2。
[0104] 本实施例提供的全方位移动机器人狭窄直角弯下转向运动规划方法，包括以下步 骤：
[0105] 第一步:参数初始化；
[0106] 第二步:实时获得机器人在全局坐标系中的质心坐标和约束量；
[0107] 第三步:根据机器人的质心坐标和约束量来判断机器人当前所属转向状态；
[0108] 第四步:根据转向状态来确定控制策略和计算控制指令；
[0109] 第五步:根据机器人控制指令范围，判断控制指令是否超出上下限，若超过，则控 制指令等于其最大或最小值；
[0110] 第六步：向机器人发送控制指令，驱动机器人运行；
[0111] 第七步:返回到第二步。
[0112] 所述参数包括机器人的规格li X 12，通道的宽度W1 X W2，待整定的参数cb，d2，lu，k2， k3，k4，k5，k6，k7，k8,角度补偿值ang_theta，距离补偿值dis_theta，Qparctandi/h)，θ2 = arctan(I2/I1)；
[0113] li、I2表不机器人的规格，li为机器人长度的一半，I2为机器人宽度的一半；
[0114] W1、W2表示直角弯场景的宽度分别为W1、W2;
[0115] dl表示机器人从状态1切换到状态2的阈值；
[0116] d2表示机器人从状态2切换到状态3的阈值；
[0117] 1^，1?，1?，1^4，1?，1?山，1?表示在各个状态下控制指令计算公式中待整定的参数，参 数的整定需综合机器人具体特性、直角弯场景等因素；
[0118] 所述约束量包括i2、disBC-F和i3 ;
[0119] 表示机器人外轮廓顶点A与x轴的距离；
[0120] disBC-F表示弯道拐点F距离轮廓BC的距离；
[0121] 表示机器人外轮廓顶点D点距离y轴的距离。
[0122] 所述转向状态包括第一状态、第二状态和第三状态；
[0123] 所述第一状态、第二状态和第三状态按照以下方式来划分：
[0124] 若w2 & <名，属于状态1;
[0125] 若& <4) ?m/ 4 <名))〇r((^^-i2为真，则属于状 态3,否则属于状态2;
[0126] 所述控制策略和计算控制指令是按照以下步骤来确定的：
[0127] 1:若机器人处于第一状态，则按照以下步骤来计算控制指令：
[0129] 其中，ahead表示行进方向控制指令;turn表示旋转控制指令;move表示左右横移 控制指令；
[0130] 返回到第三步；
[0131] 2:若机器人处于第二状态，则按照以下步骤来计算比例量、距离偏差、角度偏差：
[0133] 其中，ratio表示比例量，反应机器人外轮廓A、D距离直角弯距离的比例；
[0134] dis_deviation 表不距离偏差；
[0135] ang_deviation 表示角度偏差；
[0136] 计算角度补偿量，距离补偿量；
[0137] 若(沿·^-i2 < 4)和（θ>02)为真，或者(而￡)-& < OancKQSQi)，则turnoffset = ang_ theta；
[0138] 若(d^D-& < iiJancKQSQi)，贝lJaheadoffset = dis_theta;
[0139] 其中，turnoffset表示角度补偿量；
[OHO] ang_theta表示角度补偿值；
[0141] aheadoffset表不距离补偿量；
[0142] dis_theta表示距离补偿值；
[0143] 按照以下公式计算控制指令：
[0145] 返回到第三步；
[0146] 3:若机器人处于第三状态，则判断diSBC-F是否等于d 2,若等于则重新回到第三步， 重新判断状态；
[0147] 若不等于d2,则按照以下公式计算控制指令：
[0149] 返回到第四步，机器人继续横移。
[0150] 本实施例提供的全方位移动机器人狭窄直角弯下转向运动规划系统，包括初始模 块、实时参数采集模块、转向状态判断模块、控制命令生成模块、控制量判断模块和控制指 令发送模块；
[0151 ]所述初始模块，用于对参数进行初始化；
[0152] 所述实时参数采集模块，用于实时获得机器人在全局坐标系中的质心坐标和约束 量；
[0153] 转向状态判断模块，用于根据机器人的质心坐标和约束量来判断机器人当前所属 转向状态；
[0154] 所述控制命令生成模块，用于根据转向状态来确定控制策略和计算控制指令；
[0155] 所述控制量判断模块，用于判断控制量是否超出控制量的上下限，若超过，则控制 指令等于其最大或最小值；
[0156] 所述控制指令发送模块，用于向机器人发送控制指令，驱动机器人运行。
[0157] 实施例2
[0158] 如图1所示，轮式移动机器人轮廓为长方形，长为2h，宽为212，单位cm，其质心为Μ 点。如图2所示，狭窄直角弯场景宽*W1Xw2，E、F为两个拐点，机器人轮廓为ABCD，弯道边为 Li、L2、L3。以拐点E为坐标原点，L2为X轴建立oxy坐标系。如图3所示，机器人外轮廓顶点A与X 轴的距离为，弯道拐点F距离轮廓BC的距离为disB〇F，D点距离y轴为，单位为cm。 质心Μ在坐标系中的位置为(x，y，0)，其中Θ代表机器人的轴线与 y轴的夹角，代表姿态角。Θ 为正，代表机器人朝右，反之，向左，单位为度。
[0159] 对狭窄直角弯受限的定义是不能通过原地旋转的方式完成转向叫做空间受限，即 W1、W2满足式1 · 1 〇
[0161 ]设d为防碰撞阈量，机器人的初始状态描述为(xo，yo, θ〇)，转向的过程是一个Θ从θ〇 调整到90°的一个过程。在转向过程中，存在如下约束：
[0163] 具体的
【发明内容】
如下：
[0164] (1):状态的划分
[0165] 整个转向过程划分为3个状态，机器人初始时刻的位置如图4所示，在t时刻，小车 的位置如图5所示，设其在全局坐标系中的坐标为(x，y，0)，此亥ijF点距离AB的距离设为山油 为阈值。这个过程称为转向调整阶段，主要的目的就是在小车进入转向区域之前调整好自 身的位置和姿态，以最好的状态进入转向区域，称为状态1。其判断条件是>必， false为状态1，true为状态2〇
[0166] 状态2称为转向阶段，通过前进结合右旋的策略完成姿态的调整。在状态2中定义 0i = arctan(li/l2)，92 = arctan(l2/li)，定义防碰撞阈值d2。设在t时刻，如图6所示，机器人 前方，后方受限，则进入状态3，其判断条件为式1.3，false为状态2，true为状态3。
[0167] ((disA_h < d2) and (dis^ < d2))or ((disA_Li < d2) and {θ<θγ)) (1.3)
[0168] 状态3称为平移阶段，利用全方位移动的优势采用向右平移的策略，直至disBC-F< d2，如图7所示。此时若Θ>90，表示机器人完成转向，否则重新进入状态2。
[0169] ⑵：控制策略
[0170]状态1目的就是在小车进入转向区域之前调整好自身的位置和姿态，以最好的状 态进入转向区域。其控制策略采用前进结合旋转的方式，定义阈值ki、k2、k3。控制量见式 1.4：
[0172]状态2目的是小车在进入了转向区域后，在有限的空间下，最大限度的完成角度的 调整。其控制策略采用前进结合右旋的方式。在状态2中，定义比例量+ ， 距离偏差量&_-尖，定义角度偏差量
[0173] ang_deviation= (90_9)*：11/180，定义阈值参数1?、1^4、1?、1?、1^7、1?，定义角度补偿 量turnoffset，定义距离补偿量aheadoffset，定义角度补偿值ang_theta，定义距离补偿值 dis_theta存在下式：
[0177] 状态3称为平移阶段，利用全方位移动的优势采用向右平移的策略，定义阈值参数 k8,则控制量见式1.7
[0178] ahead = 0, turn = 0, mo ve = -ks (di SBC-F~d2) (1.7)
[0179] 针对控制量，在发送控制指令之前，判断控制量是否超出控制量的上下限，若超过 了，控制指令就等于其最大或最小值。
[0181] 实施例3
[0182 ]本实施例提供的全方位移动机器人在狭窄直角弯下转向运动规划方法，具体实施 如下：
[0183] 第一步:参数初始化，初始化的参数有机器人的规格h X 12，通道的宽度W1 X W2，待 整定的参数 di，d2，ki，k2，k3，k4，ks，k6，k7，ks，角度补偿值 ang_theta，距离补偿值 di s_theta， 9i = arctan(li/l2)，92 = arctan(l2/li) 〇
[0184] 第二步:判断机器人当前所属转向状态。从导航模块中实时获得机器人在全局坐 标系中的质心坐标(x，y，θ)，以及约束量。
[0185] 若姿态角θ>90,则认为已完成转向，直接结束。若<<，属于状态丨，若 <??2) & <?/2))ΟΓ((?'·νΖ2为真，贝 1J属于状态3,否贝 1J属 于状态2。
[0186] 第三步:确定控制策略和计算控制指令。
[0187] 1:若机器人处于状态1，则有
[0189] 回到第二步。
[0190] 2:若机器人处于状态2,先计算比例量、距离偏差、角度偏差。
[0192] 然后计算角度补偿量，距离补偿量，若名）和（θ>θ2)为真，或者 (t^D-i3<<^2)and(9>01)Uturnoffset = ang_theta<3S(<^D-i3<"2)and(9<0 1WlJ aheadoffset = dis-theta 〇
[0193] 最后计算控制量
[0195] 回到第二步。
[0196] 3:若机器人处于状态3,则判断diSBC-F是否等于山，若等于则重新回到第二步。若不 等于d2,则计算控制指令，
[0198] 然后又重新回到第三步。
[0199] 4:在发送控制指令之前，判断控制量是否超出控制量的上下限，若超过了，控制指 令就等于其最大或最小值。
[0200] 本实施例提供的具体实例，针对规格为44乂116〇11的31^300型号16〇&1111111轮式全方 位移动机器人。其控制指令范围为ahead(-40,40)，turn(-16,16)，move(-40,40)，狭窄直角 弯场景为80 X 80cm狭窄直角弯，其具体参数为^ = 58,12 = 22,(^ = 15,(12 = 10^ = 1,1^=1, k3 = 0 · 5，k4 = 0 · 5，k5 = 1，k6 = 2，k7 = 3，k8 = 0 · 3，ang_theta = dis_theta = 5，能保证良好的 通过性。
[0201]最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较 佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技 术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明 的权利要求范围当中。
【主权项】
1. 全方位移动机器人狭窄直角弯下转向运动规划方法，其特征在于:包括以下步骤： 第一步:参数初始化； 第二步:实时获得机器人在全局坐标系中的质心坐标和约束量； 第三步:根据机器人的质心坐标和约束量来判断机器人当前所属转向状态； 第四步:根据转向状态来确定控制策略和计算控制指令； 第五步:根据机器人控制指令范围，判断控制指令是否超出上下限，若超过，则控制指 令等于其最大或最小值； 第六步：向机器人发送控制指令，驱动机器人运行； 第七步:返回到第二步。2. 如权利要求1所述的全方位移动机器人狭窄直角弯下转向运动规划方法，其特征在 于:所述转向状态包括第一状态、第二状态和第三状态； 所述第一状态、第二状态和第三状态按照以下方式来确定： 若u'2 - ,、<<，属于第一状态； 若名）(边％-毛)）w* ((?·?士爾4 (没<幻)为真 J 三状态，否则属于第二状态。3. 如权利要求2所述的全方位移动机器人狭窄直角弯下转向运动规划方法，其特征在 于:所述参数包括机器人的规格liXh、通道的宽度 W1XW2、待整定的参数 k4，k5，k6，k7，ks、角度补偿值 ang_theta 和距离补偿值 di s_theta; 其中，ll、l2表不机器人的规格，ll为机器人长度的一半，12为机器人宽度的一半;W1、W2 表不直角弯场景的宽度;dl表不机器人从第一状态切换到第二状态的阈值;d2表不机器人 从第二状态切换到第三状态的阈值;匕，1?，1?，1^少 5，1?，1?，1?表示在各个状态下控制指令计 算公式中待整定的参数。4. 如权利要求1所述的全方位移动机器人狭窄直角弯下转向运动规划方法，其特征在 于:所述约束量包括AV-z:、disB〇F和;其中，?』-^表不机器人外轮廓顶点A与X轴的 距离;disBC-F表示弯道拐点F距离轮廓BC的距离；表示机器人外轮廓顶点D点距离y轴 的距离。5. 如权利要求2所述的全方位移动机器人狭窄直角弯下转向运动规划方法，其特征在 于:所述控制策略和计算控制指令是按照以下步骤来确定的： 1:若机器人处于第一状态，则按照以下步骤来计算控制指令：其中，ahead表示行进方向控制指令;turn表示旋转控制指令;move表示左右横移控制 指令； 返回到第三步； 2:若机器人处于第二状态，则按照以下步骤来计算比例量、距离偏差、角度偏差：其中，ratio表示比例量，反应机器人外轮廓A、D距离直角弯距离的比例； dis_deviation表示距离偏差； ang_deviation表示角度偏差； 计算角度补偿量，距离补偿量； 若（disA-L2<d2)和（θ>02)为真，或者（disD-，则turnoffset = ang_ theta； 若（di sd-L3 <cb)and(θ < Θ丄），则aheadoffset = di s_theta; 其中，turnoff set表示角度补偿量； ang_theta表示角度补偿值； aheadoffset表示距离补偿量； dis_theta表示距离补偿值； 按照以下公式计算控制指令：返回到第三步； 3:若机器人处于第三状态，则判断diSBC-F是否等于办，若等于则重新回到第三步，重新 判断状态； 若不等于d2,则按照以下公式计算控制指令：返回到第四步，机器人继续横移。6. 全方位移动机器人狭窄直角弯下转向运动规划系统，其特征在于:包括初始模块、实 时参数采集模块、转向状态判断模块、控制命令生成模块、控制量判断模块和控制指令发送 模块； 所述初始模块，用于对参数进行初始化； 所述实时参数采集模块，用于实时获得机器人在全局坐标系中的质心坐标和约束量； 所述转向状态判断模块，用于根据机器人的质心坐标和约束量来判断机器人当前所属 转向状态； 所述控制命令生成模块，用于根据转向状态来确定控制策略和计算控制指令； 所述控制量判断模块，用于判断控制量是否超出控制量的上下限，若超过，则控制指令 等于其最大或最小值； 所述控制指令发送模块，用于向机器人发送控制指令，驱动机器人运行。7. 如权利要求6所述的全方位移动机器人狭窄直角弯下转向运动规划系统，其特征在 于:所述转向状态判断模块中的转向状态包括第一状态、第二状态和第三状态； 所述第一状态、第二状态和第三状态按照以下方式来划分： 若 ，属于第一状态； 若((<：//Λ." _ <?/:)舰/ (必狀（("/.V, <4 三状态，否则属于第二状态。8.如权利要求6所述的全方位移动机器人狭窄直角弯下转向运动规划系统，其特征在 于:所述控制命令生成模块中的控制策略和计算控制指令是按照以下步骤来确定的： 1:若机器人处于第一状态，则按照以下步骤来计算控制指令：其中，ahead表示行进方向控制指令;turn表示旋转控制指令;move表示左右横移控制 指令； 返回到第三步； 2:若机器人处于第二状态，则按照以下步骤来计算比例量、距离偏差、角度偏差：其中，ratio表示比例量，反应机器人外轮廓A、D距离直角弯距离的比例； dis_deviation表示距离偏差； ang_deviation表示角度偏差； 计算角度补偿量，距离补偿量； 若<t/2)和（θ>θ2)为真，或者(i^D-& < ?/2)αο<τ/(0 >0)，则 turnoffset = ang_ theta； 若（?7/Λ·,υ Λ <0) Ji」aheadoffset = dis-theta; 其中，turnoff set表示角度补偿量； ang_theta表示角度补偿值； aheadoffset表示距离补偿量； dis_theta表示距离补偿值；按照以下公式计 返回到第三步； 3:若机器人处于第三状态，则判断diSBC-F是否等于办，若等于则重新回到第三步，重新 判断状态； 若不等于d2,则按照以下公式计算控制指令：返回到第四步，机器人继续横移。
【文档编号】G05D1/02GK105867372SQ201610212952
【公开日】2016年8月17日
【申请日】2016年4月7日
【发明人】孙棣华, 刘卫宁, 赵敏, 廖孝勇, 熊星
【申请人】重庆大学