CN108664024B - 无人车网络编队的运动规划和协作定位方法及装置 - Google Patents

无人车网络编队的运动规划和协作定位方法及装置 Download PDF

Info

Publication number
CN108664024B
CN108664024B CN201810433350.5A CN201810433350A CN108664024B CN 108664024 B CN108664024 B CN 108664024B CN 201810433350 A CN201810433350 A CN 201810433350A CN 108664024 B CN108664024 B CN 108664024B
Authority
CN
China
Prior art keywords
vehicle
pose
network formation
planning
path
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201810433350.5A
Other languages
English (en)
Other versions
CN108664024A (zh
Inventor
沈渊
李潇翔
许璀杰
张子健
戈锋
智强
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Huahui Tongda Tianjin Technology Co ltd
Tsinghua University
Original Assignee
Huahui Tongda Tianjin Technology Co ltd
Tsinghua University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Huahui Tongda Tianjin Technology Co ltd, Tsinghua University filed Critical Huahui Tongda Tianjin Technology Co ltd
Priority to CN201810433350.5A priority Critical patent/CN108664024B/zh
Publication of CN108664024A publication Critical patent/CN108664024A/zh
Application granted granted Critical
Publication of CN108664024B publication Critical patent/CN108664024B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
    • G05D1/0291—Fleet control
    • G05D1/0295—Fleet control by at least one leading vehicle of the fleet

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明提供一种无人车网络编队的运动规划和协作定位方法及装置，所述方法包括：在一个规划周期内，根据获取到的无人车网络编队中每一车辆的初始位姿、目标位姿和运动学参数，生成路径规划，无人车网络编队包括一个领航车辆和若干个随从车辆；将路径规划发送到每一随从车辆，以供每一随从车辆根据路径规划完成相应的目标位姿。本发明提供的无人车网络编队的运动规划和协作定位方法及装置，采用分层式体系结构，设置领航车辆和随从车辆。路径规划部分考虑多种约束条件，采取Dubins最短路径规划方式，协作定位部分通过扩展卡尔曼滤波，利用UWB测距、惯性导航、陀螺仪等易于采集的进行信息融合，消除位姿估计的不确定性，提高了定位效率。

Description

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

Claims (7)

1.一种无人车网络编队的运动规划和协作定位方法，其特征在于，包括：
在一个规划周期内，根据获取到的无人车网络编队中每一车辆的初始位姿、目标位姿和运动学参数，生成路径规划，所述无人车网络编队包括一个领航车辆和若干个随从车辆；
将所述路径规划发送到每一随从车辆，以供每一随从车辆根据所述路径规划完成相应的目标位姿；
所述根据获取到的无人车网络编队中每一车辆的初始位姿、目标位姿和运动学参数，生成路径规划具体为：
将获取到的无人车网络编队中每一车辆的初始位姿、目标位资和运动学参数输入至预设的优化模型，输出多种Dubins路径；
选择所述多种Dubins路径中最短的路径，作为路径规划；
所述优化模型为：
minimize F
subjest to p*(n+1)＝f(p*(n),u*(n))
Figure FDA0002630646140000011
Figure FDA0002630646140000012
p*(0)&p*(N)
其中，F为代价函数，
Figure FDA0002630646140000013
Figure FDA0002630646140000014
为车辆左轮上的控制参数，
Figure FDA0002630646140000015
为车辆右轮上的控制参数，Umin为最小转弯曲率，Umax为最大转弯曲率，p*(0)为初始位姿，p*(N)为目标位姿。
2.根据权利要求1所述的方法，其特征在于，还包括：
接收每个随从车辆反馈的位姿信息；
根据所述位姿信息判断每个随从车辆是否完成相应的目标位姿；
若判断获知存在任一随从车辆未完成相应的目标位姿，则进入下一个规划周期，重新规划路径。
3.根据权利要求1所述的方法，其特征在于，所述初始位姿由随从车辆基于获取到的相对观测数据，利用扩展卡尔曼滤波算法得到。
4.一种无人车网络编队的运动规划和协作定位装置，其特征在于，包括：
路径规划模块，用于在一个规划周期内，根据获取到的无人车网络编队中每一车辆的初始位姿、目标位姿和运动学参数，生成路径规划，所述无人车网络编队包括一个领航车辆和若干个随从车辆；
发送模块，用于将所述路径规划发送到每一随从车辆，以供每一随从车辆根据所述路径规划完成相应的目标位姿；
所述根据获取到的无人车网络编队中每一车辆的初始位姿、目标位姿和运动学参数，生成路径规划具体为：
将获取到的无人车网络编队中每一车辆的初始位姿、目标位资和运动学参数输入至预设的优化模型，输出多种Dubins路径；
选择所述多种Dubins路径中最短的路径，作为路径规划；
所述优化模型为：
minimize F
subjest to p*(n+1)＝f(p*(n),u*(n))
Figure FDA0002630646140000021
Figure FDA0002630646140000022
p*(0)&p*(N)
其中，F为代价函数，
Figure FDA0002630646140000023
Figure FDA0002630646140000024
为车辆左轮上的控制参数，
Figure FDA0002630646140000025
为车辆右轮上的控制参数，Umin为最小转弯曲率，Umax为最大转弯曲率，p*(0)为初始位姿，p*(N)为目标位姿。
5.根据权利要求4所述的装置，其特征在于，还包括：
接收模块，用于接收每个随从车辆反馈的位姿信息；
判断模块，用于根据所述位姿信息判断每个随从车辆是否完成相应的目标位姿；
若判断获知存在任一随从车辆未完成相应的目标位姿，则进入下一个规划周期，所述路径规划模块，用于重新规划路径。
6.一种用于无人车网络编队的运动规划和协作定位的电子设备，其特征在于，包括：
存储器和处理器，所述处理器和所述存储器通过总线完成相互间的通信；所述存储器存储有可被所述处理器执行的程序指令，所述处理器调用所述程序指令能够执行如权利要求1至3任一所述的方法。
7.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至3任一所述的方法。
CN201810433350.5A 2018-05-08 2018-05-08 无人车网络编队的运动规划和协作定位方法及装置 Active CN108664024B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810433350.5A CN108664024B (zh) 2018-05-08 2018-05-08 无人车网络编队的运动规划和协作定位方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810433350.5A CN108664024B (zh) 2018-05-08 2018-05-08 无人车网络编队的运动规划和协作定位方法及装置

Publications (2)

Publication Number Publication Date
CN108664024A CN108664024A (zh) 2018-10-16
CN108664024B true CN108664024B (zh) 2020-09-25

Family

ID=63778768

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810433350.5A Active CN108664024B (zh) 2018-05-08 2018-05-08 无人车网络编队的运动规划和协作定位方法及装置

Country Status (1)

Country Link
CN (1) CN108664024B (zh)

Families Citing this family (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109445438B (zh) * 2018-12-05 2022-03-04 英华达（上海）科技有限公司 基于地图分享的巡航装置的巡航控制方法及系统
CN110085023A (zh) * 2019-04-10 2019-08-02 同济大学 一种编队行驶车队的车辆间相对位置测量系统和方法
CN110428603B (zh) * 2019-07-26 2021-04-23 北京主线科技有限公司 集装箱卡车编队中跟随车行驶控制方法及装置
CN111735466B (zh) * 2020-01-02 2021-11-12 北京京东乾石科技有限公司 一种多车协同轨迹规划方法、装置、设备及存储介质
CN113781762B (zh) * 2020-06-09 2022-09-13 大唐高鸿智联科技(重庆)有限公司 一种编队车辆的行驶方法、装置、设备及编队车辆
CN112833876B (zh) * 2020-12-30 2022-02-11 西南科技大学 一种融合里程计与uwb的多机器人协作定位方法
CN113003229B (zh) * 2021-02-26 2023-03-24 北京卫星制造厂有限公司 一种面向异构特性大型装备多智能体协同自主转运系统
CN113110441B (zh) * 2021-04-09 2022-11-18 江苏大学 基于超宽带的农用无人车集群作业方法
CN113147764B (zh) * 2021-06-01 2022-08-16 吉林大学 基于协同式自适应巡航系统混合势函数的车辆控制方法
CN113467449B (zh) * 2021-06-30 2023-05-30 深圳市海柔创新科技有限公司 车队控制方法、装置、电子设备和存储介质
CN115016523B (zh) * 2022-08-03 2022-12-13 西安羚控电子科技有限公司 集群装置控制系统、控制方法、集群装置及存储介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101685309A (zh) * 2008-09-24 2010-03-31 中国科学院自动化研究所 多机器人协调队形控制方法
CN104238554A (zh) * 2013-06-14 2014-12-24 江南大学 一种无线传感网移动体编队控制方法
CN106094835A (zh) * 2016-08-01 2016-11-09 西北工业大学 前轮驱动车式移动机器人的动态编队控制方法
KR20180025611A (ko) * 2016-09-01 2018-03-09 국민대학교산학협력단 안전제한타원을 이용한 차량의 자율주행시스템 및 이를 이용한 자율주행방법
CN107860388A (zh) * 2017-10-27 2018-03-30 哈尔滨工程大学 一种基于混合拓扑结构的多机器人协同导航定位算法
CN107943071A (zh) * 2017-11-03 2018-04-20 中国科学院自动化研究所 无人车的编队保持控制方法及系统

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101408433B (zh) * 2008-11-12 2012-07-18 深圳市凯立德欣软件技术有限公司 车队导航系统、领航导航装置、从导航装置及其导航方法
US9669828B2 (en) * 2012-06-01 2017-06-06 Toyota Motor Engineering & Manufacturing North America, Inc. Cooperative driving and collision avoidance by distributed receding horizon control
CN103901887B (zh) * 2014-03-04 2017-05-24 重庆邮电大学 一种基于改进粒子群算法的多移动机器人编队控制方法
CN104616541A (zh) * 2015-02-03 2015-05-13 吉林大学 基于鱼群效应的无信号交叉口车车协同控制系统
CN105527960A (zh) * 2015-12-18 2016-04-27 燕山大学 一种基于领航跟随的移动机器人编队控制方法
CN107798861B (zh) * 2017-11-30 2021-03-02 湖北汽车工业学院 一种车辆协作式编队行驶方法及系统

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101685309A (zh) * 2008-09-24 2010-03-31 中国科学院自动化研究所 多机器人协调队形控制方法
CN104238554A (zh) * 2013-06-14 2014-12-24 江南大学 一种无线传感网移动体编队控制方法
CN106094835A (zh) * 2016-08-01 2016-11-09 西北工业大学 前轮驱动车式移动机器人的动态编队控制方法
KR20180025611A (ko) * 2016-09-01 2018-03-09 국민대학교산학협력단 안전제한타원을 이용한 차량의 자율주행시스템 및 이를 이용한 자율주행방법
CN107860388A (zh) * 2017-10-27 2018-03-30 哈尔滨工程大学 一种基于混合拓扑结构的多机器人协同导航定位算法
CN107943071A (zh) * 2017-11-03 2018-04-20 中国科学院自动化研究所 无人车的编队保持控制方法及系统

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Decentralized overlapping tracking control of a formation of autonomous unmanned vehicles;Srdjan S. Stankovic 等;《2009 American Control Conference》;20090710;第3878-3883页 *
多机器人的路径规划关键技术及其挑战;陈超毅 等;《第三十四届中国控制会议论文集（E卷）》;20150731;第7171-7174页 *

Also Published As

Publication number Publication date
CN108664024A (zh) 2018-10-16

Similar Documents

Publication Publication Date Title
CN108664024B (zh) 无人车网络编队的运动规划和协作定位方法及装置
CN108227735B (zh) 基于视觉飞行自稳定的方法、计算机可读介质和系统
CN109885080B (zh) 自主控制系统及自主控制方法
Yang et al. Optimal path following for small fixed-wing UAVs under wind disturbances
JP6855524B2 (ja) 低速特徴からのメトリック表現の教師なし学習
CN104764457B (zh) 一种用于无人车的城市环境构图方法
Oh et al. Decentralised standoff tracking of moving targets using adaptive sliding mode control for UAVs
Leishman et al. Relative navigation approach for vision-based aerial GPS-denied navigation
Hasberg et al. Simultaneous localization and mapping for path-constrained motion
Dai et al. Path planning and state estimation for unmanned aerial vehicles in hostile environments
CN109375643A (zh) 基于领航-跟随式三角形编队的多四旋翼对峙跟踪目标制导律
Marin et al. Event-based localization in ackermann steering limited resource mobile robots
JP2020530569A (ja) 車両センサの較正及び位置特定
CN113183975B (zh) 自动驾驶车辆的控制方法、装置、设备以及存储介质
KR20170071443A (ko) 군집 로봇의 행위 기반 분산 제어 시스템 및 방법
Burchett Unscented Kalman filters for range-only cooperative localization of swarms of munitions in three-dimensional flight
CN114771551A (zh) 自动驾驶车辆轨迹规划方法、装置和自动驾驶车辆
Zhou et al. Toward autonomy of micro aerial vehicles in unknown and global positioning system denied environments
Cristofalo et al. Vision-based control for fast 3-d reconstruction with an aerial robot
CN111076724A (zh) 三维激光定位方法及系统
Choi et al. Multi-UAV trajectory optimization utilizing a NURBS-based terrain model for an aerial imaging mission
Fu et al. Collision-free and kinematically feasible path planning along a reference path for autonomous vehicle
CN116295342A (zh) 一种用于飞行器勘测的多传感状态估计器
CN109459769A (zh) 一种自主定位方法与系统
CN112747752A (zh) 基于激光里程计的车辆定位方法、装置、设备和存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant