CN115826583A - 一种基于点云地图的自动驾驶车辆编队方法 - Google Patents

一种基于点云地图的自动驾驶车辆编队方法 Download PDF

Info

Publication number
CN115826583A
CN115826583A CN202211698266.9A CN202211698266A CN115826583A CN 115826583 A CN115826583 A CN 115826583A CN 202211698266 A CN202211698266 A CN 202211698266A CN 115826583 A CN115826583 A CN 115826583A
Authority
CN
China
Prior art keywords
vehicle
time
pilot
following
point cloud
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.)
Pending
Application number
CN202211698266.9A
Other languages
English (en)
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.)
Beihang University
Original Assignee
Beihang 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 Beihang University filed Critical Beihang University
Priority to CN202211698266.9A priority Critical patent/CN115826583A/zh
Publication of CN115826583A publication Critical patent/CN115826583A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明涉及一种基于点云地图的自动驾驶车辆编队方法，属于自动驾驶领域；包括以下步骤：针对测试的自动驾驶车辆，融合GNSS、IMU和LIDAR测量数据实时构建场景的点云地图；然后，从点云地图中获得自动驾驶车队中各车辆的运动状态信息，建立包括领航车和跟随车的平面编队运动学模型；根据目标点跟随策略设计跟随车的横纵向控制器，在离散时间下建立坡度变化路面上的车辆纵向编队的动力学模型，对领航车设计MPC，跟随车设计DMPC，仿真自动驾驶车辆上下坡路段的编队行驶结果。本发明考虑点云地图提供的地面坡度信息提高建模准确度，帮助车辆在进入上下坡路段前提前加减速，从而减小队列在上下坡中的速度波动，促使队列以更快的速度收敛到平衡状态。

Description

一种基于点云地图的自动驾驶车辆编队方法
技术领域
本发明属于自动驾驶领域，具体涉及一种基于点云地图的自动驾驶车辆编队方法。
背景技术
目前，计算机、自动化和通信等学科的高度发展，为创建智慧化、网联化的城市交通系统奠定了基础。智能交通系统(Intelligent Traffic System，ITS)是将先进的信息技术和通信技术有效地综合应用到交通运输管理体系中，从而通过协调各类交通参与者来达到提高交通运输效率，缓解交通阻塞，减少交通事故，降低能源消耗和减轻环境污染的目的。
ITS的主要组成部分是智能网联汽车和智能道路设施，智能网联汽车通常由自动驾驶汽车和通信设施构成，兼具单车智能性和多车协同性。自动驾驶汽车和基础设施上安装的传感器所采集的信息通过车间通信(Vehicle-to-Vehicle，V2V)，汽车与基础设施之间的通信(Vehicle-to-Infrastructure technology，V2I)实现信息交互，帮助汽车从宏观层面上理解当前的交通状况。
解决城市道路环境内交通拥堵问题的主要方法是对车道内行驶的车辆编队控制，车辆协同编队控制能够使车队维持较小的纵向间距行驶，形成稳定的队列，大幅提高道路通行能力。
点云地图可以为车辆提供更加丰富的环境信息和更加准确的定位信息，将点云地图引入到车辆编队控制中能够提升编队控制效果。
发明内容
本发明旨在设计一种基于点云地图的自动驾驶车辆编队方法，包括融合多传感器数据建立场景地图和基于点云地图信息实现编队控制两个内容，实现了在城市道路环境内高精定位以及编队行驶的目的。
所述的基于点云地图的自动驾驶车辆编队方法，具体步骤如下：
步骤一、针对测试的自动驾驶车辆，融合LIDAR、IMU和GNSS三种传感器的测量数据，得到场景的点云地图；
融合过程为：
首先，通过该车辆的前端里程计，对IMU传感器的测量数据进行优化；
优化结果为x：
Figure SMS_1
pwb为世界坐标系下的IMU位置向量，3×1维；vwb为世界坐标系下的IMU速度向量，3×1维；qwb为世界坐标系下的IMU姿态四元数，4×1维向量；ba为IMU中的加速度计的零偏，3×1维向量；bw为IMU中的陀螺仪的零偏，3×1维向量。
然后，利用后端对位姿矩阵
Figure SMS_2
进行优化。
接着，通过优化的位姿矩阵对各时刻LIDAR传感器采集的点云进行坐标变换，分别形成边特征点云集合Me和面特征点云集合Ms；
Figure SMS_3
Figure SMS_4
Figure SMS_5
表示第i-1帧LIDAR传感器采集的点云，
Figure SMS_6
是使用优化的位姿矩阵将
Figure SMS_7
变换到世界坐标下的边特征点云，
Figure SMS_8
是将
Figure SMS_9
变换到世界坐标下的面特征点云；
最后，将集合Me和集合Ms中的特征点云进行拼接，组成场景的点云地图M：
M＝{Me,Ms}
步骤二、从点云地图中获得自动驾驶车队中各车辆的运动状态信息，建立包括领航车和跟随车的平面编队运动学模型；
领航车的状态信息通过车间通信传递给跟随车，跟随车的控制输入量为车速和前轮转角。
领航车沿手动记录的路径以固定速度循迹行驶，跟随车在起始时刻与领航车的纵向距离大于期望间距，初始速度为0。
具体过程为：
步骤201、针对当前跟随车A，在该跟随车A中采用滑动窗口保存领航车的行驶路径离散点序列
Figure SMS_10
当接收到最新的领航车数据
Figure SMS_11
时，按时间先后顺序将数据压入滑动窗口中，将窗口内的离散点序列当作跟随车A横向跟踪控制器的跟踪路径；
步骤202、逐一计算该跟随车A与滑动窗口内各离散点的距离，找到距离跟随车A当前位置最小的离散点
Figure SMS_12
以点
Figure SMS_13
为基准，将该点
Figure SMS_14
之前的点从滑动窗口中移除，累加计算从离散点
Figure SMS_15
到离散点
Figure SMS_16
之间的距离，作为跟随车A至领航车的期望纵向间距；
步骤203、根据车辆的实际纵向间距与期望纵向间距之间的偏差作纵向PID控制，得到跟随车A期望的车速；
步骤204、同理，以离散点
Figure SMS_17
为基准，将该点
Figure SMS_18
之前的点从滑动窗口中移除，在离散点序列
Figure SMS_19
内查找横向跟踪控制器的目标点xG，并计算跟随车A期望的前轮转角；
步骤205、将跟随车A期望的车速与期望前轮转角整合后作用于该跟随车A。
步骤206、重复上述所有步骤，在循环中完成平面编队任务。
步骤三、在离散时间下建立车辆纵向编队的动力学模型，并改写成状态空间方程的形式；
具体为：
编号为i的车辆的纵向动力学模型为：
pi(t+1)＝pi(t)+vi(t)Δt
Figure SMS_20
Figure SMS_21
pi(t)表示编号为i的车辆在t时刻的位移，vi(t)表示编号为i的车辆在t时刻的速度，Δt表示离散时间间隔；mi表示编号为i的车辆质量，ηi表示编号为i的车辆的传动效率，Ri表示编号为i的车辆的轮胎滚动半径，Ti(t)表示编号为i的车辆在t时刻的实际驱动/制动力矩，τi表示编号为i的车的传动系惯性迟滞时间，ui(t)表示编号为i的车辆在t时刻的期望驱动/制动力矩，Fi(vi(t))表示编号为i的车辆行驶阻力；
令状态变量xi(t)＝[pi(t),vi(t),Ti(t)]T，把位移和速度作为输出变量yi(t)＝[pi(t)，vi(t)]T，将编号为i的车辆在离散时间下的纵向动力学模型写成状态空间方程的形式：
xi(t+1)＝φi（xi（t））+ψiui（t)
yi(t)＝γxi（t）
Figure SMS_22
Figure SMS_23
Figure SMS_24
步骤四、对领航车设计模型预测控制器MPC，跟随车设计分布式模型预测控制器DMPC，结合状态空间方程，仿真自动驾驶车辆在城市道路上下坡路段的编队行驶结果。
具体为：
对领航车设计控制器MPC，采用状态空间方程，根据预测时域内输出变量与期望输出变量的差异，构建领航车的代价函数：
Figure SMS_25
Np为总时刻；Q0和R0分别为领航车输出量与输入量的权重矩阵，
Figure SMS_26
表示领航车k时刻的预测输入，
Figure SMS_27
表示领航车k时刻的预测输出，ydes,0(k|t）表示领航车k时刻的期望输出，
Figure SMS_28
表示领航车k时刻的预测速度，
Figure SMS_29
表示抵消领航车
Figure SMS_30
匀速行驶阻力的驱动力矩。
跟随车采用控制器DMPC，目标函数不仅包含每辆车自身状态信息，还包括邻域内各车辆的状态信息，编号为i的跟随车在k时刻的代价函数为：
Figure SMS_31
Qi,Ri,Fi,Gi均为半正定的权重矩阵，
Figure SMS_33
为编号为i的跟随车k时刻的预测输出；ydes,i（k|t）为期望平衡状态输出；
Figure SMS_35
为编号为i的跟随车k时刻的期望输入力矩，
Figure SMS_39
为编号为i的跟随车在t时刻预测时域内的假设输出；
Figure SMS_34
为抵消编号为i的跟随车
Figure SMS_37
匀速行驶阻力产生的阻力矩，
Figure SMS_38
为编号为i的跟随车邻域集合
Figure SMS_40
的假设输出
Figure SMS_32
Figure SMS_36
为预测输出与假设输出的差值的期望值。
最后，通过李雅普诺夫稳定性分析，设置权重矩阵Fi和Gi，使得自动驾驶车辆队列渐近稳定。
本发明的优点在于：
(1)一种基于点云地图的自动驾驶车辆编队方法，定位算法的结果均来自于LIDAR、IMU和GNSS三种传感器测量数据的融合，当由于信号遮挡，GNSS定位出现跳变时，基于LIDAR的特征匹配仍能提供可靠的结果输出，改善了固定场景的定位精度，并且该算法可在价格低廉的Jeston TX2实时运行。
(2)一种基于点云地图的自动驾驶车辆编队方法，在不依赖GNSS测量的条件下，先验点云地图仍能为车辆编队提供可靠的定位信息；通过设计一种简单有效的平面编队控制策略，使得平面编队的横向跟随偏差和纵向车距偏差都在±0.3m范围内。
(3)一种基于点云地图的自动驾驶车辆编队方法，在纵向编队的模型预测控制中，考虑点云地图提供的地面坡度信息会提高建模准确度，帮助车辆在进入上下坡路段前提前加减速，从而减小队列在上下坡过程中的速度波动，促使队列以更快的速度收敛到平衡状态。
附图说明
图1为本发明一种基于点云地图的自动驾驶车辆编队方法的流程图；
图2为本发明多传感器融合建图前端里程计示意图。
图3为本发明多传感器融合建图后端优化示意图。
图4为本发明建立的测试场景点云地图。
图5为本发明平面编队控制器执行流程图。
图6为本发明平面编队实验中领航车与跟随车行驶路径对比。
图7为本发明纵向编队任务中队列的通信拓扑结构示意图。
图8为本发明坡度信息对领航车纵向速度的影响。
图9为本发明分布式模型预测控制思想。
具体实施方式
下面将结合附图和实例对本发明作进一步的详细说明；
本发明公开了一种封闭场景下的自动驾驶车辆编队控制方法，通过融合激光雷达和惯导数据，估计自动驾驶车辆的运动状态并进行队列间距优化控制；为了达到上述目的，具体使用包括：
(1)设计融合GNSS、IMU和LIDAR测量数据的实时建图算法，并在实验车搭载的Jeston TX2上运行该算法，验证建图和定位精度，并通过估计的位姿拼接特征点云得到实验场景的点云地图。
(2)根据目标点跟随策略设计平面编队任务中跟随车的横纵向控制器，并基于点云地图提供的定位信息进行平面编队的实车试验。
(3)建立坡度变化路面上的车辆纵向编队动力学模型，对领航车设计模型预测控制器，跟随车设计分布式模型预测控制器；通过Matlab仿真模拟了队列在上下坡时的相邻车间距变化。
所述的基于点云地图的自动驾驶车辆编队方法，如图1所示，具体步骤如下：
步骤一、针对测试的自动驾驶车辆，融合LIDAR、IMU和GNSS三种传感器的测量数据，得到场景的点云地图；
融合过程为：
首先，通过该车辆的前端里程计，对连续两个LIDAR帧之间的IMU传感器的测量数据的状态变量进行优化；
优化结果为x：
Figure SMS_41
pwb为世界坐标系下的IMU位置向量，3×1维；vwb为世界坐标系下的IMU速度向量，3×1维；qwb为世界坐标系下的IMU姿态四元数，4×1维向量；ba为IMU中的加速度计的零偏，3×1维向量；bw为IMU中的陀螺仪的零偏，3×1维向量。
然后，对帧图匹配的LIDAR里程计因子，GNSS测量因子和回环检测因子进行后端优化，优化变量为关键帧所对应的位姿
Figure SMS_42
关键帧所包含的数据包括测量时间戳，LIDAR传感器在该时刻采集的点云以及位姿
Figure SMS_43
其中点云是通过LIDAR传感器测量的，位姿根据优化过程求解得到。
接着，通过优化的位姿矩阵对各时刻LIDAR传感器采集的点云进行坐标变换，分别形成边特征点云集合Me和面特征点云集合Ms；
Figure SMS_44
Figure SMS_45
Figure SMS_46
表示第i-1帧LIDAR传感器采集的点云，
Figure SMS_47
是使用优化的位姿矩阵将
Figure SMS_48
变换到世界坐标下的边特征点云，
Figure SMS_49
是将
Figure SMS_50
变换到世界坐标下的面特征点云；
将当前时刻的特征点云
Figure SMS_51
与附近局部地图M的特征点配准的结果作为LIDAR里程计的测量。
最后，将集合Me和集合Ms中的特征点云进行拼接，组成场景的点云地图M：
M＝{Me,Ms}
步骤二、从点云地图中获得自动驾驶车队中各车辆的运动状态信息，建立包括领航车和跟随车的平面编队运动学模型；完成平面编队的任务。
领航车的状态信息通过车间通信传递给跟随车，跟随车的控制输入量为车速和前轮转角。
领航车沿手动记录的路径以固定速度循迹行驶，跟随车在起始时刻与领航车的纵向距离大于期望间距，初始速度为0。
具体过程为：
步骤201、针对当前跟随车A，在该跟随车A中采用滑动窗口数据结构保存领航车的行驶路径离散点序列
Figure SMS_52
当接收到最新的领航车数据
Figure SMS_53
时，按时间先后顺序将数据压入滑动窗口中，将窗口内的离散点序列当作跟随车A横向跟踪控制器的跟踪路径；
通过使用滑动窗口保存领航车发来的数据，这种方式可以节省保存牵扯数据所需的空间，并且需要领航车发送的数据量小，在工程实践中可以有效减低由于前后车的通信负担。
步骤202、逐一计算该跟随车A与滑动窗口内各离散点的距离，找到距离跟随车A当前位置最小的离散点
Figure SMS_54
以点
Figure SMS_55
为基准，将该点
Figure SMS_56
之前的点从滑动窗口中移除，累加计算从离散点
Figure SMS_57
到离散点
Figure SMS_58
序列内两点之间的距离，作为跟随车A至领航车的期望纵向间距；
步骤203、根据车辆的实际纵向间距与期望纵向间距之间的偏差作纵向PID控制，得到跟随车A期望的车速，计算当前车速下的前视距离ld；
步骤204、同理，以离散点
Figure SMS_59
为基准，将该点
Figure SMS_60
之前的点从滑动窗口中移除，在离散点序列
Figure SMS_61
内查找横向跟踪控制器的目标点xG，并计算跟随车A期望的前轮转角；
步骤205、将跟随车A期望的车速与期望前轮转角整合后作用于该跟随车A。
步骤206、重复上述所有步骤，在循环中完成平面编队任务。
步骤三、在离散时间下建立考虑和不考虑坡度阻力的两种车辆纵向编队的动力学模型，并改写成状态空间方程的形式；
具体为：
队列内编号为i的车辆在离散时间下的纵向动力学模型为：
pi(t+1)＝pi(t)+vi(t)Δt
Figure SMS_62
Figure SMS_63
pi(t)表示编号为i的车辆在t时刻的位移，vi(t)表示编号为i的车辆在t时刻的速度，Δt表示离散时间间隔；mi表示编号为i的车辆质量，ηi表示编号为i的车辆的传动效率，Ri表示编号为i的车辆的轮胎滚动半径，Ti(t)表示编号为i的车辆在t时刻的实际驱动/制动力矩，τi表示编号为i的车的传动系惯性迟滞时间，ui(t)表示编号为i的车辆在t时刻的期望驱动/制动力矩，Fi(vi(t))表示编号为i的车辆行驶阻力；
令状态变量xi(t)＝[pi(t),vi(t),Ti(t)]T，把位移和速度作为输出变量yi(t)＝[pi(t)，vi(t)]T，将编号为i的车辆在离散时间下的纵向动力学模型写成状态空间方程的形式：
xi(t+1)＝φii(xi(t))+ψiui(t)
yi(t)＝γxi(t)
Figure SMS_64
Figure SMS_65
Figure SMS_66
步骤四、对领航车设计模型预测控制器MPC，跟随车设计分布式模型预测控制器DMPC，结合状态空间方程，仿真自动驾驶车辆在城市道路上下坡路段的编队行驶结果。
具体为：
对领航车设计控制器MPC，采用状态空间方程，根据预测时域内输出变量与期望输出变量的差异，构建领航车的代价函数：
Figure SMS_67
Np为总时刻；Q0和R0分别为领航车输出量与输入量的权重矩阵，
Figure SMS_68
表示领航车k时刻的预测输入，
Figure SMS_69
表示领航车k时刻的预测输出，ydes,0(k|t)表示领航车k时刻的期望输出，
Figure SMS_70
表示领航车k时刻的预测速度，
Figure SMS_71
表示抵消领航车
Figure SMS_72
匀速行驶阻力的驱动力矩。
跟随车采用控制器DMPC，队列内每辆车都视为单独子系统，为每个单独子系统设计单独的模型预测控制器，目标函数不仅包含每辆车自身状态信息，还包括邻域内各车辆的状态信息；
由以下四部分组成：
(1)衡量节点i预测输出与平衡状态输出的偏差项
将编号为i的跟随车作为节点i，当队列内车辆都保持以期望的速度v0匀速运动，称队列到达了期望的平衡状态，节点i的平衡状态对应它的期望状态xdes,i(k|t)，可以通过预测时域内的预测输出
Figure SMS_73
与期望平衡状态输出ydes,i(k|t)偏差项记为
Figure SMS_74
Figure SMS_75
(2)衡量节点i预测输入与平衡状态输入的偏差项
当队列到达平衡状态时，节点i的期望输入力矩
Figure SMS_76
应当等于由行驶阻力产生的阻力矩
Figure SMS_77
在非平衡状态记预测输入与平衡状态输入的偏差项为
Figure SMS_78
Figure SMS_79
(3)衡量节点i预测输出与节点i假设输出的偏差项
节点i在t时刻预测时域内的假设输出
Figure SMS_80
由t-1时刻预测时域内的优化输出
Figure SMS_81
传递得到，预测时域内的假设输出
Figure SMS_82
与预测时域内的假设输入
Figure SMS_83
的关系为：
Figure SMS_84
Figure SMS_85
假设输入
Figure SMS_86
的前Np-1个元素，是在t时刻求解的最优控制量序列
Figure SMS_87
中后Np-1个元素沿时间轴向左平移一个单位得到的，
Figure SMS_88
的第Np个元素
Figure SMS_89
设置为终端约束：
Figure SMS_90
从节点i在预测时域内的预测输出
Figure SMS_91
应接近假设输出
Figure SMS_92
才能保证系统逐渐稳定，记预测输出与假设输出的偏差项
Figure SMS_93
Figure SMS_94
(4)衡量节点i预测输出与邻域集合
Figure SMS_95
内假设输出的偏差项
节点i接收节点
Figure SMS_96
的状态信息，故节点i的预测输出
Figure SMS_97
与其邻域集合
Figure SMS_98
的假设输出
Figure SMS_99
也应当尽可能接近：
Figure SMS_100
以上四项中，Qi,Ri,Fi,Gi均为半正定的权重矩阵，合并以上4项，则节点i在k时刻的代价函数为：
Figure SMS_101
Figure SMS_103
为编号为i的跟随车k时刻的预测输出；ydes,i(k|t)为期望平衡状态输出；
Figure SMS_107
为编号为i的跟随车k时刻的期望输入力矩，
Figure SMS_108
为编号为i的跟随车在t时刻预测时域内的假设输出；
Figure SMS_104
为抵消编号为i的跟随车
Figure SMS_106
匀速行驶阻力产生的阻力矩，
Figure SMS_109
为编号为i的跟随车邻域集合
Figure SMS_110
的假设输出
Figure SMS_102
Figure SMS_105
为预测输出与假设输出的差值的期望值。
最后，通过李雅普诺夫稳定性分析，设置权重矩阵Fi和Gi，使得自动驾驶车辆队列渐近稳定。
实施例：
本发明的平台是基于Ubuntu18.04系统下的ROS melodic搭建的。实验中将编写的功能函数封装成ROS节点，节点之间的数据传输通过消息通信机制实现，本发明中主要的节点为车辆定位节点和领航-跟随控制器节点；
具体算法为：
第一步，设计了一种基于因子图优化模型的多传感器融合建图算法，包括前端里程计和后端优化模块，并在校区内下测试该算法定位与建图效果。
0)LIDAR点云预处理
采用文献1中Zhang J提出的基于曲率提取特征点的方式，从原始点云中提取边特征点pe和面特征点。
对于16线激光雷达，分线束逐一计算每条线束上的所有点的曲率，记第k帧点云中第m根线束上第n个点为
Figure SMS_111
则点
Figure SMS_112
处的曲率定义为：
Figure SMS_113
式中，
Figure SMS_116
是点
Figure SMS_119
在水平方向上的邻域点集合，设定点
Figure SMS_123
沿第m根线束顺时针和逆时针各取5个点组成
Figure SMS_115
Figure SMS_120
表示
Figure SMS_122
内点的数量。对于打在比较平整的墙面上的
Figure SMS_124
计算得到的
Figure SMS_114
会比较小；而对于不连续的特征，
Figure SMS_125
会相对大。在实现中，为了避免边特征过于集中，当点
Figure SMS_126
被判定为边特征点时，邻域
Figure SMS_127
内的点便不再作为边特征点，面特征亦然。最终满足
Figure SMS_117
的点
Figure SMS_118
组成边特征点集Pe，
Figure SMS_121
的点组成面特征点集Ps。
1)前端里程计
前端里程计的作用是高频率低精度地估计LIDAR连续帧之间的车辆相对运动，为低频率高精度的后端优化提供优化初始值，并接受后端优化的结果用以对自身低精度结果插值，纠正里程计累计偏差。
本实施例设计的前端里程计算法如图2所示，该因子图中包含了连续帧之间的LIDAR里程计因子和IMU预积分因子。记图模型的优化状态变量为x：
Figure SMS_128
连续帧之间的LIDAR里程计因子通过配准连续两帧之间的边特征点和面特征点，计算连续帧之间的变换矩阵，特征提取及匹配的计算过程参考文献[2]实现；由于IMU数据频率高于LIDAR数据频率，在两个LIDAR帧之间，存在多帧IMU数据。为了使用IMU测量数据约束图模型，需要将IMU测量数据先进行积分得到IMU预积分因子。预积分因子的详细原理及推导可在文献[3]中找到。随着每一帧LIDAR数据的引入，前端里程计的优化变量会随时间增加，为了降低时间复杂度，前端里程计基于滑动窗口的思想进行非线性优化。
2)后端优化
前端里程计可以得到连续帧之间的变换矩阵，进而递推得到每帧所对应的优化变量，但是优化中滑动窗口算法的使用使得优化变量的迭代求解仅仅在固定长度内的窗口范围内进行，窗口之外的优化变量则保持原值。为了消除前端里程计的累计误差，后端优化中使用GNSS测量的绝对位置和闭环检测进行修正[4]，同时采用关键帧机制：当前帧与因子图中的次新帧间隔设定距离阈值10m以上时，才将当前帧加入到因子图中优化，次新帧与当前帧之间的雷达帧全部丢弃。如图3所示的后端优化模型中，包括了帧图匹配的LIDAR里程计因子，GNSS测量因子和回环检测因子，优化变量为关键帧所对应的位姿
Figure SMS_129
在图2中，计算LIDAR里程计因子是将第k+1帧的特征点与第k帧的特征点匹配，图3中则是将当前帧的特征点
Figure SMS_130
与附近局部地图M的特征点配准的结果作为LIDAR里程计的测量。M由边特征地图Me和面特征地图Ms组成，具体表述为：
Figure SMS_131
第一步的建图结果如图4所示，从图中可以看出建立的点云地图特征明显且闭合，并无重叠或明显畸变。在这一步中，本发明提出了一种新颖的多传感器数据融合方案，实验结果表明这种方法可以应用在校园等封闭环境内。
第二步，建立平面编队运动学模型，从实验车的实际可控制量出发分别设计横纵向控制策略和控制目标，横向采用纯追踪控制[5]，纵向采用PID控制，结合点云地图获得自身运动状态信息完成平面编队的任务，跟随车控制器的执行流程图如图5所示。
(1)在跟随车中采用滑动窗口数据结构保存领航车的行驶路径离散点序列
Figure SMS_132
当接收到最新的领航车数据
Figure SMS_133
时则将时间先后顺序压入滑动窗口中，这样窗口内的离散点序列可以当作跟随车横向跟踪控制器的跟踪路径；
(2)逐一计算跟随车与窗口内的路径点的距离，找到距离跟随车当前位置的路点
Figure SMS_134
以点
Figure SMS_135
为基准，将之前的点从窗口中移除，减少窗口长度，累加计算从
Figure SMS_136
到
Figure SMS_137
的离散点序列内两点之间距离得到跟随车至领航车的纵向间距；
(3)根据实际纵向间距与期望纵向间距之间的偏差作纵向PID控制得到跟随车期望的车速，计算当前车速下的前视距离ld；
(4)以
Figure SMS_138
为基准，在
Figure SMS_139
序列内查找横向跟踪控制器的目标点xG，并计算期望前轮转角，与期望车速信息整合后作用于跟随车。最后返回1)步，在循环中完成平面编队任务。
在平面编队的实验中，领航车沿手动记录的路径以固定速度循迹行驶，跟随车在起始时刻与领航车的纵向距离大于期望间距，初始速度为0。
图6显示了平面编队过程中领航车与跟随车的行驶路径对比，可以看出跟随车在直线和曲线路段都能够沿着领航车的路径行驶。该部分以滑动窗口的数据结构存储领航车的行驶路径，既降低了两车之间的通讯负担，又简化了跟随车控制量的计算复杂度，以一种简单有效的方式完成了跟随车对领航车行驶路径的跟随和纵向车距的保持。
第三步，从车辆纵向动力学模型出发，建立了分别考虑坡度阻力和不考虑坡度阻力的两种队列模型，对队列中的领航车设计模型预测控制器(Model Predictive Control,MPC)，跟随车设计分布式模型预测控制器(Distributed Model Predictive Control，DMPC)，通过Matlab仿真队列在上下坡路段的行驶结果。
1)纵向编队车辆动力学模型
根据文献[5]，汽车的三阶非线性纵向动力学模型被广泛用于纵向编队控制中。三阶非线性纵向动力学模型中假设：(1)车身左右两侧对称，将车辆简化成刚体；(2)队列在干燥路面上行驶，轮胎的纵向侧滑可以忽略不计；(3)汽车的传动系为一阶惯性延迟环节；由假设和牛顿动力学定律，队列内编号为i的车在离散时间下的纵向动力学模型为：
Figure SMS_140
Fi(vi(t))表示汽车行驶阻力，由滚动阻力、空气阻力和坡度阻力构成：
Fi(vi(t))＝migfcosα+CA,ivi(t)2+migsinα (4)
f表示道路阻力系数，CA,i表示空气阻力系数，α表示道路坡度角，g表示重力常量。
令状态变量xi(t)＝[pi(t),vi(t),Ti(t)]T，把位移和速度作为输出变量yi(t)＝[pi(t)，vi(t)]T，得到态空间方程的形式：
Figure SMS_141
2)队列通信拓扑结构
本发明中的通信借助V2V的方式实现，队列内的车辆通过无线网络传输各自的运动状态，通信拓扑结构采用领航跟随式如图7所示，队列内的每辆跟随车都可收到领航车的信息，通过领航车的状态计算跟随车的期望状态。采用含有N个节点的有向图模型
Figure SMS_142
中的有向边集合
Figure SMS_143
表征队列内的跟随车消息收发路径，节点集合VN表征队列内的跟随车。定义邻接矩阵
Figure SMS_144
用于描述跟随车之间的通信关系。
Figure SMS_145
其中：
Figure SMS_146
当aij=1时，存在由节点j指向节点i的有向边。定义节点i的跟随车输入邻域集合
Figure SMS_147
节点i只从
Figure SMS_148
中的节点接收信息，即有：
Figure SMS_149
类似地，定义节点i的输出邻域集合
Figure SMS_150
节点i向
Figure SMS_151
中的节点发送信息，有：
Figure SMS_152
3)队列控制目标
本发明重点探究在地图信息的基础上，队列队形的保持能力：随着时间的推移，相邻车辆的纵向间距逐渐收敛在期望值附近，相对纵向速度趋近于0。
Figure SMS_153
式(9)中，di-1,i表示相邻车辆期望的纵向间距，v0(t)表示期望的队列行进速度。
4)领航车控制器设计
若队列在坡度变化的路面上行驶，领航车的车速发生变化，则跟随车的期望车速也将发生变化，这种情况存在多辆跟随车时，后车加减速频繁，不利于队列稳定。这里对领航车设计MPC，预测模型采用式(5)，根据模型预测求解的预测时域内输出变量与期望输出变量的差异构建领航车的代价函数：
Figure SMS_154
在MATLAB中对设计控制器进行仿真，仿真条件为初始时刻t＝0s领航车以期望速度10m/s在平直路面上匀速行驶，经过10s后，到达坡度角为10度的上坡路段。图8对比了上坡过程中，预测模型中考虑坡度信息和不考虑坡度信息的领航车的速度控制效果。在t＝10s附近，由于存在上坡路段，两种模型下的纵向速度都有波动，区别是已知坡度的模型通过预测时域内的预测位移得知道路前方坡度变化，提前增大驱动力矩以克服坡度阻力，因此在上坡后领航车的速度很快恢复到10m/s。
5)跟随车控制器设计
分布式模型预测控制具备MPC的优点，能够将队列内相邻车的相对距离和相对速度约束显式地表现出来，从而在队列优化控制的过程中加入约束。队列中跟随车控制采用DMPC，队列内每辆车都视为单独子系统，为每个单独子系统设计单独的模型预测控制器，模型控制的目标函数不仅包含自身状态信息，还包括邻域内的状态信息，分布式模型预测控制思想如图9所示。设计的优化目标函数由以下四部分组成：
(1)衡量节点i预测输出与平衡状态输出的偏差项
当队列内车辆都保持以期望的速度v0匀速运动，称队列到达了期望的平衡状态，节点i的平衡状态对应它的期望状态xdes,i(k|t)，可以通过预测时域内的预测输出
Figure SMS_155
与期望平衡状态输出ydes,i(k|t)偏差项记为
Figure SMS_156
Figure SMS_157
(2)衡量节点i预测输入与平衡状态输入的偏差项
当队列到达平衡状态时，节点i的期望输入力矩
Figure SMS_158
应当等于由行驶阻力产生的阻力矩
Figure SMS_159
在非平衡状态记预测输入与平衡状态输入的偏差项为
Figure SMS_160
Figure SMS_161
(3)衡量节点i预测输出与节点i假设输出的偏差项
节点i在t时刻预测时域内的假设输出
Figure SMS_162
由t-1时刻预测时域内的优化输出
Figure SMS_163
传递得到，预测时域内的假设输出
Figure SMS_164
与预测时域内的假设输入
Figure SMS_165
的关系为：
Figure SMS_166
Figure SMS_167
假设输入
Figure SMS_168
的前Np-1个元素是在t时刻求解的最优控制量序列
Figure SMS_169
中后Np-1个元素沿时间轴向左平移一个单位得到的，
Figure SMS_170
的第Np个元素
Figure SMS_171
设置为终端约束：
Figure SMS_172
从节点i在预测时域内的预测输出
Figure SMS_173
应接近假设输出
Figure SMS_174
才能保证系统逐渐稳定，记预测输出与假设输出的偏差项
Figure SMS_175
Figure SMS_176
(4)衡量节点i预测输出与邻域集合
Figure SMS_177
内假设输出的偏差项
节点i接收节点
Figure SMS_178
的状态信息，故节点i的预测输出
Figure SMS_179
与其邻域集合
Figure SMS_180
的假设输出
Figure SMS_181
也应当尽可能接近：
Figure SMS_182
以上四项中，Qi,Ri,Fi,Gi均为半正定的权重矩阵，合并以上4项，则节点i在k时刻的代价函数为：
Figure SMS_183
通过李雅普诺夫稳定性分析，通过权重矩阵Fi和Gi的设置，可以使得队列具有渐近稳定的性质[5]。
通过给出了1辆领航车与4辆跟随车组成的队列在坡度变化工况下的仿真结果。编号0代表领航车，编号1～4代表跟随车，初始时刻t＝0s队列内相邻车辆纵向间距20m，以期望速度10m/s在平直路面上匀速行驶，经过10s后，到达坡度角为10度的路段，队列中的领航车开始上坡，随后跟随车逐辆上坡，在t＝18s附近(具体时间与控制效果有关)，最后一辆跟随车上坡，整个队列都在坡度角为10度的路段行驶，直到仿真结束。相比于不考虑坡度时队列内车辆最低速度降低到9.5m/s，考虑坡度的车辆队列速度变化曲线显然波动更小。
综上，多传感器融合建图时得到地图内行驶路面的坡度信息，建立地图内位置与坡度的映射关系表；根据地图提供的定位便可通过查表得到实时的坡度信息，从而将已知的坡度信息加入到预测模型中，进而提高编队控制的效果，改善道路通行能力。
参考文献
[1]Zhang J.,Singh S.Low-drift and real-time lidar odometry andmapping[J].Autonomous Robots,2017,41(2):401-416.
[2]Qin T.,Li P.,Shen S.Vins-mono:A robust and versatile monocularvisual-inertial state estimator[J].IEEE Transactions on Robotics,2018,34(4):1004-1020.
[3]Kim G.,Kim A.Scan context:Egocentric spatial descriptor for placerecognition within 3d point cloud map[C]//2018IEEE/RSJ InternationalConference on Intelligent Robots and Systems(IROS).IEEE,2018:4802-4809.
[4]Samuel M.,Hussein M.,Mohamad M.B.A review of some pure-pursuitbased path tracking techniques for control of autonomous vehicle[J].International Journal of Computer Applications,2016,135(1):35-38.
[5]Zheng Y.,Li S.E.,Li K.,et al.Platooning of connected vehicles withundirected topologies:Robustness analysis and distributed H-infinitycontroller synthesis[J].IEEE Transactions on Intelligent TransportationSystems,2017,19(5):1353-1364.。

Claims (4)

1.一种基于点云地图的自动驾驶车辆编队方法，其特征在于，具体步骤如下：
首先，针对测试的自动驾驶车辆，融合LIDAR、IMU和GNSS三种传感器的测量数据，通过估计的位姿拼接特征点云实时构建场景的点云地图；
融合过程为：
首先，通过该车辆的前端里程计，对IMU传感器的测量数据进行优化；
优化结果为x：
Figure FDA0004023034680000011
pwb为世界坐标系下的IMU位置向量，3×1维；vwb为世界坐标系下的IMU速度向量，3×1维；qwb为世界坐标系下的IMU姿态四元数，4×1维向量；ba为IMU中的加速度计的零偏，3×1维向量；bw为IMU中的陀螺仪的零偏，3×1维向量；
然后，利用后端对位姿矩阵
Figure FDA0004023034680000012
进行优化；
接着，通过优化的位姿矩阵对各时刻LIDAR传感器采集的点云进行坐标变换，分别形成边特征点云集合Me和面特征点云集合Ms；
Figure FDA0004023034680000013
Figure FDA0004023034680000014
Figure FDA0004023034680000015
表示第i-1帧LIDAR传感器采集的点云，
Figure FDA0004023034680000016
是使用优化的位姿矩阵将
Figure FDA0004023034680000017
变换到世界坐标下的边特征点云，
Figure FDA0004023034680000018
是将
Figure FDA0004023034680000019
变换到世界坐标下的面特征点云；
最后，将集合Me和集合Ms中的特征点云进行拼接，组成场景的点云地图M：
M＝{Me,Ms}
然后，从点云地图中获得自动驾驶车队中各车辆的运动状态信息，建立包括领航车和跟随车的平面编队运动学模型；根据目标点跟随策略设计平面编队任务中跟随车的横纵向控制器，领航车的状态信息通过车间通信传递给跟随车，跟随车的控制输入量为车速和前轮转角；
所述平面编队运动学模型的具体建立过程为：
步骤201、针对当前跟随车A，在该跟随车A中采用滑动窗口保存领航车的行驶路径离散点序列
Figure FDA00040230346800000110
当接收到最新的领航车数据
Figure FDA00040230346800000111
时，按时间先后顺序将数据压入滑动窗口中，将窗口内的离散点序列当作跟随车A横向跟踪控制器的跟踪路径；
步骤202、逐一计算该跟随车A与滑动窗口内各离散点的距离，找到距离跟随车A当前位置最小的离散点
Figure FDA00040230346800000112
以点
Figure FDA00040230346800000113
为基准，将该点
Figure FDA00040230346800000114
之前的点从滑动窗口中移除，累加计算从离散点
Figure FDA00040230346800000115
到离散点
Figure FDA00040230346800000116
之间的距离，作为跟随车A至领航车的期望纵向间距；
步骤203、根据车辆的实际纵向间距与期望纵向间距之间的偏差作纵向PID控制，得到跟随车A期望的车速；
步骤204、同理，以离散点
Figure FDA00040230346800000117
为基准，将该点
Figure FDA00040230346800000118
之前的点从滑动窗口中移除，在离散点序列
Figure FDA0004023034680000021
内查找横向跟踪控制器的目标点xG，并计算跟随车A期望的前轮转角；
步骤205、将跟随车A期望的车速与期望前轮转角整合后作用于该跟随车A；
步骤206、重复上述所有步骤，在循环中完成平面编队任务；
接着，在离散时间下建立坡度变化路面上的车辆纵向编队的动力学模型，并改写成状态空间方程的形式；
最后，对领航车设计模型预测控制器MPC，跟随车设计分布式模型预测控制器DMPC，结合状态空间方程，仿真自动驾驶车辆在城市道路上下坡路段的编队行驶结果。
2.如权利要求1所述的一种基于点云地图的自动驾驶车辆编队方法，其特征在于，所述领航车沿手动记录的路径以固定速度循迹行驶，跟随车在起始时刻与领航车的纵向距离大于期望间距，初始速度为0。
3.如权利要求1所述的一种基于点云地图的自动驾驶车辆编队方法，其特征在于，所述将车辆纵向编队的动力学模型改写成状态空间方程的形式，具体为：
编号为i的车辆的纵向动力学模型为：
pi(t+1)=pi(t)+vi(t)Δt
Figure FDA0004023034680000022
Figure FDA0004023034680000023
pi（t）表示编号为i的车辆在t时刻的位移，vi（t）表示编号为i的车辆在t时刻的速度，Δt表示离散时间间隔；mi表示编号为i的车辆质量，ηi表示编号为i的车辆的传动效率，Ri表示编号为i的车辆的轮胎滚动半径，Ti（t)表示编号为i的车辆在t时刻的实际驱动/制动力矩，τi表示编号为i的车的传动系惯性迟滞时间，ui(t)表示编号为i的车辆在t时刻的期望驱动/制动力矩，Fi(vi(t))表示编号为i的车辆行驶阻力；
令状态变量xi(t)＝[pi(t),vi(t),Ti(t)]T，
把位移和速度作为输出变量yi(t)=[pi(t)，vi(t)]T，将编号为i的车辆在离散时间下的纵向动力学模型写成状态空间方程的形式：
xi(t+1)=φi(xi(t))+ψiui(t)
yi(t)=γxi(D
Figure FDA0004023034680000031
Figure FDA0004023034680000032
Figure FDA0004023034680000033
4.如权利要求1所述的一种基于点云地图的自动驾驶车辆编队方法，其特征在于，所述对领航车设计MPC，跟随车设计DMPC，仿真编队行驶结果的过程，具体为：
对领航车设计控制器MPC，采用状态空间方程，根据预测时域内输出变量与期望输出变量的差异，构建领航车的代价函数：
Figure FDA0004023034680000034
Np为总时刻；Q0和R0分别为领航车输出量与输入量的权重矩阵，
Figure FDA0004023034680000035
表示领航车k时刻的预测输入，
Figure FDA0004023034680000036
表示领航车k时刻的预测输出，ydes,0(k|t)表示领航车k时刻的期望输出，
Figure FDA0004023034680000037
表示领航车k时刻的预测速度，
Figure FDA0004023034680000038
表示抵消领航车
Figure FDA0004023034680000039
匀速行驶阻力的驱动力矩；
跟随车采用控制器DMPC，目标函数不仅包含每辆车自身状态信息，还包括邻域内各车辆的状态信息，编号为i的跟随车在k时刻的代价函数为：
Figure FDA00040230346800000310
Qi,Ri,Fi,Gi均为半正定的权重矩阵，
Figure FDA00040230346800000311
为编号为i的跟随车k时刻的预测输出；ydes,i(k|t)为期望平衡状态输出；
Figure FDA00040230346800000312
为编号为i的跟随车k时刻的期望输入力矩，
Figure FDA00040230346800000313
为编号为i的跟随车在t时刻预测时域内的假设输出；
Figure FDA00040230346800000314
为抵消编号为i的跟随车
Figure FDA00040230346800000315
匀速行驶阻力产生的阻力矩，
Figure FDA00040230346800000316
为编号为i的跟随车邻域集合
Figure FDA00040230346800000317
的假设输出
Figure FDA00040230346800000318
Figure FDA00040230346800000319
为预测输出与假设输出的差值的期望值；
最后，通过李雅普诺夫稳定性分析，设置权重矩阵Fi和Gi，使得自动驾驶车辆队列渐近稳定。
CN202211698266.9A 2022-12-28 2022-12-28 一种基于点云地图的自动驾驶车辆编队方法 Pending CN115826583A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211698266.9A CN115826583A (zh) 2022-12-28 2022-12-28 一种基于点云地图的自动驾驶车辆编队方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211698266.9A CN115826583A (zh) 2022-12-28 2022-12-28 一种基于点云地图的自动驾驶车辆编队方法

Publications (1)

Publication Number Publication Date
CN115826583A true CN115826583A (zh) 2023-03-21

Family

ID=85518997

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211698266.9A Pending CN115826583A (zh) 2022-12-28 2022-12-28 一种基于点云地图的自动驾驶车辆编队方法

Country Status (1)

Country Link
CN (1) CN115826583A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116546067A (zh) * 2023-06-20 2023-08-04 广东工业大学 一种基于鸿蒙系统的车联网编队方法、系统及介质

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116546067A (zh) * 2023-06-20 2023-08-04 广东工业大学 一种基于鸿蒙系统的车联网编队方法、系统及介质
CN116546067B (zh) * 2023-06-20 2023-09-08 广东工业大学 一种基于鸿蒙系统的车联网编队方法、系统及介质

Similar Documents

Publication Publication Date Title
CN113945206B (zh) 一种基于多传感器融合的定位方法及装置
CN112083726B (zh) 一种面向园区自动驾驶的双滤波器融合定位系统
JP7036545B2 (ja) 能動的探索なしの強化学習に基づくオンライン学習法及び車両制御方法
CN112640417B (zh) 匹配关系确定方法及相关装置
CN112650220B (zh) 一种车辆自动驾驶方法、车载控制器及系统
EP4141736A1 (en) Lane tracking method and apparatus
US10829114B2 (en) Vehicle target tracking
CN110155081B (zh) 一种智能驾驶汽车的自适应避障控制系统
CN111860493A (zh) 一种基于点云数据的目标检测方法及装置
WO2014149044A1 (en) Smart cruise control system
US11975736B2 (en) Vehicle path planning
CN112099378B (zh) 考虑随机测量时滞的前车侧向运动状态实时估计方法
CN107600073A (zh) 一种基于多源信息融合的车辆质心侧偏角估计系统及方法
CN111257853B (zh) 一种基于imu预积分的自动驾驶系统激光雷达在线标定方法
CN115826583A (zh) 一种基于点云地图的自动驾驶车辆编队方法
CN112519777B (zh) 自动驾驶车队的控制方法、车载装置、车辆及系统
CN115303265A (zh) 车辆避障控制方法、装置及车辆
KR20220070467A (ko) 자동차의 폭방향 및 길이방향 안내 동안 데드타임 보상 기술
CN115525054B (zh) 大型工业园区无人清扫车沿边路径跟踪控制方法及系统
CN115042816A (zh) 路径跟踪方法、装置、设备、介质及程序
CN115675481A (zh) Gps增强摩擦估计
JP7234617B2 (ja) 車体姿勢角推定装置
CN113819914B (zh) 一种地图构建方法及装置
CN114074672A (zh) 辨识车辆的轮胎侧偏刚度的方法和相关装置
CN116331225B (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