本发明涉及机器控制技术领域，尤其涉及一种无人车网络编队的运动规划和协作定位方法及装置。
背景技术：
对于这些新一代的智能无人车，在农业、军事、医用、服务行业等等方面，人们提出了更高层次的需求，而且越来越往集群化、互协作方向发展。车联网这个概念已然渐渐被越来越多的大众所熟知，也得到了越来越多的专家和企业的关注，车联网也被广泛得证明有着长远的发展空间，而车联网所提供的大多是基于位置的服务，因此车辆的定位在未来车联网的体系架构中是最重要的核心技术之一。
现有技术中，无人车的定位通常采用全球卫星定位系统来实现，无人车的路径规划常采用人工势场法、格点法和视觉导航法。其中，人工势场路径规划容易陷入局部极值点而难以达到全局最优，势函数在多目标的复杂网络中也很选择；格点法则在控制精度和运算量的折中上很难达成一致，格点划分细密的同时将带来网络存储的灾难；视觉导航对视觉传感器和中央处理器都提出了较高的要求，在处理图像信息的时候对于存储空间带来了较大的负担。并且，无人车要想获得较高精度的定位，就需要消耗更多的通信资源和计算资源去接收和处理全球卫星定位系统发送的定位数据，从而使无人车在车间通信和路径规划等方面的可用资源减少，使无人车的控制效率和定位精度降低。
技术实现要素：
本发明的目的是提供一种无人车网络编队的运动规划和协作定位方法及装置，解决了现有技术中在集群相对定位过程中遇到的定位容量受限、定位效率低的技术问题。
为了解决上述技术问题，一方面，本发明提供一种无人车网络编队的运动规划和协作定位方法，其特征在于，包括：
在一个规划周期内，根据获取到的无人车网络编队中每一车辆的初始位姿、目标位姿和运动学参数，生成路径规划，所述无人车网络编队包括一个领航车辆和若干个随从车辆；
将所述路径规划发送到每一随从车辆，以供每一随从车辆根据所述路径规划完成相应的目标位姿。
另一方面，本发明提供一种无人车网络编队的运动规划和协作定位装置，其特征在于，包括：
路径规划模块，用于在一个规划周期内，根据获取到的无人车网络编队中每一车辆的初始位姿、目标位姿和运动学参数，生成路径规划，所述无人车网络编队包括一个领航车辆和若干个随从车辆；
发送模块，用于将所述路径规划发送到每一随从车辆，以供每一随从车辆根据所述路径规划完成相应的目标位姿。
再一方面，本发明提供一种用于无人车网络编队的运动规划和协作定位的电子设备，包括：
存储器和处理器，所述处理器和所述存储器通过总线完成相互间的通信；所述存储器存储有可被所述处理器执行的程序指令，所述处理器调用所述程序指令能够执行上述的方法。
又一方面，本发明提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述的方法。
本发明提供的无人车网络编队的运动规划和协作定位方法及装置，采用分层式体系结构，设置领航车辆和随从车辆；领航车辆统一规划最优路径，将整体方案分配调度，随从车辆分布式自主控制，完成任务。路径规划部分考虑到实际情况中的最大曲率与速度限制等约束条件，采取dubins最短路径规划方式，设计出最短路径，具有实用性强，路径最优等特性。协作定位部分通过扩展卡尔曼滤波，利用uwb测距、惯性导航、陀螺仪等易于采集的进行信息融合，并且对传感器和存储空间的要求低，所测量数据极易获得，易于存储，处理方便，所以有低延时的优点，同时消除了位姿估计的不确定性，增加了网络的相对定位精度，保证系统在低数据量获取情况下的高定位精度，提高了定位效率。
附图说明
图1为依照本发明实施例的无人车网络编队的运动规划和协作定位方法示意图；
图2为依照本发明实施例的无人车网络编队的运动规划和协作定位方法的逻辑流程图；
图3为依照本发明实施例的pid控制器流程图；
图4为依照本发明实施例的系统分层式体系结构示意图；
图5为依照本发明实施例的dubins路径示意图；
图6为依照本发明实施例的场景中无人车的运动建模示意图；
图7为依照本发明实施例的无人车网络编队的运动规划和协作定位装置示意图；
图8为依照本发明实施例的用于无人车网络编队的运动规划和协作定位的电子设备示意图。
具体实施方式
为了使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
图1为依照本发明实施例的无人车网络编队的运动规划和协作定位方法示意图，如图1所示，本发明实施例提供一种无人车网络编队的运动规划和协作定位方法，其执行主体为无人车网络编队的领航车辆，该方法包括：
步骤s101、在一个规划周期内，根据获取到的无人车网络编队中每一车辆的初始位姿、目标位姿和运动学参数，生成路径规划，所述无人车网络编队包括一个领航车辆和若干个随从车辆；
步骤s102、将所述路径规划发送到每一随从车辆，以供每一随从车辆根据所述路径规划完成相应的目标位姿。
具体的，在一个大规模的无人车网络编队中，给定一个领航车辆和若干随从车辆，首先，需要将目标队形(任务)通过终端发送给领航单元，目标队形中包含每一随从车辆的目标位姿。
在一个规划周期内，领航车辆根据获取到的无人车网络编队中每一车辆的初始位姿、目标位姿和运动学参数，生成路径规划。
然后，领航车辆通过自带的超宽带模块将路径规划发送到每一随从车辆，每一随从车辆根据从领航车辆获取的路径规划完成相应的目标位姿。最终完成队形。
本发明实施例提供的无人车网络编队的运动规划和协作定位方法，采用分层式体系结构，设置领航车辆和随从车辆；领航车辆统一规划最优路径，将整体方案分配调度，随从车辆分布式自主控制，完成任务。该方法仅需要领航车辆配置较高性能的定位设备和计算设备，随从车辆通过相互协作完成精确定位，增加了网络的相对定位精度，保证系统在低数据量获取情况下的高定位精度，提高了定位效率。
在上述实施例的基础上，进一步地，所述方法还包括：
接收每个随从车辆反馈的位姿信息；
根据所述位姿信息判断每个随从车辆是否完成相应的目标位姿；
若判断获知存在任一随从车辆未完成相应的目标位姿，则进入下一个规划周期，重新规划路径。
具体的，图2为依照本发明实施例的无人车网络编队的运动规划和协作定位方法的逻辑流程图，如图2所示，领航车辆接收终端发送的指令，指令中包含目标队形；领航车辆根据获取到的无人车网络编队中每一车辆的初始位姿、目标位姿和运动学参数，生成路径规划；领航车辆将路径规划发送到每一随从车辆，在得到理想路径规划，即领航车辆发送的路径规划后，各随从车辆将自主控制，完成目标位姿。运动控制的过程中主要依靠经典pid控制器来实现。
在运动控制过程中，领航车辆接收每个随从车辆反馈的位姿信息；
领航车辆根据反馈的位姿信息判断每个随从车辆是否完成相应的目标位姿；若判断获知存在任一随从车辆未完成相应的目标位姿，则将反馈误差反馈给相应的随从车辆，进入下一个规划周期，重新规划路径，若判断获知所有的随从车辆都完成相应的目标位姿，领航车辆继续判断有无新指令，若有新指令，则接收新指令，若无新指令，则完成队形。
图3为依照本发明实施例的pid控制器流程图，如图3所示，本发明实施例提供的方法，通过pid反馈调节的方式将整个编队精准而平滑地控制到目标队形，即，根据理想航迹x*和估计所得位姿
本发明实施例提供的无人车网络编队的运动规划和协作定位方法，采用分层式体系结构，设置领航车辆和随从车辆；领航车辆统一规划最优路径，将整体方案分配调度，随从车辆分布式自主控制，完成任务。该方法仅需要领航车辆配置较高性能的定位设备和计算设备，随从车辆通过相互协作完成精确定位，增加了网络的相对定位精度，保证系统在低数据量获取情况下的高定位精度，提高了定位效率。
在以上各实施例的基础上，进一步地，所述根据获取到的无人车网络编队中每一车辆的初始位姿、目标位姿和运动学参数，生成路径规划具体为：
将获取到的无人车网络编队中每一车辆的初始位姿、目标位姿和运动学参数输入至预设的优化模型，输出多种dubins路径；
选择所述多种dubins路径中最短的路径，作为路径规划。
其中，所述优化模型为：
minimizef
subjesttop*(n+1)＝f(p*(n),u*(n))
p*(0)&p*(n)
其中，f为代价函数，
具体的，图4为依照本发明实施例的系统分层式体系结构示意图，如图4所示，领航单元在获取无人车网络编队的拓扑结构后，构建系统分层式体系结构，以领航单元为坐标原点，根据各随从单元的初始坐标和目标位置，建立各节点的状态空间和测量空间，将车辆的速度、加速度、角速度和转弯曲率等运动学参数纳入滤波范围。特殊的，以第i辆车n时刻状态和测量为例，其状态空间和测量空间设计如下：
其中，状态空间
如若扩展到三位无人机，可相应地增加z轴分量。测量空间
本发明中利用陀螺仪进行测量。图5为依照本发明实施例的dubins路径示意图，如图5所示，在进行航迹规划时，我们以路径最短为目标设计dubins路径。
具体方法为：
设计代价函数
minimizef
subjesttop*(n+1)＝f(p*(n),u*(n))
p*(0)&p*(n)
图6为依照本发明实施例的场景中无人车的运动建模示意图，如图6所示，此处的f(·)是在运动过程中速度、加速度等物理量遵循运动学定律以及遵循的几何关系函数。此处对几何关系则做了如下近似：
本发明实施例提供的无人车网络编队的运动规划和协作定位方法，采用分层式体系结构，设置领航车辆和随从车辆；领航车辆统一规划最优路径，将整体方案分配调度，随从车辆分布式自主控制，完成任务。路径规划部分考虑到实际情况中的最大曲率与速度限制等约束条件，采取dubins最短路径规划方式，设计出最短路径，具有实用性强，路径最优等特性。协作定位部分通过扩展卡尔曼滤波，利用uwb测距、惯性导航、陀螺仪等易于采集的进行信息融合，并且对传感器和存储空间的要求低，所测量数据极易获得，易于存储，处理方便，所以有低延时的优点，同时消除了位姿估计的不确定性，增加了网络的相对定位精度，保证系统在低数据量获取情况下的高定位精度，提高了定位效率。
在以上各实施例的基础上，进一步地，所述初始位姿由随从车辆基于获取到的相对观测数据，利用扩展卡尔曼滤波算法得到。
具体的，在初次进行路径规划是，由于没有任何关于位置的先验信息，首先需通过无人车自带的超宽带模块进行相互测距结算得到网络的拓扑结构，此处采用余弦定理结算即可：任意三辆车若得知两两之间的距离，通过a＝arccos[(b2+c2-a2)/2bc]即可结算出任意两边之间夹角，进而得到整个网络的拓扑结构。
初始位姿由随从车辆基于获取到的相对观测数据，利用扩展卡尔曼滤波算法得到，具体步骤如下：
对于状态方程
如图5所示，此处在时间间隙足够短的情况下，对小车单位时隙内的运动做了线性化处理，将其质心走过的圆弧段近似为直线，所以根据本发明的运动建模，我们有如下关系：
f(·)和h(·)是非线性的，利用雅各比矩阵对系统做一阶泰勒近似，可以得到
l(n)是作用在u(n)上的变换模型，本发明中由f(·)对u泰勒一阶近似得到，
h(n)是观测模型，它把n时刻的真实状态空间映射成观测空间；本发明中由h(·)对x泰勒一阶近似得到，
所以扩展卡尔曼滤波过程可分为两个阶段：状态预测与测量更新，具体表述为：
状态预测阶段：
测量更新阶段：
此处通过测量更新能够修正状态预测过程中的状态误差，主要通过卡尔曼增益系数来控制预测和测量的权重，通过信息融合弥补不确定性。
本发明实施例提供的无人车网络编队的运动规划和协作定位方法，采用分层式体系结构，设置领航车辆和随从车辆；领航车辆统一规划最优路径，将整体方案分配调度，随从车辆分布式自主控制，完成任务。路径规划部分考虑到实际情况中的最大曲率与速度限制等约束条件，采取dubins最短路径规划方式，设计出最短路径，具有实用性强，路径最优等特性。协作定位部分通过扩展卡尔曼滤波，利用uwb测距、惯性导航、陀螺仪等易于采集的进行信息融合，并且对传感器和存储空间的要求低，所测量数据极易获得，易于存储，处理方便，所以有低延时的优点，同时消除了位姿估计的不确定性，增加了网络的相对定位精度，保证系统在低数据量获取情况下的高定位精度，提高了定位效率。
图7为依照本发明实施例的无人车网络编队的运动规划和协作定位装置示意图，如图7所示，本发明实施例提供一种无人车网络编队的运动规划和协作定位装置，包括路径规划模块701和发送模块702，其中，路径规划模块701用于在一个规划周期内，根据获取到的无人车网络编队中每一车辆的初始位姿、目标位姿和运动学参数，生成路径规划，所述无人车网络编队包括一个领航车辆和若干个随从车辆；
发送模块702用于将所述路径规划发送到每一随从车辆，以供每一随从车辆根据所述路径规划完成相应的目标位姿。
本发明实施例提供一种无人车网络编队的运动规划和协作定位装置，用于完成上述实施例中所述的方法，通过本实施例提供的装置完成上述实施例中所述的方法的具体步骤与上述实施例相同，此处不再赘述。
本发明实施例提供的无人车网络编队的运动规划和协作定位装置，采用分层式体系结构，设置领航车辆和随从车辆；领航车辆统一规划最优路径，将整体方案分配调度，随从车辆分布式自主控制，完成任务。路径规划部分考虑到实际情况中的最大曲率与速度限制等约束条件，采取dubins最短路径规划方式，设计出最短路径，具有实用性强，路径最优等特性。协作定位部分通过扩展卡尔曼滤波，利用uwb测距、惯性导航、陀螺仪等易于采集的进行信息融合，并且对传感器和存储空间的要求低，所测量数据极易获得，易于存储，处理方便，所以有低延时的优点，同时消除了位姿估计的不确定性，增加了网络的相对定位精度，保证系统在低数据量获取情况下的高定位精度，提高了定位效率。
在上述实施例的基础上，进一步地，所述装置还包括：
接收模块，用于接收每个随从车辆反馈的位姿信息；
判断模块，用于根据所述位姿信息判断每个随从车辆是否完成相应的目标位姿；
若判断获知存在任一随从车辆未完成相应的目标位姿，则进入下一个规划周期，所述路径规划模块，用于重新规划路径。
本发明实施例提供一种无人车网络编队的运动规划和协作定位装置，用于完成上述实施例中所述的方法，通过本实施例提供的装置完成上述实施例中所述的方法的具体步骤与上述实施例相同，此处不再赘述。
本发明实施例提供的无人车网络编队的运动规划和协作定位装置，采用分层式体系结构，设置领航车辆和随从车辆；领航车辆统一规划最优路径，将整体方案分配调度，随从车辆分布式自主控制，完成任务。路径规划部分考虑到实际情况中的最大曲率与速度限制等约束条件，采取dubins最短路径规划方式，设计出最短路径，具有实用性强，路径最优等特性。协作定位部分通过扩展卡尔曼滤波，利用uwb测距、惯性导航、陀螺仪等易于采集的进行信息融合，并且对传感器和存储空间的要求低，所测量数据极易获得，易于存储，处理方便，所以有低延时的优点，同时消除了位姿估计的不确定性，增加了网络的相对定位精度，保证系统在低数据量获取情况下的高定位精度，提高了定位效率。
在以上各实施例的基础上，进一步地，所述路径规划模块，具体用于：
将获取到的无人车网络编队中每一车辆的初始位姿、目标位姿和运动学参数输入至预设的优化模型，输出多种dubins路径；
选择所述多种dubins路径中最短的路径，作为路径规划。
本发明实施例提供一种无人车网络编队的运动规划和协作定位装置，用于完成上述实施例中所述的方法，通过本实施例提供的装置完成上述实施例中所述的方法的具体步骤与上述实施例相同，此处不再赘述。
本发明实施例提供的无人车网络编队的运动规划和协作定位装置，采用分层式体系结构，设置领航车辆和随从车辆；领航车辆统一规划最优路径，将整体方案分配调度，随从车辆分布式自主控制，完成任务。路径规划部分考虑到实际情况中的最大曲率与速度限制等约束条件，采取dubins最短路径规划方式，设计出最短路径，具有实用性强，路径最优等特性。协作定位部分通过扩展卡尔曼滤波，利用uwb测距、惯性导航、陀螺仪等易于采集的进行信息融合，并且对传感器和存储空间的要求低，所测量数据极易获得，易于存储，处理方便，所以有低延时的优点，同时消除了位姿估计的不确定性，增加了网络的相对定位精度，保证系统在低数据量获取情况下的高定位精度，提高了定位效率。
图8为依照本发明实施例的用于无人车网络编队的运动规划和协作定位的电子设备示意图，如图8所示，所述设备包括：处理器801、存储器802和总线803；
其中，处理器801和存储器802通过所述总线803完成相互间的通信；
处理器801用于调用存储器802中的程序指令，以执行上述各方法实施例所提供的方法，例如包括：在一个规划周期内，根据获取到的无人车网络编队中每一车辆的初始位姿、目标位姿和运动学参数，生成路径规划，所述无人车网络编队包括一个领航车辆和若干个随从车辆；将所述路径规划发送到每一随从车辆，以供每一随从车辆根据所述路径规划完成相应的目标位姿。
本发明实施例公开一种计算机程序产品，所述计算机程序产品包括存储在非暂态计算机可读存储介质上的计算机程序，所述计算机程序包括程序指令，当所述程序指令被计算机执行时，计算机能够执行上述各方法实施例所提供的方法，例如包括：在一个规划周期内，根据获取到的无人车网络编队中每一车辆的初始位姿、目标位姿和运动学参数，生成路径规划，所述无人车网络编队包括一个领航车辆和若干个随从车辆；将所述路径规划发送到每一随从车辆，以供每一随从车辆根据所述路径规划完成相应的目标位姿。
本发明实施例提供一种非暂态计算机可读存储介质，所述非暂态计算机可读存储介质存储计算机指令，所述计算机指令使所述计算机执行上述各方法实施例所提供的方法，例如包括：在一个规划周期内，根据获取到的无人车网络编队中每一车辆的初始位姿、目标位姿和运动学参数，生成路径规划，所述无人车网络编队包括一个领航车辆和若干个随从车辆；将所述路径规划发送到每一随从车辆，以供每一随从车辆根据所述路径规划完成相应的目标位姿。
本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于一计算机可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：rom、ram、磁碟或者光盘等各种可以存储程序代码的介质。
以上所描述的装置及设备等实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如rom/ram、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。