一种基于势能场的机器人路径跟踪算法
【专利摘要】本发明提供本发明的一种基于势能场的机器人路径跟踪算法，包括以下步骤：(1)获得转弯半径；(2)根据转弯半径R和当前线速度Vx(t)，估算下一个时刻线速度Vx(t+1)；(3)根据R和Vx(t+1)计算角速度ω，获得合理的路径跟踪速度(Vx，ω)，随后对获得的当前路径与预设路径两者之间进行拟合获得最优的路径跟踪速度(Vx，ω优)，由此确定最优跟踪路径轨迹。本发明通过对机器人实时虚拟路径的分析计算得到基础速度分解量(Vx，ω)，然后进一步通过势能场中机器人路径预测来得到最优路径，能够极大的提高计算效率，降低对计算机硬件的要求，并且不论系统本身的特性如何变化，或者机器人相对预设路径的初始偏角多大，算法本身都会保证机器人收敛到预设路径上。
【专利说明】
-种基于势能场的机器人路径跟踪算法
技术领域
[0001] 本发明设及一种路径跟踪算法，具体设及一种基于势能场的机器人路径跟踪算 法。
【背景技术】
[0002] 路径跟踪算法是为了让移动机器人运行的轨迹局限在既定路径上可接受的误差 范围之内。
[0003] 目前常用的移动机器人是通过磁条或色条来规划机器人的轨迹，机器人通过磁感 探头或者光电传感器阵列来检测机器人实际运行轨迹和预定轨迹之间的误差。传感器所检 测到的误差作为反馈输入到运动控制单元，当误差为零时，机器人即在预定的轨迹上。运是 线性控制理论的一个自然应用，控制器大多采用PID的方式实现。
[0004] 另一种常见的机器人路径跟踪方法是基于栅格地图计算预设路径到二维平面上 所有点的曼哈顿距离。再根据机器人当前速度，W及机器人自身加速度性能，计算出机器人 所有可能的轨迹。对所有可能的轨迹在二维平面上通过曼哈顿距离加和进行打分，得分最 低者（即偏离预设路径最小)被选为最优轨迹。例如，如果按照某个速度(Vx，CO)，在接下去 的一段时间t内所产生的轨迹是{trji}，且{trji}和预设栅格地图上的路径完全吻合，则得 分为0，即所得轨迹为最优的。
[0005] 磁条式或色条式路径跟踪，其算法是线性控制理论的延伸，原理通俗易懂，控制器 设计简单。但是运种方式需要预先铺设和规划机器人路径，一旦机器人工作路径需要变化， 则需要人工干预，重新铺设路径。运样的方式费时费力，也不灵活。运种方式也只适用于单 向路径的跟踪，对于路径出现交叉等情况，磁条或色条都无法有效的解决，必须要依赖外界 辅助手段来处理，如RFID，QR code等。另外一个重大缺陷是对于PID控制器的调试，由于PID 控制器是对系统控制性能的线性优化，其参数仅在系统特性，如载重、驱动能力、机械损耗 等，不变的情况下有效。一旦系统发生变化，其参数也随之改变，如果控制器的参数不变，贝U 控制性能将会下降，机器人对路径的跟踪能力也会下降。
[0006] 普通的基于栅格地图的路径跟踪算法比较有效的解决了上述问题。尤其是对于工 作路径多变的环境，利用栅格地图预设路径较磁条色条导航有着天然的成本优势。但是运 种方式计算量大且捜索范围无法控制，只能对机器人所有可能达到的速度(Vx，CO )进行捜 索，对于计算机硬件的要求偏高。且一大缺点是，当机器人位置偏角和当前预设路径偏离过 大时，机器人仅通过(Vx，CO )的组合，无法修正偏角，最终会造成机器人脱离预设路径。
【发明内容】
[0007] 为解决上述技术问题，本发明提供了一种基于势能场的机器人路径跟踪算法，能 够极大的提高计算效率，降低对计算机硬件的要求，并且不论系统本身的特性如何变化，或 者机器人相对预设路径的初始偏角多大，算法本身都会保证机器人收敛到预设路径上。
[000引为达到上述目的，本发明的技术方案如下：一种基于势能场的机器人路径跟踪算 法，其特征在于，包括W下步骤：
[0009] (1)获得转弯半径：
[0010] 1-1)创建路径信息数据库，路径信息数据库中包含有机器人将要行走的路径点坐 标的集合；
[0011] 1-2)从路径信息数据库中获取关于预设路径的坐标点集合{G[i]}、和当前所在的 路径点索引i;
[0012] 1-3)从路径信息数据库中获取S个相隔一段索引号n的路径点G[i]、G[i+n]、G[i+ 2n] ,n>0；
[OOU] 1-4)计算路径点G山、G[i+n]之间的距离Ad;
[0014] 1-5)路径点6[;1]、6[1+]1]形成向量一，路径点6[1+]1]、6[1+化]形成向量二，计算向 量一和向量二之间的夹角A 01;
[001引1-6)计算车头朝向4)巧向量6「11、6「1+11]之间的夹角么白2;
[0016] 1-7)计算转弯半轻
[0017] 当计算得的R>RmaJ寸，取R = Rmax;
[001引当计算得的R<Rmin时，取R = O;
[001 9 ]当Rmin<R<Rmax 时
[0020] 其中Rmax、Rmin均为算法预先设置的参数；
[0021] (2)根据转弯半径R和当前线速度Vx(t)，估算下一个时刻的线速度Vx(t+1):
[0022] 根据步骤(1)中获得的转弯半径R计算当前曲率半径允许的最大线速度 巧max 其中Vmax和Rmax挽为算法预先巧晉的参数；
[0023] 如果
，则在下一个控制周期内继续加速，Vx(t+l)=Vx(t)+A V;
[0024] AV = a*Tc;
[0025] a-加速度；
[0026] Tc-控制周期，即本次计算和下一次计算之间的时间间隔； K rnq.\?.街 /?
[0027] 否则，W当前曲率半径允许的最大速度-为下一个控制周期内的线速度， R max 即
；
[0028] (3)计算角速度CO;
[0029] (4)由获得的路径跟踪速度(Vx，CO)来确定跟踪路径轨迹。
[0030] 本发明的一个较佳实施例中，进一步包括获得路径跟踪速度(Vx，CO)后，通过与预 设路径的拟合来优化路径跟踪速度W获得最优路径跟踪速度(Vx，《优），具体包括W下步 骤：
[0031] 3-1)创建基于路径信息的路径代价势能场图数据库；
[0032] 3-2)选择参考角速度?，
[003引当R - Rmax时，取 W 二0
[0036] 3-3)根据角速度参考值^，获得角速度捜索区向
[0034] 当R = O时， r为增益系数；
[0035] 当 0<R<F W search]，其中《 search为算法预先设置的参数；
[0037] 3-4)轨迹仿真初始化:设1
[0038] 其中：Os-角速度当前采样值；
[0039] A O-角速度细分；
[0040] N-角速度采样数量；
[0041] At-时间颗粒度；
[0042] 机器人当前朝向；
[0043] X-轨迹横坐标；
[0044] y-轨迹纵坐标；
[0045] 3-5)设置即时仿真速度(Vx，《s)契;：
[0046] 3-6)前向轨迹仿真:对即时仿真速度(Vx，Ws)f求积分'
，获 得下一个仿真时间点上机器人所在的坐标(x，y);其中，求积分W机器人当前位置为起点、 沿车头朝向的初始方向4、W A t为时间颗粒度进行，由此获得前向仿真轨迹；
[0047] 3-7)从数据库中获取实时路径代价势能场图；
[004引 3-8)累积轨迹的路径代价值:cost(Vx，《s)(t) = cost(Vx，《s)(t-l)+C(x，y);
[0049] CU,y)为步骤3-6)中仿真获得的机器人当前所在坐标对应在实时路径代价势能 场图中的路径代价值；
[(K)加]cost(Vx，COsKt-I)为机器人在……0每个时刻对应的坐标对应在实时路 径代价势能场图中的路径代价值总和；
[0051] cost(Vx，COsKt)为机器人当前轨迹的累积路径代价值；
[0052] 3-9)判断当前轨迹的累积路径代价值cost(Vx，《s):
[0化3] i)如果cost(Vx，《 s)>〇、且C(x，y)小于此前最小路径代价值，记录当前角速度为 最优角速度，且进入步骤3-5)继续下一组速度(Vx，《s)9的轨迹仿真，直至仿真角速度超过 角速度捜索区间，即《 S= (? S+ A O )> O max，如此循环，不断更新角速度为最优角速度《优；
[0054] ii)判断最优角速度W优的路径代价值C(x，y)min:
[0055] 如果C(X，y)min>〇,下发速度指令(Vx、《优)；
[0056] 否则认为无法规划合理的速度，将速度命令设为(0,0)。
[0057] 本发明的一个较佳实施例中，进一步包括步骤1-3)中n取1或者2。
[005引本发明的有益效果是：
[0059] 本发明的一种基于势能场的机器人路径跟踪算法，通过对机器人实时虚拟路径的 分析计算得到基础速度分解量(Vx，CO )，然后进一步通过势能场中机器人路径预测来得到 最优速度分解量(Vx、《优)，获得与预设路径最为贴近的跟踪路径。算法能够极大的提高计 算效率，降低对计算机硬件的要求，并且不论系统本身的特性如何变化，或者机器人相对预 设路径的初始偏角多大，算法本身都会保证机器人收敛到预设路径上。
【附图说明】
[0060] 为了更清楚地说明本发明实施例技术中的技术方案，下面将对实施例技术描述中 所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实 施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可W根据运些附图 获得其他的附图。
[0061] 图1是典型的差分驱动轮机器人底盘及其即时速度分解示意图；
[0062] 图2是本发明优选实施例的总流程图；
[0063] 图3是本发明优选实施例获得转弯半径的流程图；
[0064] 图4是本发明优选实施例获得当前线速度和参考角速度的流程图；
[0065] 图5是本发明优选实施例获得当前路径与预设路径两者之间拟合程度的流程图。
【具体实施方式】
[0066] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完 整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于 本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他 实施例，都属于本发明保护的范围。
[0067] 实施例
[0068] 图1所示为典型的差分驱动轮机器人底盘，黑色的两个后轮为主动轮，空屯、白色的 前轮为从动轮，点划线是底盘任意行进经过的路径。由运动学基础理论，任意一个时刻，其 速度都是一个线速度Vx和角速度CO的组合(Vx，CO )，且其角速度和线速度之间存在如下换 算关系:Vx = R* CO，其中R是底盘转轴中屯、相对于车辆实际旋转圆屯、0的转弯半径，Vx的方向 为即时路径的切线方向。
[0069] 相反的，如果给定一段路径{G[i]}，让机器人去跟踪，则为了能够让机器人尽快到 达目的地，机器人底盘无论何时都应该保持最大合理线速度。由此，在藉由对即时路径分析 得出的实时转弯半径R，可W推算出合理的角速度《=Vx/R，再根据对前向轨迹实时的仿真 在路径代价地图中的代价值，可W得到与预设路径最为贴近的一组路径跟踪速度（Vx， W优)。
[0070] 基于此，如图2、3、4、5所示，本实施例中公开了一种基于势能场的机器人路径跟踪 算法，可W得到与预设路径最为贴近的一组路径跟踪速度(Vx，CO )，包括W下步骤：
[0071] (1)获得转弯半径：
[0072] 1-1)创建路径信息数据库，路径信息数据库中包含有机器人将要行走的路径点坐 标的集合；
[0073] 1-2)从路径信息数据库中获取关于预设路径的坐标点集合{G[i]}、和当前所在的 路径点索引i;
[0074] 1-3)从路径信息数据库中获取S个相隔一段索引号n的路径点G[i]、G[i+n]、G[i+ 2n]，n>0;n的取值根据具体应用而定，本实施例中优选n取1或者2,能够体现路径真实的曲 率。
[00对 1-4)计算路径点G山、G[i+n]之间的距离Ad;
[0076] 1-5)路径点6[;1]、6[1+]1]形成向量一，路径点6[1+]1]、6[1+化]形成向量二，计算向 量一和向量二之间的夹角A 01;
[0077] 1-6)计算车头朝向d)和向量G[i]、G[i+n]之间的夹角A 02;
[0078] 1-7)计算转弯半轻 I 9
[0079] 当计算得的R>Rmax时，取R = Rmax;
[0080] 当计算得的R<Rmin时，取R = O;
[0081 ]当Rmin<R<Rmax时，.
[0082] 其中Rmax、Rmin均为算法预先设置的参数；
[0083] (2)根据转弯半径R和当前线速度Vx(t)，估算下一个时刻的线速度Vx(t+1):
[0084] 根据步骤(1)中获得的转弯半径R计算当前曲率半径允许的最大线速度 R max 其中Vmax和Rmax均为算法预先设置的参数；
[0085] 如果
，则在下一个控制周期内继续加速，Vx(t+l)=Vx(t)+A V;
[0086] AV = a*Tc;
[0087] a-加速度；
[0088] Tc-控制周期，即本次计算和下一次计算之间的时间间隔； m 巧'ir 杳 /?.
[0089] 否则，^当前曲率半径允许的最大速度0 为下一个控制周期内的线速度， 即
[0090] (3)计算角速度CO:
[0091] (4)获得合理的路径跟踪速度(Vx，《 )。
[0092] 获得路径跟踪速度(Vx，CO)后，通过与预设路径的拟合来优化路径跟踪速度W获 得最优路径跟踪速度(Vx，《优)，具体包括W下步骤：
[0093] 3-1)创建基于路径信息的路径代价势能场图数据库；
[0094] 3-2)选择参考角速度W ;
[009引当R = Rmax时，巧
[0096]当R = O时，
，其中Kr为增益系数；
[0097]当 0<R<Rmax 时
[009引3-3)根据角速度参考值烁，获得角速度捜索区间[? mm=?- ? search，议max=? + W search]，其中W sear。功算法预先设置的参数；
[0099] 3-4)轨迹仿真初始化：设置 Os= ?min，A O =( 〇max-?min)/N，A t，9.，X = 0，y = 0;
[0100] 其中：Os-角速度当前采样值；
[0101] Ao-角速度细分；
[0102] N-角速度采样数量；
[0103] At-时间颗粒度；
[0104] @一机器人当前朝向；
[0105] X-轨迹横坐标；
[0106] y-轨迹纵坐标；
[0107] 3-5)设置即时仿真速度(Vx，《s)声；
[0108] 3-6)前向轨迹仿真:对即时仿真速度(Vx，求积分
，获 得下一个仿真时间点上机器人所在的坐标(x，y);其中，求积分W机器人当前位置为起点、 沿车头朝向的初始方向4、W A t为时间颗粒度进行，由此获得前向仿真轨迹；
[0109] 3-7)从数据库中获取实时路径代价势能场图；
[0110] 3-8)累积轨迹的路径代价值:cost(Vx，《s)(t) = cost(Vx，《s)(t-l)+C(x，y);
[0111] CU,y)为步骤3-6)中仿真获得的机器人当前所在坐标对应在实时路径代价势能 场图中的路径代价值；
[0112] cost(Vx，COsKt-I)为机器人在……0每个时刻对应的坐标对应在实时路 径代价势能场图中的路径代价值总和；
[0113] cost(Vx，COsKt)为机器人当前轨迹的累积路径代价值；
[0114] 3-9)判断当前轨迹的累积路径代价值cost(Vx，《s):
[0115] i)如果。〇3*(￥、，《3)>0、且(：^，7)小于此前最小路径代价值，记录当前角速度为 最优角速度，且进入步骤3-5)继续下一组速度(Vx，《s)^^的轨迹仿真，直至仿真角速度超过 角速度捜索区间，即《 S= (CO S+ A O )> O max，如此循环，不断更新角速度为最优角速度《优；
[0116] ii)判断最优角速度CO优的路径代价值C(X，y)min:
[0117] 如果C(X，y)min>〇,下发速度指令(Vx、《优)；
[0118] 否则认为无法规划合理的速度，将速度命令设为(0,0)。
[0119] 本发明通过对获得的当前路径与预设路径两者之间进行拟合，使得不论系统本身 的特性如何变化，或者机器人相对预设路径的初始偏角多大，算法本身都会保证机器人收 敛到预设路径上，同时能够极大的提高计算效率，降低对计算机硬件的要求。
[0120] 对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。 对运些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的 一般原理可W在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明 将不会被限制于本文所示的运些实施例，而是要符合与本文所公开的原理和新颖特点相一 致的最宽的范围。
【主权项】
1. 一种基于势能场的机器人路径跟踪算法，其特征在于，包括以下步骤： (1) 获得转弯半径： 1-1)创建路径信息数据库，路径信息数据库中包含有机器人将要行走的路径点坐标的 集合； 1-2)从路径信息数据库中获取关于预设路径的坐标点集合{G[i]}、和当前所在的路径 点索引i; 1-3)从路径信息数据库中获取三个相隔一段索引号η的路径点G[i]、G[i+n]、G[i+2n]， n>0； 1-4)计算路径点G[i]、G[i+n]之间的距离Δ d; 1-5)路径点G[i]、G[i+n]形成向量一，路径点G[i+n]、G[i+2n]形成向量二，计算向量一 和向量二之间的夹角ΔΘ1; 1-6)计算车头朝向Φ和向量G[i]、G[i+n]之间的夹角ΔΘ2; 1-7)计算转弯半召当计算得的R>Rmax时，取R = Rmax; 当计算得的R<Rmin时，取R = O; 当 Rmin<R<Rmax 时其中Rmax、Rmln均为算法预先设置的参数； (2) 根据转弯半径R和当前线速度Vx(t)，估算下一个时刻的线速度Vx(t+1): 根据步骤（1)中获得的转弯半径R计算当前曲率半径允许的最大线速，其中 VmajPRmax均为算法预先设置的参数；如男.则在下一个控制周期内继续加速，Vx(t+l) = Vx(t)+ Δ V; Δ V = a*Tc; a一加速度； Tc 一控制周期，即本次计算和下一次计算之间的时间间隔； 否则，以当前曲率半径允许的最大速）为下一个控制周期内的线速度，BP(3) 计算角速度c(4) 由获得的路径跟踪速度(Vx，ω )来确定跟踪路径轨迹。2. 根据权利要求1所述的一种基于势能场的机器人路径跟踪算法，其特征在于:获得路 径跟踪速度(Vx，ω )后，通过与预设路径的拟合来优化路径跟踪速度以获得最优路径跟踪 速度(Vx，ω优)，具体包括以下步骤： 3-1)创建基于路径信息的路径代价势能场图数据库； 3-2)选择参考角速度 当R = Rmax时，取 W =O ; 当R=Q时，S= Λ & 其中Kr为增益系数； 当 0<R<Rmax 时；3-3)根据角速度参考值&，获得角速度搜索β scare I小其中COsearch为算法预先设置的参数；3-4)轨迹仿真初始化:设置Cos= comin， 其中：ω s-角速度当前米样值； A ω -角速度细分； N-角速度采样数量； At-时间颗粒度； 中一机器人当前朝向； X一轨迹横坐标； y一轨迹纵坐标；3-5)设置即时仿真速度CVxj Ws)供； 3-6)前向轨迹仿真:对即时仿真速度（VXf，s ) ^求积另 获得下一个仿真时间点上机器人所在的坐标(x，y);其中，求积分以机器人当前位置为起 点、沿车头朝向的初始方向Φ、以A t为时间颗粒度进行，由此获得前向仿真轨迹； 3-7)从数据库中获取实时路径代价势能场图； 3-8)累积轨迹的路径代价值:cost(Vx，c〇s)(t) = cost(Vx，c〇s) (t_l)+C(x，y); C(x，y)为步骤3-6)中仿真获得的机器人当前所在坐标对应在实时路径代价势能场图 中的路径代价值； cost(Vx，cos)(t-l)为机器人在t-l，t-2……0每个时刻对应的坐标对应在实时路径代 价势能场图中的路径代价值总和； cost(Vx，cos)(t)为机器人当前轨迹的累积路径代价值； 3-9)判断当前轨迹的累积路径代价值cost(Vx，c〇s): i)如果cost(Vx，c〇s)>〇、且C(x，y)小于此前最小路径代价值，记录当前角速度为最优 角速度，且进入步骤3-5)继续下一组速度(Vx，ws) (67的轨迹仿真，直至仿真角速度超过 角速度搜索区间，即《s=( cos+A ω )> comax，如此循环，不断更新角速度为最优角速度ω优； i i)判断最优角速度ω优的路径代价值C (X，y) min: 如果以1，7)]^彡0，下发速度指令(^^、〇优)； 否则认为无法规划合理的速度，将速度命令设为(〇，〇)。3.根据权利要求1或2所述的一种基于势能场的机器人路径跟踪算法，其特征在于：步 骤1-3)中η取1或者2。
【文档编号】G05D1/02GK106020197SQ201610502919
【公开日】2016年10月12日
【申请日】2016年6月30日
【发明人】曹睿, 王宏涛, 丁蕾
【申请人】苏州坤厚自动化科技有限公司