CN107943071B

CN107943071B - 无人车的编队保持控制方法及系统

Info

Publication number: CN107943071B
Application number: CN201711072217.3A
Authority: CN
Inventors: 韩双双; 王迎春; 要婷婷; 王飞跃
Original assignee: Institute of Automation of Chinese Academy of Science
Current assignee: Qingdao Vehicle Intelligence Pioneers Inc
Priority date: 2017-11-03
Filing date: 2017-11-03
Publication date: 2020-02-07
Anticipated expiration: 2037-11-03
Also published as: CN107943071A

Abstract

本发明涉及无人车驾驶领域，具体涉及一种无人车的编队保持控制方法及系统，目的在于提供一种基于改进领航‑跟随法的无人车编队保持控制方法，并提高队形保持精度与乘坐舒适度。本发明提出的无人车的编队保持控制方法，包括：根据无人车编队队形、目标轨迹，规划出领航者下一时刻的行驶轨迹，并计算出跟随者下一时刻需要与领航者保持的目标距离和目标角度；设计改进的领航‑跟随算法，基于无人车运动学模型，根据领航者、跟随者当前的行驶状态参数，计算出跟随者下一时刻所需要的底层控制输入参数；根据所述底层控制输入参数，对跟随者进行底层控制，进而实现编队保持。在保证无人车编队的同时，也实现了安全舒适。

Description

无人车的编队保持控制方法及系统

技术领域

本发明涉及无人车驾驶领域，具体涉及一种无人车的编队保持控制方法及系统。

背景技术

机器人编队属于机器人的协同控制研究问题，通过形成特定队形在时间和空间上协作完成复杂的任务，提高了工作效率、各机器人在功能上互补，具有经济可靠、响应快速等优点，并且具有极为重要的现实意义。在危险环境作业、探测、军事、物流、娱乐等领域具有广阔的应用前景。

编队保持控制是无人车编队研究中重要的一个方面。无人车是一个非线性系统，关键技术包括环境感知、路径规划、智能决策、车辆的导航定位以及车辆的智能控制，控制系统作为智能车的最底层、最重要的环节，如何实现高精度的横向和纵向控制是无人车研究必须解决的重点问题，受到广大专家学者的重视。车辆控制主要是对转向、油门和制动器的控制。车辆横向动力学始于20世纪50年代，主要对车辆建立车辆动力学模型，并对车辆进行开环控制，研究车辆的时域和频域响应。对于车辆动力学模型，研究学者先后提出车辆“线性二自由度”、“线性三自由度”、“线性四自由度”以及其他自由度的研究。车辆的控制主要分为横向控制和纵向控制，在横向控制中出现许多的理论和方法，包括支持向量机方法、分阶控制方法、传统PID控制方法、智能控制方法包括模糊控制和神经网络控制方法。

目前国内在车辆的控制方式上，有基于“预瞄-跟随”的控制模型、跟随车道线的控制、滑模控制规律等一系列控制方法。“领航-跟随”算法多数应用于轮式机器人，还未应用到无人车的编队控制上。而且现有的“领航-跟随”技术未将编队保持方法和底层控制方法结合，如果直接应用到无人车上，无法实现精准保持和安全舒适。

发明内容

为了解决现有技术中的上述问题，本发明提出了一种无人车的编队保持控制方法及系统，实现了无人车编队队形的精准保持与控制。

本发明的一方面，提出一种无人车的编队保持控制方法，包括：

根据无人车编队队形、目标轨迹，规划出领航者下一时刻的行驶轨迹，并计算出跟随者下一时刻需要与领航者保持的目标距离和目标角度；

基于无人车运动学模型，根据领航者、跟随者当前的行驶状态参数，计算出跟随者下一时刻所需要的底层控制输入参数；

其中，

领航者当前的行驶状态参数，包括：领航者的位置坐标、角度、速度、角速度、前轮相对于车体的转向角；

跟随者当前的行驶状态参数，包括：位置坐标、角度；

所述底层控制输入参数，用于对跟随者进行底层控制，进而实现编队保持；包括：无人车速度、角速度、向心加速度、前轮相对于车体的转向角。

优选地，计算所述底层控制输入参数时，采用改进的领航-跟随算法模型：

其中：

ω₁(t)和ω₂(t)分别为领航者、跟随者在t时刻的角速度，单位是rad/s；d为无人车前后轮间的距离，单位是m；α和β为闭环控制中的比例系数；l_d代表领航者和跟随者质心间需要保持的所述目标距离，单位是m；

代表领航者和跟随者的质心连线与领航者行驶方向的夹角需要保持的所述目标角度，单位是rad；l(t)为领航者和跟随者间在t时刻的质心间的距离，单位是m；

为领航者和跟随者之间质心连线与领航者行驶方向在t时刻的夹角，单位是rad；v_r1(t)、v_r2(t)分别为领航者、跟随者的后轮轴中心点在t时刻的速度，单位是m/s；a₂(t)为跟随者的加速度，θ₂(t)为跟随者前轮相对于车体的转向角；ψ₁(t)、ψ₂(t)分别表示地面坐标系下领航者、跟随者车体纵向对称轴与X轴在t时刻的夹角，单位是rad。

优选地，所述底层控制包括：横向控制和纵向控制；

所述横向控制，包括对车辆方向盘转角的控制，从而使车辆沿着期望的路径行驶；

所述纵向控制，包括油门控制和制动控制，从而使车辆按照期望的速度行驶。

优选地，所述横向控制，基于遗传算法和模糊控制算法；

所述横向控制的输入参数包括：前轮相对于车体的转向角、角速度、向心加速度。

优选地，所述纵向控制，采用专家PID控制。

优选地，所述油门控制的专家规则包括：

期望速度输入值R_i、速度误差e_i、速度误差变化率Δe_i对PID控制参数的相应修正系数、以及控制输出值u_i的限幅。

优选地，所述制动控制的专家规则包括：

当v(k)<V₁时，车辆需要加速，设置u_brake＝0；

当V₁<|v(k)|<V₂时，车速需要保持，设置u_brake＝0；

当v(k)>V₂，且Δv(k)≤0时，需要提供较小的制动力，u_brake＝M₁；

当v(k)>V₂，且Δv(k)>0时，需要提供较大的制动力，设置u_brake＝M₂；

当v(k)>0，且期望车速为0时，尤其是车速较快时，需要紧急停车，设置最大制动力u_brake＝M₃；

其中，v(k)为车速；Δv(k)为车辆的加速度；u_brake为制动输出量；V₁、V₂分别为预设的最小和最大期望速度阈值，单位为m/s；M₁、M₂、M₃分别为第一制动力阈值、第二制动力阈值、最大制动力阈值，且M₁<M₂<M₃，单位为N，具体值由车辆实验测试得到。

本发明的另一方面，提出一种无人车的编队保持控制系统，基于上面所述无人车的编队保持控制方法；包括：上层规划决策子系统、车辆底层控制子系统；

所述上层规划决策子系统，配置为：根据无人车编队队形、目标轨迹计算领航者、跟随者下一时刻需要保持的目标距离和目标角度，进而计算出跟随者下一时刻所需要的底层控制输入参数；

所述车辆底层控制子系统，配置为：根据所述底层控制输入参数，对跟随者进行底层控制，进而实现编队保持。

优选地，所述上层规划决策子系统，包括：目标距离和目标角度计算单元、底层控制输入参数计算单元；

所述目标距离和目标角度计算单元：用于根据无人车编队队形、目标轨迹，计算出领航者、跟随者下一时刻需要保持的目标距离和目标角度；

所述底层控制输入参数计算单元：用于根据领航者、跟随者当前的行驶状态参数，以及所述目标距离和所述目标角度，基于无人车运动学模型计算出跟随者下一时刻所需要的底层控制输入参数。

优选地，所述车辆底层控制子系统，包括：横向控制器、纵向控制器；

所述横向控制器，用于车辆方向盘转角的控制，使车辆沿着期望的路径行驶；

所述纵向控制器，用于油门控制和制动控制，使车辆按照期望的速度行驶。

本发明的有益效果：

本发明中考虑机器人集群任务中常见的无人驾驶汽车编队保持控制方法。针对现有的领航-跟随算法多数应用于轮式机器人，还未应用到无人车的编队控制上。本发明结合无人车的特点以及其底层控制输入，设计改进的领航-跟随算法，将其输出的参数设计为无人车底层所需的时间函数，在保证编队的同时，实现无人车乘坐的安全舒适。针对编队保持控制方法，本发明针对现有技术未将编队保持方法和底层控制方法结合的问题，将无人车的编队保持和底层控制系统无缝衔接，同时结合无人车特点及领航-跟随算法，将无人车速度、角速度、向心加速度、前轮相对于车体的转向角作为无人车规划决策的输出，同时也是无人车底层控制的输入，从而达到车辆编队队形的精准保持与控制的目标。该研究前沿新颖，多无人车编队在多个领域具有广泛的应用，如军事应用、集成制造、交通运输、协助震后搜索与营救、清除危险区域等，因此多无人车系统协同控制的研究有着极其重要的战略与经济意义。

附图说明

图1是本发明无人车的编队保持控制方法实施例的流程示意图；

图2是本发明基于无人车改进的领航-跟随模型示意图；

图3是本发明无人车的编队保持控制系统实施例的构成示意图；

图4是本发明无人车的编队保持控制系统实施例的控制信号流向示意图。

具体实施方式

下面参照附图来描述本发明的优选实施方式。本领域技术人员应当理解的是，这些实施方式仅用于解释本发明的技术原理，并非旨在限制本发明的保护范围。

本发明结合无人车通信系统与改进的领航-跟随法，实现无人车分布式协同控制；基于所形成的编队与无人车路径信息，提出编队保持控制方法，通过对无人车横向和纵向移动的控制，完成编队的保持与变换、动态队形切换与避障等任务。

无人车编队控制方法综合运用无人驾驶、车辆控制、网络通信、资源分配等关键技术，实现无人车编队保持以及底层控制，保证无人车车队能按照安全、稳定、舒适的要求完成相应的任务。

本发明提出的无人车的编队保持控制方法的实施例，如图1所示，包括以下步骤：

步骤S1，根据无人车编队队形、目标轨迹，规划出领航者下一时刻的行驶轨迹，并计算出跟随者下一时刻需要与领航者保持的目标距离和目标角度；

步骤S2，基于无人车运动学模型，根据领航者、跟随者当前的行驶状态参数，计算出跟随者下一时刻所需要的底层控制输入参数。

具体地，领航者当前的行驶状态参数，包括：领航者的位置坐标、角度、速度、角速度、前轮相对于车体的转向角；跟随者当前的行驶状态参数，包括：位置坐标、角度；底层控制输入参数，包括：无人车速度、角速度、向心加速度、前轮相对于车体的转向角。底层控制输入参数用于对跟随者进行底层控制，进而实现编队保持；包括：无人车速度、角速度、向心加速度、前轮相对于车体的转向角。

下面介绍本实施例中，改进的领航-跟随模型的构建过程：

如图2所示，基于无人车改进的领航-跟随模型中，假设无人车的领航者和跟随者间的距离为l(t)，他们之间的角度为了保持队形，跟随者需要与领航者保持l(t)和

的值在允许范围内。因此，跟随者需要知道领航者的位置坐标(x₁,y₁)、角度ψ₁(t)、速度v_r1(t)、角速度

前轮相对于车体的转向角θ₁(t)，以及跟随着自身的位置(x₂,y₂)、角度ψ₂(t)。通过计算确定自身的速度v_r2(t)、角速度即航向偏差、向心加速度a₂(t)即横向加速度、前轮相对于车体的转向角θ₂(t)即横向偏差，来完成编队控制。改进的领航-跟随模型如公式(1)所示：

其中，

v_r1(t)、v_r2(t)分别为领航者、跟随者的后轮轴中心点在t时刻的速度，单位是m/s；ω₁(t)和ω₂(t)分别为领航者、跟随者在t时刻的角速度，单位是rad/s；d为无人车前后轮间的距离，单位是m；ψ₁(t)、ψ₂(t)分别表示地面坐标系下领航者、跟随者车体纵向对称轴与X轴在t时刻的夹角，单位是rad，且有

根据算法原理结合结构图的闭环特性可得公式(2)：

其中，α,β是闭环控制中的比例系数，

为队列中的要求目标，l_d代表领航者和跟随者质心间需要保持的目标距离，

代表领航者和跟随者之间质心连线与领航者行驶方向的夹角需要保持的目标角度，则控制的最终目标为：(l(t)-l_d)→0，

根据公式(1)和(2)可以得到公式(3)：

公式(3)中，

根据车辆动力学模型，得到公式(4)：

其中，l(t)为领航者和跟随者间在t时刻的质心间的距离，单位是m；

为领航者和跟随者之间质心连线与领航者行驶方向在t时刻的夹角，单位是rad；a₂(t)为跟随者的加速度，θ₂(t)为跟随者前轮相对于车体的转向角。

下面介绍本实施例中，无人车底层控制的实现方法：

底层控制是智能车的一个基础系统，所有上层规划的处理结果都需要依靠底层控制来实现，其主要负责智能车的运行控制，包括：转向、前进、后退、停车以及紧急避障。它接收由局部路径规划发送来的命令，然后依照这些命令执行一定的动作，从而控制智能车的运行。底层控制系统的功能和任务要求：与局部路径规划系统通过总线进行通信，通过电机驱动器来控制转向电机以及驱动电机，从而控制智能车的纵向和横向运动，达到无人驾驶的目的。

底层控制包括：横向控制和纵向控制。其中，横向控制包括对车辆方向盘转角的控制，从而使车辆沿着期望的路径行驶；纵向控制包括油门控制和制动控制，从而使车辆按照期望的速度行驶。

1)横向控制：

无人驾驶车辆的横向控制主要研究如何控制车辆沿上层决策所规划的道路准确无误的行驶，同时要保证车辆行驶的平稳性和安全性，并尽量满足舒适性的要求。无人车横向控制采用模糊控制。模糊控制可以融合人类驾驶的经验，并且对于不确定因素具有适应性，另外可以通过参数的优化，达到车辆的横向控制。

横向控制器的输入变量的选择对模糊控制系统的性能是非常关键的。无人车的横向控制是控制车辆相对于规划轨迹的位姿(横向偏差和航向偏差)的调整，因此，把车辆与行车路线的横向偏差和航向偏差作为模糊控制器的两个输入项。同时考虑车辆运动平稳性，添加车辆横向加速度(车辆绝对横向加速度的稳态值，即车辆的向心加速度)作为模糊控制器的一个输入，从而保证车辆运动的平稳性。

模糊控制器调整隶属度函数参数和控制规则，通常采用专家的经验或反复试探的方法，其实质是一个寻优的过程。因此采用遗传算法用于模糊控制器的参数优化。

本实施例中，横向控制是基于遗传算法和模糊控制算法。横向控制的输入参数包括：前轮相对于车体的转向角、角速度、向心加速度。

2)纵向控制：

无人驾驶车辆的纵向控制主要研究如何控制车辆纵向运动，即控制车辆按照期望的速度行驶，控制车辆间距离，控制车辆的纵向加速度，控制节气门的开度和制动器的制动量以及两者之间的切换。

传统上一般采用PID控制，其PID控制算法如公式(5)所示：

其中，u(t)是控制器系统的输出，e(t)是系统给定量与输出量的偏差，T_i，T_d分别为控制器积分时间常数和微分时间常数；K_p、K_i、K_d分别为控制器的比例系数、积分系数和微分系数。

当釆用增量式PID控制方式时，其算法的数学递推表达式如公式(6)所示：

Δu_i＝K_pΔe_i+K_ie_i+K_d(Δe_i-Δe_i-1) (6)

其中，Δu_i＝u_i-u_i-1，Δe_i＝e_i-e_i-1，Δe_i-1＝e_i-1-e_i-2；u_i、u_i-1分别代表i、i-1时刻的控制量；e_i、e_i-1、e_i-2分别代表i、i-1、i-2时刻系统给定量与输出量的偏差。

传统的PID控制和增量式PID控制在对象随时间空间发生变化或被干扰时，不容易达到原先预期的最佳的设定值跟踪效果和干扰量抑制，甚至容易产生振荡。根据无人车纵向控制的要求，采用在传统PID控制算法的基础上，结合专家控制系统的思想设计专家PID控制方法控制车辆的速度，建立速度控制PID系统的调参规则，从而解决车辆传动系统高度非线性和复杂的纵向干扰条件所带来的控制精度问题。

采用专家PID控制来实现车辆的自动驾驶，其目的主要是为了控制车辆的油门和制动，以使车辆能够舒适平稳的保持恒定速度或者进行快速跟踪期望速度。

本实施例中，纵向控制采用专家PID控制。

油门控制的主要目的是使车辆跟踪期望速度行驶，当车速低于期望速度时，控制器提供合适的加速度；当速度达到期望速度时，能提供稳定的动力来维持当前速度。

本实施例中，油门控制的专家规则包括：

制动控制的主要目的是使车辆减速和停车，当车速高于期望车速时，控制器将提供合适的加速度，使车辆减速，但此时不宜过大的制动力，但是在紧急停车时，为了避免碰撞，应该提供最大的制动力。

本实施例中，制动控制的专家规则包括：

(1)当v(k)<V₁时，车辆需要加速，设置u_brake＝0；

(2)当V₁<|v(k)|<V₂时，车速需要保持，设置u_brake＝0；

(3)当v(k)>V₂，且Δv(k)≤0时，车速过高并且车辆正在减速，需要提供较小的制动力，设置u_brake＝M₁；

(4)当v(k)>V₂，且Δv(k)>0时，车速过高并且加速度为正(如下坡行驶)，需要提供较大的制动力，设置u_brake＝M₂；

(5)当v(k)>0，且期望车速为0时，尤其是车速较快的情况下，需要紧急停车，设置最大制动力u_brake＝M₃。

以上所述的具体实施过程，对本发明的技术方案进行了详细说明。应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。例如：相关应用领域比如无人物流车编队、无人农用车编队、无人垃圾车编队等也包含在本发明的保护范围之内。

本发明的无人车的编队保持控制系统的实施例，基于上面所述无人车的编队保持控制方法；如图3所示的无人车的编队保持控制系统10，包括：上层规划决策子系统11、车辆底层控制子系统12；

上层规划决策子系统11，配置为：根据无人车编队队形、目标轨迹计算领航者、跟随者下一时刻需要保持的目标距离和目标角度，进而计算出跟随者下一时刻所需要的底层控制输入参数。

车辆底层控制子系统12，配置为：根据所述底层控制输入参数，对跟随者进行底层控制，进而实现编队保持。

具体地，上层规划决策子系统11包括：目标距离和目标角度计算单元111、底层控制输入参数计算单元112；车辆底层控制子系统12包括：横向控制器121、纵向控制器122。

其中：

目标距离和目标角度计算单元111用于根据无人车编队队形、目标轨迹，计算出领航者、跟随者下一时刻需要保持的目标距离和目标角度。

底层控制输入参数计算单元112用于根据领航者、跟随者当前的行驶状态参数，以及所述目标距离和所述目标角度，基于无人车运动学模型计算出跟随者下一时刻所需要的底层控制输入参数。

横向控制器121用于车辆方向盘转角的控制，使车辆沿着期望的路径行驶。

纵向控制器122用于油门控制和制动控制，使车辆按照期望的速度行驶。

本发明无人车的编队保持控制系统实施例的控制信号流向示意图，如图4所示。

上层规划决策时，跟随者根据前车状态及本车状态进行局部路径规划，而领航者根据本车感知进行局部路径规划。车辆底层控制子系统通过速度传感器采集车辆驱动电机信息，通过角度传感器采集车辆转向电机信息，并将上述行驶状态参数通过CAN总线发送到上层规划决策子系统；上层规划决策子系统根据改进的领航-跟随模型计算出无人车下一刻要保持队形所需要的底层控制输入参数，并通过CAN总线发送到车辆底层控制子系统。车辆底层控制子系统根据底层控制输入参数中的前轮相对于车体的转向角、角速度和向心加速度来控制转向电机进行方向盘角度的控制，根据输入的速度值控制驱动电机进行油门控制和制动控制。

本领域技术人员应该能够意识到，结合本文中所公开的实施例描述的各示例的方法步骤、子系统，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明电子硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以电子硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。本领域技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

至此，已经结合附图所示的优选实施方式描述了本发明的技术方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征做出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。