一种导航控制方法和装置与流程
本发明涉及自动控制技术领域，尤其涉及一种导航控制方法和装置。
背景技术：
在无人仓储技术中，对自动导引运输车导航的一般方法是采用分段导航方式：在路径段的连接处设置二维码标签等识别标志，当车辆通过该识别标志时，根据标识的位置对车辆位姿进行矫正。
在实现本发明过程中，发明人发现现有技术中至少存在如下问题：
由于位姿估计误差、识别标志设置误差等误差的存在，车辆在经过识别标志时的实际位置与规划的运行位置会存在误差，从而引起车辆运动状态的跳变，导致车辆发生摆尾、侧滑等问题，影响运行稳定性。
技术实现要素：
有鉴于此，本发明实施例提供一种导航控制方法和装置，能够实现车辆行驶过程的平顺控制，缓解位姿矫正时的摆尾现象。
为实现上述目的，根据本发明实施例的一个方面，提供了一种导航控制方法，包括：
获取车辆的位置信息和速度信息；
根据所述位置信息和速度信息计算车辆在下一插补周期中的规划速度；
构建用于表示导航路径的多项式函数，根据导航路径在路径段拼接点处的预期曲率值确定边界条件，根据所述边界条件确定所述多项式函数中各项的系数；
根据所述规划速度和所述多项式函数，计算车辆各驱动轮的规划转速；
在下一插补周期内，控制所述驱动轮依所述规划转速运行。
可选的，根据所述位置信息和速度信息计算车辆在下一插补周期中的规划速度的步骤包括：
根据所述定位信息计算车辆的剩余运动距离；
根据所述速度信息确定车辆的当前速度；
基于梯形或s形加减速策略，根据所述当前速度规划移动所述剩余运动距离时的速度函数；
根据所述速度函数确定所述车辆在下一插补周期中的规划速度。
可选的，构建用于表示导航路径的多项式函数的步骤包括：
以沿路径段向前的方向为x轴正向，以垂直于车辆运动平面向外的方向为z轴正向，构建符合右手定则的坐标系；
基于所述坐标系构建用于表示导航路径的多项式函数；
根据导航路径在路径段拼接点处的预期曲率值确定边界条件的步骤包括：
在路径段拼接点处，导航路径的预期曲率值为0，所述边界条件包括：对于所述多项式函数所对应的曲线上x坐标与所述当前路径段终点的x坐标相同的点，所述多项式函数中y的值、y对x的一阶导数值和y对x的二阶导数值在该点均为零。
可选的，在根据所述边界条件确定所述多项式函数中各项的系数的步骤前，还包括：
根据所述位置信息判断所述车辆是否位于当前路径段和下一路径段的拼接点处；
若是，则根据所述速度信息确定车辆的当前速度和当前转动角速度，根据所述当前速度和所述转动角速度计算当前路径曲率；根据所述当前路径曲率计算导航路径起点处y对x的二阶导数值；
若不是，则将导航路径起点处y对x的二阶导数值设置为零；
将所述导航路径起点处y对x的二阶导数加入边界条件。
可选的，根据所述规划速度和所述多项式函数，计算车辆各驱动轮的规划转速的步骤包括：
判断所述规划速度和所述多项式函数是否满足可跟随性要求；
若满足，则根据所述规划速度和所述多项式函数，计算车辆各驱动轮的规划转速；
若不满足，则调整所述规划速度，然后根据调整后的规划速度和所述多项式函数，计算车辆各驱动轮的规划转速；或者；
若不满足，则计算规划旋转半径，然后根据所述规划速度和所述规划旋转半径计算车辆各驱动轮的规划转速。
可选的，判断所述规划速度和所述多项式函数是否满足可跟随性要求的步骤包括：
根据所述多项式函数计算车辆在下一插补周期中的旋转半径；
根据所述规划速度和所述下一插补周期中的旋转半径计算车辆在下一插补周期中所需要的向心加速度；
判断所述向心加速度是否大于向心加速度阈值；若不大于，则确定所述规划速度和所述多项式函数满足可跟随性要求；若大于，则确定所述规划速度和所述多项式函数不满足可跟随性要求；
调整所述规划速度的步骤包括：
根据所述下一插补周期中的旋转半径和所述向心加速度阈值计算速度阈值，将所述规划速度设置为不大于所述速度阈值的值；
计算规划旋转半径的步骤包括：
据所述规划速度和所述向心加速度阈值计算半径阈值，将规划旋转半径设置为不小于所述半径阈值的值。
为实现上述目的，根据本发明实施例的另一个方面，提供了一种导航控制装置，包括：
位姿获取模块，用于获取车辆的位置信息和速度信息；
速度规划模块，用于根据所述位置信息和速度信息计算车辆在下一插补周期中的规划速度；
路径规划模块，用于构建用于表示导航路径的多项式函数，根据导航路径在路径段拼接点处的预期曲率值确定边界条件，根据所述边界条件确定所述多项式函数中各项的系数；
转速计算模块，用于根据所述规划速度和所述多项式函数，计算车辆各驱动轮的规划转速；
驱动模块，用于在下一插补周期内，控制所述驱动轮依所述规划转速运行。
可选的，所述速度规划模块还用于：
根据所述定位信息计算车辆的剩余运动距离；
根据所述速度信息确定车辆的当前速度；
基于梯形或s形加减速策略，根据所述当前速度规划移动所述剩余运动距离时的速度函数；
根据所述速度函数确定所述车辆在下一插补周期中的规划速度。
可选的，所述路径规划模块还用于：
以沿路径段向前的方向为x轴正向，以垂直于车辆运动平面向外的方向为z轴正向，构建符合右手定则的坐标系；
基于所述坐标系构建用于表示导航路径的多项式函数；
在路径段拼接点处，导航路径的预期曲率值为0，所述边界条件包括：对于所述多项式函数所对应的曲线上x坐标与所述当前路径段终点的x坐标相同的点，所述多项式函数中y的值、y对x的一阶导数值和y对x的二阶导数值在该点均为零。
可选的，所述路径规划模块还用于：
根据所述位置信息判断所述车辆是否位于当前路径段和下一路径段的拼接点处；
若是，则根据所述速度信息确定车辆的当前速度和当前转动角速度，根据所述当前速度和所述转动角速度计算当前路径曲率；根据所述当前路径曲率计算导航路径起点处y对x的二阶导数值；
若不是，则将导航路径起点处y对x的二阶导数值设置为零；
将所述导航路径起点处y对x的二阶导数加入边界条件。
可选的，所述转速计算模块还用于：
判断所述规划速度和所述多项式函数是否满足可跟随性要求；
若满足，则根据所述规划速度和所述多项式函数，计算车辆各驱动轮的规划转速；
若不满足，则调整所述规划速度，然后根据调整后的规划速度和所述多项式函数，计算车辆各驱动轮的规划转速；或者；
若不满足，则计算规划旋转半径，然后根据所述规划速度和所述规划旋转半径计算车辆各驱动轮的规划转速。
可选的，所述转速计算模块还用于：
根据所述多项式函数计算车辆在下一插补周期中的旋转半径；
根据所述规划速度和所述下一插补周期中的旋转半径计算车辆在下一插补周期中所需要的向心加速度；
判断所述向心加速度是否大于向心加速度阈值；若不大于，则确定所述规划速度和所述多项式函数满足可跟随性要求；若大于，则确定所述规划速度和所述多项式函数不满足可跟随性要求；
根据所述下一插补周期中的旋转半径和所述向心加速度阈值计算速度阈值，将所述规划速度设置为不大于所述速度阈值的值；
计算规划旋转半径的步骤包括：
据所述规划速度和所述向心加速度阈值计算半径阈值，将规划旋转半径设置为不小于所述半径阈值的值。
为实现上述目的，根据本发明实施例的再一个方面，提供了一种导航控制电子设备，包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序，
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器至少实现：
获取车辆的位置信息和速度信息；
根据所述位置信息和速度信息计算车辆在下一插补周期中的规划速度；
构建用于表示导航路径的多项式函数，根据导航路径在路径段拼接点处的预期曲率值确定边界条件，根据所述边界条件确定所述多项式函数中各项的系数；
根据所述规划速度和所述多项式函数，计算车辆各驱动轮的规划转速；
在下一插补周期内，控制所述驱动轮依所述规划转速运行。
为实现上述目的，根据本发明实施例的又一个方面，提供一种计算机可读介质，其上存储有计算机程序，所述程序被处理器执行时至少实现：
获取车辆的位置信息和速度信息；
根据所述位置信息和速度信息计算车辆在下一插补周期中的规划速度；
构建用于表示导航路径的多项式函数，根据导航路径在路径段拼接点处的预期曲率值确定边界条件，根据所述边界条件确定所述多项式函数中各项的系数；
根据所述规划速度和所述多项式函数，计算车辆各驱动轮的规划转速；
在下一插补周期内，控制所述驱动轮依所述规划转速运行。
上述发明中的一个实施例具有如下优点或有益效果：通过构建特定边界条件的多项式进行路径规划，从而保证了车辆在经过路径段的拼接点时平顺运行，不会发生摆尾、侧滑等现象，提高了车辆运行的稳定性。此外，通过对规划速度和导航路径进行可跟随性调整，避免了正常运行过程中车辆的侧滑问题。整体而言，本发明实施例提供了一种平顺导航控制方法，可以维持车辆的平顺运行，提高了运输稳定性和工作效率。
上述的非惯用的可选方式所具有的进一步效果将在下文中结合具体实施方式加以说明。
附图说明
附图用于更好地理解本发明，不构成对本发明的不当限定。其中：
图1是根据本发明实施例的导航控制方法的主要步骤的示意图；
图2是基于现有的导航控制方法的车辆运行示意图；
图3a是梯形加减速策略的速度规划示意图；
图3b是梯形加减速策略的位置规划示意图；
图4是根据本发明实施例的导航控制装置的主要模块的示意图；
图5是本发明实施例可以应用于其中的示例性系统架构图；
图6是适于用来实现本发明实施例的终端设备或服务器的计算机系统的结构示意图。
具体实施方式
以下结合附图对本发明的示范性实施例做出说明，其中包括本发明实施例的各种细节以助于理解，应当将它们认为仅仅是示范性的。因此，本领域普通技术人员应当认识到，可以对这里描述的实施例做出各种改变和修改，而不会背离本发明的范围和精神。同样，为了清楚和简明，以下的描述中省略了对公知功能和结构的描述。
本发明实施例以两轮差速驱动的车辆为例，说明本发明提供的导航控制方法的具体应用。其中，所述车辆可以为自动导引运输车(automatedguidedvehicle,agv)，但应当理解，此方法也可应用于其他通过两轮差速驱动的运动设备，例如机器人、无人车辆等。
任意时刻，车辆运动的旋转运动半径r满足：
则左轮速满足：
其中，d为左右驱动轮间的距离，v为车辆的整体运行速度，ω为车辆的旋转角速度。
同理，右轮速满足：
基于差速式车辆的运动原理，驱动轮连续的速度变化过程是差速式车辆的基本要求，否则将导致车辆运动过程中出现明显振动，影响控制质量。
基于t形或s形速度规划原理，规划的车辆车体中心速度v是连续的，为保证vl,vr的连续性，则车辆运动的曲率半径r至少为连续的。而为保证车辆运动曲率的连续性，要求车辆的导航轨迹至少为g2连续。因此，只有高阶光滑的导航路径才能满足控制需求。
图2是基于现有的导航控制方法的车辆运行示意图。如图2所示，当前的车辆二维码导航的一般方法是采用分段导航方式，通过在地面设置二维码标签201，以相邻两二维码标签201之间的路段作为路径段实行导航；为了便于运算，在进入每一个路径段后，坐标系原点被重定义在当前所在二维码导向标的中心位置，车辆预期前进方向(即路径段起始处二维码标签中心指向路径段终止处二维码标签中心的方向)定义为坐标系x正方向，z轴垂直于地面朝上，坐标系y轴方向满足右手法则。当车辆进入下一路径段时，将系统坐标系重置为当前二维码为原点的局部坐标系，将车辆运动的x向位移置为当前局部坐标系的x向偏移。
在规划车辆路径时，不仅要保证在相邻两个二维码间路径段内控制轨迹的光滑性，在相邻路径段的拼接点处(即二维码处)同样要求控制的光滑输出。记相邻两条拼接路径分别为pj,pj+1，则满足控制光滑性需求的曲率关系为：
其中，l为相邻两二维码标签201之间的距离，即单路径段的长度，为一定值；κ为曲率标识；px表示路径段的x轴坐标。式(3)表示，对于任意二维码标签所对应的点，相邻的两条拼接路径在该点处的曲率应相同，以避免轮速突变造成摆尾震动现象。
但在车辆实际运行过程中，在路径的每个二维码标签处均存在位姿状态不连续的情况，其产生的原因主要包括：车辆路径的二维码路标采用人工方式张贴于地面，在张贴角度，张贴位置等方面存在误差(例如图2位于中部的二维码标签201，与标准方向存在大小为α的偏差角)；车辆配备的视觉摄像头在车辆高速运动过程中对二维码的测量过程存在误差；相邻二维码之间的车辆轨迹依托差速轮采集到的轮速信息进行估计，其估计原理为根据采集到的轮速实时对车辆的运动学模型进行数值积分，此估计轨迹与车辆实际运行轨迹存在较大误差。例如，在图2中，估计轨迹202和实际轨迹203之间就存在着较大的误差，导致估计轨迹202与实际轨迹203在中部的二维码标签处存在误差204，根据现有技术的方法，估计轨迹202需要跟随实际轨迹203进行变化，而估计轨迹202的突变会导致车辆驱动轮轮速的突变，引起摆尾震动现象。
因此，对于连续、同向的路径段pj,pj+1而言，式(3)成立的唯一条件为：
式(4)的进一步解释为相邻导航路径的拼接点处，考虑到二维码测量误差，状态估计误差等导致的状态不连续，基于状态反馈的导航方式无法实现平顺的控制输出，本发明给出的处理方法为在路径拼接点设计端点曲率为0的轨迹段，实现车辆估计的状态轨迹不连续性情况下控制作用的连续变化。
下面通过具体的推导计算过程对本发明提供的方法进行说明。本方法主要基于以下理论基础实现：
1.五次多项式曲线路径规划
(1)五次多项式曲线路径表述
简化计算，记y＝py,x＝px-px_c，py表示车辆在规划的运行过程中的y轴坐标，px表示车辆在规划的运行过程中的x轴坐标，px∈(px_c,l]，px_c表示车辆的当前x轴坐标，则定义五次多项式曲线：
y＝a0+a1x+a2x2+a3x3+a4x4+a5x5(5)
计算多项式(5)的导数信息如下：
式(5)-(7)中，a0-a5为待定参数。基于5次多项式及其各阶导数信息，可实现规划路径的始末位置，方向，曲率的精确可控。
记导航路径的已知边界条件有：
xs＝0,xe＝l-px_c
ys＝py_c
其中，xs为曲线起始点的x轴坐标，xe为曲线终止点的x轴坐标，ys为曲线起始点的y轴坐标；l表示相邻二维码标签间的距离，为固定值；px_c，py_c为在当前路径段坐标系下、车辆的当前x轴和y轴坐标，
px_c，py_c可以通过车辆的位姿估计器获取，属于已知量。
定义记号
ys＝a0
ye＝a0+a1xe+a2xe2+a3xe3+a4xe4+a5xe5
ys'＝a1
ye'＝a1+2a2xe+3a3xe2+4a4xe3+5a5xe4
ys”＝2a2
ye”＝2a2+6a3xe+12a4xe2+20a5xe3(8)
进一步，可以求得多项式的各项系数满足如下关系：
a0＝ys
a1＝ys'
其中，ye,ys',ye',ys”,ye”为需进一步确定的边界信息，其中，ye为曲线终止点处y的取值，各导数项分别为曲线的起始点(对应角标s)或终止点(对应角标e)处y对x的一阶或二阶导数。
(2)路径边界条件确定
规划路径的终止条件期望为当px＝l时，y向偏差与角度偏差均为0，即：
且曲线终止点处的曲率κe为零，意味着此时ωe＝veκe＝0。
由平面曲线的曲率公式：
终止曲率条件κe＝0等价于：
ye”＝0(12)
定义位姿状态三元组(px,py,θ)，py表示车辆在规划的运行过程中的y轴坐标，px表示车辆在规划的运行过程中的x轴坐标，θ是车辆的相对于x轴正向的偏移角。
以车辆上一插补时刻的运动状态作为规划路径的起始状态，记该起始状态为(px_c,py_c,θc)，则有：
由于θc可以根据计算或测量获取，因此ys'的值可计算。
关于路径初始边界条件ys”的确定，分为两种情况：
情况a.若车辆在当前导航路径段内部。已知车辆当前插补周期内的给进速度为vc，转动角速度为ωc，(均可通过上一插补时刻规划的轮速计算获得，或者可以通过测量获得)则可知此插补周期内的路径曲率为
情况b.若车辆在路径段拼接点处。根据式(4)平顺控制的要求，有κs＝0。因而有ys”＝0。
综上，将上述边界条件带入式(9)中，即可求取a0-a5的值，从而确定五次多项式(5)。
2.速度规划方法
计算导航运动的剩余距离长度len，其满足：
len＝((n-i)l+l-px_c)
其中，n为路径段总数。采用梯形或s形加减速策略规划轨迹运动len距离的速度信息，记为v(t)；相应地，可求得总运动时间记为t。简单起见，速度规划过程可记为
[x(t),v(t),t]＝velocityplanning(vc,len,ve,vb,accb,decb)
其中，x(t)是与速度信息对应的位移信息；vc为当前行进速度，可以通过测量获取或直接获取前一插补时刻计算得到的规划速度作为当前行进速度；ve为终点处速度，取0；vb为速度规划设定的最大速度约束，accb为最大加速度约束，decb为最大减速度约束。为了提高精确度，在实际运算时，vc和len可以分别为各自实际值沿x轴正向的分量，或者各自沿规划曲线函数当前点切向的分量。
图3a是梯形加减速策略的速度规划示意图；图3b是梯形加减速策略的位置规划示意图。作为参考，图3a为初始速度、终点处速度均为0时的梯形速度规划示意图，图3b为相应的位移示意图。图中acc表示加速区间，dec表示减速区间，xc表示初始位移，xe表示终点处位移。
3.规划路径的可跟随性处理方法
已知惯性离心力f与车辆轮系抓地力ffriction需满足如下关系：
f＜ffriction(15)
其中，
rmin也可以称为半径阈值，若以速度v行驶时，旋转半径小于该半径阈值，则车辆会发生侧滑。
因此，在完成曲线路径规划和车辆在下一插补周期中的规划速度后(速度规划的具体实现方式见后文)。需要判断车辆依规划速度在曲线路径上运行时是否会发生侧滑。例如，由于车辆工作面的摩擦系数μ0已知，因此可以通过判断预期运动所需的向心加速度
(1)精确导航模式
此模式中，在运动半径较小处降低车辆的运行速度，从而保证车辆按照规划的导航轨迹曲线精确行进的同时避免侧滑。实际操作时，若判定可能发生侧滑，则将计算得到的下一插补周期中的旋转半径带入
(2)误差可控模式
在此模式中，将规划速度带入
上面两种调整方法各有利弊，模式(1)可以实现对规划路径曲线的精确追踪，但是在出现局部高曲率区域时会降低车辆的速度，影响车辆的运行效率；模式(2)可以尽可能地减小对运动效率的影响，但是会相应增大导航误差，只能保证导航误差在可控范围内。
图1是根据本发明实施例的导航控制方法的主要步骤的示意图。如图1所示，以前述理论为基础，本发明提供一种导航控制方法。为了便于理解，下面通过伪代码的方式对插补时刻所做的运算过程进行说明：
输入信息：车辆行走的路径段数目n,每个路径段的长度l,最高行进速度vb,最大加速度ab,初始行进速度vstart,路径终止速度ve＝0.插补周期ts。
初始化：当前所在路径段索引i＝1.获得车辆定位信息(px_c,py_c,θc)。
loop:
while(i≠n&&px_c≠l){
s100，获取车辆的位置信息和速度信息。其中，位置信息可以包括车辆当前的坐标、相对于x轴正向的偏转角；速度信息可以包括车辆的当前速度、各驱动轮的当前转速和车辆的当前转动速度。如果车辆处于导航路径拼接点处，则i＝i+1，并更新导航基准坐标系。
s101，根据所述位置信息和速度信息，依据前述“速度规划方法”的理论计算车辆在下一插补周期中的规划速度。具体而言，是根据速度规划方法计算车辆在下一插补周期中的速度信息v(t)和位移信息x(t)，以及计算总运动时间t。此外，计算导航运动所述的插补周期个数
s102，依据前述“五次多项式曲线路径规划”的理论，构建用于表示导航路径的多项式函数，根据导航路径在路径段拼接点处的预期曲率值确定边界条件，根据所述边界条件确定所述多项式函数中各项的系数；
s103，根据所述规划速度和所述多项式函数，计算车辆各驱动轮的规划转速。具体而言，s103包括如下子步骤：
判断所述规划速度和所述多项式函数是否满足可跟随性要求；
若满足，则根据所述规划速度和所述多项式函数，计算车辆各驱动轮的规划转速；
若不满足，则调整所述规划速度，然后根据调整后的规划速度和所述多项式函数，计算车辆各驱动轮的规划转速(参考前文“精确导航模式”)；或者；
若不满足，则计算规划旋转半径，然后根据所述规划速度和所述规划旋转半径计算车辆各驱动轮的规划转速(参考前文“误差可控模式”)。
根据式(1)和式(2)，以及曲率
其中，根据s102计算得到的五次多项式曲线(5)在插补点x(ts)处的曲率信息κ(ts)，如下：
其中，
y'(ts)＝a1+2a2x(ts)+3a3x(ts)2+4a4x(ts)3+5a5x(ts)4
y”(ts)＝2a2+6a3x(ts)+12a4x(ts)2+20a5x(ts)3
s104，输出vl(ts)、vr(ts)至左右轮的驱动伺服器，在下一插补周期内，控制所述驱动轮依所述规划转速运行。
等待ts时长，进行下一个循环。
}
综上可见可以看出，本发明实施例提供的导航控制方法，通过构建特定边界条件的多项式进行路径规划，从而保证了车辆在经过路径段的拼接点时平顺运行，不会发生摆尾、侧滑等现象，提高了车辆运行的稳定性。此外，通过对规划速度和导航路径进行可跟随性调整，避免了正常运行过程中车辆的侧滑问题。整体而言，本发明实施例提供了一种平顺导航控制方法，可以维持车辆的平顺运行，提高了运输稳定性和工作效率。
图4是根据本发明实施例的导航控制装置的主要模块的示意图。如图所示，在另一实施例中，提供一种导航控制装置400，包括：
位姿获取模块401，用于获取车辆的位置信息和速度信息；
速度规划模块402，用于根据所述位置信息和速度信息计算车辆在下一插补周期中的规划速度；
路径规划模块403，用于构建用于表示导航路径的多项式函数，根据导航路径在路径段拼接点处的预期曲率值确定边界条件，根据所述边界条件确定所述多项式函数中各项的系数；
转速计算模块404，用于根据所述规划速度和所述多项式函数，计算车辆各驱动轮的规划转速；
驱动模块405，用于在下一插补周期内，控制所述驱动轮依所述规划转速运行。
在一些可选的实施例中，所述速度规划模块402还用于：
根据所述定位信息计算车辆的剩余运动距离；
根据所述速度信息确定车辆的当前速度；
基于梯形或s形加减速策略，根据所述当前速度规划移动所述剩余运动距离时的速度函数；
根据所述速度函数确定所述车辆在下一插补周期中的规划速度。
在一些可选的实施例中，所述路径规划模块403还用于：
以沿路径段向前的方向为x轴正向，以垂直于车辆运动平面向外的方向为z轴正向，构建符合右手定则的坐标系；
基于所述坐标系构建用于表示导航路径的多项式函数；
在路径段拼接点处，导航路径的预期曲率值为0，所述边界条件包括：对于所述多项式函数所对应的曲线上x坐标与所述当前路径段终点的x坐标相同的点，所述多项式函数中y的值、y对x的一阶导数值和y对x的二阶导数值在该点均为零。
在一些可选的实施例中，所述路径规划模块403还用于：
根据所述位置信息判断所述车辆是否位于当前路径段和下一路径段的拼接点处；
若是，则根据所述速度信息确定车辆的当前速度和当前转动角速度，根据所述当前速度和所述转动角速度计算当前路径曲率；根据所述当前路径曲率计算导航路径起点处y对x的二阶导数值；
若不是，则将导航路径起点处y对x的二阶导数值设置为零；
将所述导航路径起点处y对x的二阶导数加入边界条件。
在一些可选的实施例中，所述转速计算模块404还用于：
判断所述规划速度和所述多项式函数是否满足可跟随性要求；
若满足，则根据所述规划速度和所述多项式函数，计算车辆各驱动轮的规划转速；
若不满足，则调整所述规划速度，然后根据调整后的规划速度和所述多项式函数，计算车辆各驱动轮的规划转速；或者；
若不满足，则计算规划旋转半径，然后根据所述规划速度和所述规划旋转半径计算车辆各驱动轮的规划转速。
在一些可选的实施例中，所述转速计算模块404还用于：
根据所述多项式函数计算车辆在下一插补周期中的旋转半径；
根据所述规划速度和所述下一插补周期中的旋转半径计算车辆在下一插补周期中所需要的向心加速度；
判断所述向心加速度是否大于向心加速度阈值；若不大于，则确定所述规划速度和所述多项式函数满足可跟随性要求；若大于，则确定所述规划速度和所述多项式函数不满足可跟随性要求；
根据所述下一插补周期中的旋转半径和所述向心加速度阈值计算速度阈值，将所述规划速度设置为不大于所述速度阈值的值；
计算规划旋转半径的步骤包括：
据所述规划速度和所述向心加速度阈值计算半径阈值，将规划旋转半径设置为不小于所述半径阈值的值。
本实施例提供的导航控制装置通过构建特定边界条件的多项式进行路径规划，从而保证了车辆在经过路径段的拼接点时平顺运行，不会发生摆尾、侧滑等现象，提高了车辆运行的稳定性。此外，通过对规划速度和导航路径进行可跟随性调整，避免了正常运行过程中车辆的侧滑问题。整体而言，本发明实施例提供了一种平顺导航控制方法，可以维持车辆的平顺运行，提高了运输稳定性和工作效率。
图5示出了可以应用本发明实施例的导航控制方法或导航控制装置的示例性系统架构500。
如图5所示，系统架构500可以包括终端设备501、502、503，网络504和服务器505。网络504用以在终端设备501、502、503和服务器505之间提供通信链路的介质。网络504可以包括各种连接类型，例如有线、无线通信链路或者光纤电缆等等。
用户可以使用终端设备501、502、503通过网络504与服务器505交互，以接收或发送消息等。终端设备501、502、503为车辆等，用于接收运行控制指令并进行运动。
服务器505可以是提供各种服务的服务器，例如接收外部运行控制指令并指示终端设备501、502、503自动运行的管理服务器。
需要说明的是，本发明实施例所提供的导航控制方法一般由终端设备501、502、503执行，相应地，导航控制装置一般设置于终端设备501、502、503中。
应该理解，图5中的终端设备、网络和服务器的数目仅仅是示意性的。根据实现需要，可以具有任意数目的终端设备、网络和服务器。
根据本发明的实施例，本发明还提供了一种电子设备和一种可读存储介质。
图6是适于用来实现本发明实施例的终端设备或服务器的计算机系统的结构示意图。
下面参考图6，其示出了适于用来实现本发明实施例的终端设备的计算机系统600的结构示意图。图6示出的终端设备仅仅是一个示例，不应对本发明实施例的功能和使用范围带来任何限制。
如图6所示，计算机系统600包括中央处理单元(cpu)601，其可以根据存储在只读存储器(rom)602中的程序或者从存储部分608加载到随机访问存储器(ram)603中的程序而执行各种适当的动作和处理。在ram603中，还存储有系统600操作所需的各种程序和数据。cpu601、rom602以及ram603通过总线604彼此相连。输入/输出(i/o)接口605也连接至总线604。
以下部件连接至i/o接口605：包括键盘、鼠标等的输入部分606；包括诸如阴极射线管(crt)、液晶显示器(lcd)等以及扬声器等的输出部分607；包括硬盘等的存储部分608；以及包括诸如lan卡、调制解调器等的网络接口卡的通信部分609。通信部分609经由诸如因特网的网络执行通信处理。驱动器610也根据需要连接至i/o接口605。可拆卸介质611，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器610上，以便于从其上读出的计算机程序根据需要被安装入存储部分608。
特别地，根据本发明的实施例，上文主要步骤的示意图描述的过程可以被实现为计算机软件程序。例如，本发明的实施例包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行主要步骤的示意图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信部分609从网络上被下载和安装，和/或从可拆卸介质611被安装。在该计算机程序被中央处理单元(cpu)601执行时，执行本发明的系统中限定的上述功能。
需要说明的是，本发明所示的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(ram)、只读存储器(rom)、可擦式可编程只读存储器(eprom或闪存)、光纤、便携式紧凑磁盘只读存储器(cd-rom)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本发明中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本发明中，计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：无线、电线、光缆、rf等等，或者上述的任意合适的组合。
附图中的流程图和框图，图示了按照本发明各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，上述模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图或流程图中的每个方框、以及框图或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
描述于本发明实施例中所涉及到的模块可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的模块也可以设置在处理器中，例如，可以描述为：一种处理器包括位姿获取模块、速度规划模块、路径规划模块、转速计算模块和驱动模块。其中，这些模块的名称在某种情况下并不构成对该模块本身的限定，例如，驱动模块还可以被描述为“用于在下一插补周期内，控制所述驱动轮依所述规划转速运行的模块”。
作为另一方面，本发明还提供了一种计算机可读介质，该计算机可读介质可以是上述实施例中描述的设备中所包含的；也可以是单独存在，而未装配入该设备中。上述计算机可读介质承载有一个或者多个程序，当上述一个或者多个程序被一个该设备执行时，使得该设备包括：
获取车辆的位置信息和速度信息；
根据所述位置信息和速度信息计算车辆在下一插补周期中的规划速度；
构建用于表示导航路径的多项式函数，根据导航路径在路径段拼接点处的预期曲率值确定边界条件，根据所述边界条件确定所述多项式函数中各项的系数；
根据所述规划速度和所述多项式函数，计算车辆各驱动轮的规划转速；
在下一插补周期内，控制所述驱动轮依所述规划转速运行。
根据本发明实施例的技术方案，通过构建特定边界条件的多项式进行路径规划，从而保证了车辆在经过路径段的拼接点时平顺运行，不会发生摆尾、侧滑等现象，提高了车辆运行的稳定性。此外，通过对规划速度和导航路径进行可跟随性调整，避免了正常运行过程中车辆的侧滑问题。整体而言，本发明实施例提供了一种平顺导航控制方法，可以维持车辆的平顺运行，提高了运输稳定性和工作效率。
上述具体实施方式，并不构成对本发明保护范围的限制。本领域技术人员应该明白的是，取决于设计要求和其他因素，可以发生各种各样的修改、组合、子组合和替代。任何在本发明的精神和原则之内所作的修改、等同替换和改进等，均应包含在本发明保护范围之内。
- 还没有人留言评论。精彩留言会获得点赞！