一种基于信息物理网的车队编队控制装置及编队控制方法

技术领域

本发明属于车联网导航控制领域，是一种具备车载信息物理网络的车队编队 控制装置及编队控制方法。

背景技术

随着汽车工业的迅猛发展，人民收入的不断提高，越来越多的人拥有了自己 的汽车。与此同时，组队自驾游、商务出巡等多车辆组队行驶的情况越来越多。 我们将这类行驶方式统称之为编队行驶。编队行驶可以有效的整合资源，最大 限度利用编队中的车辆。随着计算机技术和无线通信技术的飞速发展，多辆智能 车组成的群体系统通过协调、合作来完成原本由单个智能车无法或难以完成的工 作已经成为可能。智能车编队控制主要是指多个智能车在群体运动过程中，能够 在克服环境限制的同时还保持着期望队形，最终全体都能够顺利到达指定目的地 的控制技术。由若干辆车组成的一列线性车队以较小车间距保持某种队形行驶， 能够很好的扩充现有道路容量，降低车辆拥堵情况，提高实际交通道路利用率， 增强交通的畅通性及安全性。

中国专利CN1975802公开了一种机动车辆编队行驶系统的控制方法，在两 个以上成员组成的车队中，各成员的车载导航系统通过GPRS技术相互通讯构成 一个网络，该网络的软件控制由以下四个机制组成：编队创建机制：以某一成员 为队长，当有另一成员向队长发送加入编队的申请时，队长可做出接受、拒绝两 种选择，如队长做出接受选择，则创建编队并取得对编队的管理权限；编队成员 位置报告机制：编队成员周期性向队长报告自己的位置信息，并由队长组织后发 送到下一个编队成员；编队消息发送机制：编队成员可以选择发送公告消息或者 与某一个成员私语；编队终止机制：当队长设定的编队时间到时终止或者由队长 主动终止。该专利实现编队成员之间的通讯联络，编队成员可以选择公告方式或 者私语方式，方便地将某一消息发送给全体队员或者与某一特定队员进行点对点 通讯。但该专利并不能智能地控制车队编队的行进。

中国专利CN101408433公开了一种车队导航系统、领航导航装置、从导航 装置及其导航方法。队导航系统包括领航导航装置，用于接收始发地和目的地信 息，生成并发出导航路径；接收卫星导航信号，计算并发出当前位置坐标；从导 航装置用于接收并显示所述领航导航装置发出的导航路径；接收所述领航导航装 置发出的当前位置坐标，据此显示领航导航装置的当前位置。本发明还提供了一 种领航导航装置、从导航装置及其导航方法。领航导航装置将最佳路径发往从导 航装置，使得从导航装置只需配置移动通信功能和显示功能便可实现导航功能， 无需为车队中的每台车辆都配置全功能的导航装置，降低了车队整体的导航成 本。但该专利需要驾驶人员人工控制车速等车辆行进因素，无法智能地进行整个 车队编队的智能行进。

中国专利CN102256207公开了一种车队导航方法。主车导航设备和各从车 导航设备分别在后台服务器上以具有唯一性的标识相区别；主车导航设备以无线 方式向后台服务器发送包括当前位置、目标位置和所属标识的信息；后台服务器 根据接收的主车导航设备的当前位置和目标位置信息，制定导航路线并将通过无 线方式发送至主车导航设备；从车导航设备以无线方式向后台服务器发送包括当 前位置和所属标识的信息；后台服务器将各从车的当前位置信息以无线方式发送 至主车导航设备；后台服务器以无线方式发送包括主车导航设备位置和与唯一性 的标识相对应的从车导航设备的位置信息至从车导航设备。但该专利需要驾驶人 员人工控制车速等车辆行进因素，无法智能地进行整个车队编队的智能行进。

国内外学者对编队控制作了大量前沿的研究，并取得了一定的研究成果主要 包括：基于行为法、人工势场法、虚拟结构法、领航跟随法等。

基于行为的控制方法主要是通过对车辆基本行为以及局部控制规则的设计 使得车辆群体产生所需的整体行为，这些行为包括碰撞避免、障碍物回避、驶向 目标、形成队形、保持队形和变换队形等。这种方法很容易根据当前具体情况选 择合适的控制器，但是无法明确地指出达到整体行为的局部控制规则，队形控制 的稳定性很难得到保证。

人工势场法主要是通过设计人工势场和势场函数来表示环境以及队形中各 辆车之间的约束关系，并以此为基础进行分析和控制。基本思想是智能车在一个 虚拟的力场中运动，障碍物被斥力势场包围，其产生的排斥力随智能车与障碍物 距离的减少而迅速增大；目标点被引力势场包围，其产生的吸引力随智能车与目 标点的接近而减少；在合力的作用下智能车沿最小势能的方向运动。这种编队方 法缺点是势场函数设计比较困难，而且存在局部极值点的问题。

虚拟结构法是将智能车编队队形想象成一个刚体的虚拟结构，该结构上固定 的各点对应各智能车的位置，智能车根据刚体上相应固定点的移动来调节自己的 运动即可形成指定队形。这种方法要求队形按照刚体的虚拟结构运动，这种集中 控制方式使得多智能车系统的运动缺乏灵活性和适应性。

领航跟随法是将队形中的某些智能车作为领航者，其他智能车为跟随者，将 队形控制问题变换为跟随者跟踪领航者位置和方向的问题。领航跟随法的优点是 队形运动完全由领航者轨迹确定，控制简单便于实现，即使领航者受到干扰也不 会影响整个队形，可以把编队控制问题简化为独立的跟踪问题，每个智能车只需 要获得它的领航机器人的状态信息，从而大大简化了队形间的合作问题；缺点在 于领航者和跟随者的控制相对独立，领航者很难得到跟随者的跟踪误差反馈，会 损失全局最优性。

除了上述的四种编队控制方法以外，车辆编队还有MPC模型预测控制、基 于图论的队形控制，神经网络编队控制一些方法，这三种方法控制比较复杂，实 现比较困难。

目前国内的车辆编队的研究多在控制模型和控制策略等理论研究，并未提出 一种可行的应用到实际当中的车辆编队控制行驶的方法。

因此，可以实现智能车实时自主编队行驶且可以回避障碍物的车队编队时本 领域迫切解决的技术问题。

发明内容

本发明公开了一种基于信息物理网的车队编队控制装置及控制方法，可以实 现避开障碍物的智能车实时自主编队行驶。

第一方面，本发明公开了一种基于信息物理网的车队编队控制装置，其中， 所述车队包括领航车辆(R_i)和跟随车辆(R_j)，设在车队中的每辆车(R_i、R_j) 上的所述车队智能避障装置包括车外传感器模块、GPS定位模块、无线通信模块 和中央处理单元模块。

车外传感器模块包括用于检测前方障碍物的红外传感器和用于检测车行驶 速度的速度传感器。

GPS定位模块安装在每辆车(R_i、R_j)前端中间位置用于实时采集所述车 的位置信息且同步时间。

所述无线通信模块包括无线网卡和无线路由器用于与所述车队中的其他车 辆建立网络连接。

用于实现通信和应用协议的所述中央处理单元模块通过所述无线通信模块 接收车辆的速度、位置信息并处理，其中，当红外传感器检测前方存在障碍物， 所述中央处理单元模块通过公式(F-1)计算获得跟随车辆(R_j)的线速度(v_j) 和角速度(ω_j)，公式(F-1)如下：

其中，

γ_ij＝θ_i+ψ_ij-θ_j、γ_kj＝θ_k-θ_j、α₁、α₂为常数，l_ij为领航车辆(R_i)和跟随车辆(R_j)之间的相对距离，d表示跟随 车辆(R_j)质心到轴心的距离，其中定义虚拟车辆R_k，它以恒定线速度v_k相切 于障碍物的方向θ_k沿着障碍物运动，即θ_k的运动方向v_k始终垂直于R_j与虚拟车 辆R_k的连线δ_jk，δ_jk表示跟随车辆(R_j)与障碍物间的最近距离，θ_k为虚拟车 辆R_k相切于障碍物的方向，θ_i、θ_j分别是领航车辆(R_i)和跟随车辆(R_j)的 角度，v_i、v_j分别是领航车辆(R_i)和跟随车辆(R_j)的线速度，ψ_ij和l_ij分别 表示领航车辆(R_i)和跟随车辆(R_j)间相对角度和距离；跟随车辆(R_j)通 过CAN总线将中央处理单元模块获得的线速度(v_j)和角速度(ω_j)发送到车 辆控制器进行调整跟随车辆(R_j)的线速度(v_j)和角速度(ω_j)从而确保车 队在编队的情况下进行避障。

当红外传感器检测前方不存在障碍物，所述中央处理单元模块通过公式 (F-2)计算获得领航车辆(R_i)根据参考轨迹所需控制的线速度(v_i)和角速 度(ω_i)，公式(F-2)如下：

$\left(\begin{array}{c}{v}_i\\ {\omega }_i\end{array}\right)=\left(\begin{array}{c}{y}_{\mathrm{ie}}{\omega }_i+v_r\cos {\theta }_{\mathrm{ie}}+(ϵ{\left|s_1\right|}^a\frac{s_1}{\left|s_1\right|+\sigma }+k_2{s}_1)y_{\mathrm{ie}}^2+ϵ{\left|s_1\right|}^a\frac{s_1}{\left|s_1\right|+\sigma }+k_2{s}_1\\ \frac{{\omega }_r+\frac{\partial \alpha }{{\partial v}_r}{\stackrel{.}{v}}_r+\frac{\partial \alpha }{{\partial y}_{\mathrm{ie}}}\left(v_r\sin {\theta }_{\mathrm{ie}}\right)+ϵ{\left|s_2\right|}^a\frac{s_2}{\left|s_2\right|+\sigma }+k_3{s}_2}{1+\frac{\partial \alpha }{{\partial y}_{\mathrm{ie}}}{x}_{\mathrm{ie}}}\end{array}\right),$其中， v_r、ω_r分别为参考轨迹的线速度和角速度，ε、k₂、k₃均为常数，0＜a＜1， 位姿误差e_i＝[x_ie，y_ie，θ_ie]^T收敛到零，$s=\left(\begin{array}{c}{s}_1\\ s_2\end{array}\right)=\left(\begin{array}{c}{x}_{\mathrm{ie}}\\ {\theta }_{\mathrm{ie}}+\arctan \left(v_r{y}_{\mathrm{ie}}\right)\end{array}\right),$

所述中央处理单元模块通过公式(F-3)计算获得跟随车辆(R_j)根据参考 轨迹所需控制的线速度((v_j)和角速度(ω_j)，公式(F-3)如下：

$\left(\begin{array}{c}{v}_j\\ {\omega }_j\end{array}\right)=\left(\begin{array}{c}{v}_i\cos e_{j3}+k_4{e}_{j1}\\ {\omega }_i+(v_i+k_v)k_5{e}_{j2}+(v_i+k_v)k_6\sin e_{j3}\end{array}\right)+\left(\begin{array}{c}{\gamma }_{\mathrm{vj}}\\ {\gamma }_{\mathrm{\omega j}}\end{array}\right)$

γ_vj＝-ω_il_ijdsin(ψ_ijd+e_j3)、${\gamma }_{\mathrm{\omega j}}=-\frac{\left|e_{j2}\right|({\omega }_i(d_j+l_{\mathrm{ijd}})+k_6(v_i+k_v)d_j+k_v)}{1/k_5+\left|e_{j2}\right|d_j}$

其中k₄、k₅、k₆、k_v均为正实数，ψ_ijd和l_ijd分别表示领航车辆(R_i)和 跟随车辆(R_j)期望的角度和车间距，

位姿误差$e_j=\left(\begin{array}{c}{e}_{j1}\\ e_{j2}\\ e_{j3}\end{array}\right)=\left(\begin{array}{c}{l}_{\mathrm{ijd}}\cos ({\psi }_{\mathrm{ijd}}+e_{j3})-l_{\mathrm{ij}}\cos ({\psi }_{\mathrm{ij}}+e_{j3})\\ l_{\mathrm{ijd}}\sin ({\psi }_{\mathrm{ijd}}+e_{j3})-l_{\mathrm{ij}}\sin ({\psi }_{\mathrm{ij}}+e_{j3})\\ {\theta }_i-{\theta }_j\end{array}\right).$

优选地，车外传感器模块在车辆R_i、R_j左前侧和右前侧分别安装红外传感 器用于检测前方障碍物，在车轮轴上安装速度传感器用于检测车辆R_i、R_j行驶 的速度。

优选地，GPS定位模块含有GPS天线及转接线，其供电电压为5V。

优选地，所述无线通信模块采用FPV5.8G200MW发送接收套装，其供电 电压均为12V。

优选地，所述中央处理单元模块为嵌入式DSP处理单元。

第二方面，本发明公开了一种基于信息物理网的车队编队控制方法。

第一步骤中，将所述车队编队，其中所述车队包括领航车辆R_i和跟随车辆 R_j，设在车队中的每辆车R_i、R_j上的所述车队智能避障装置包括车外传感器模 块、GPS定位模块、无线通信模块和中央处理单元模块。

车外传感器模块包括用于检测前方障碍物的红外传感器和用于检测车行驶 速度的速度传感器。

GPS定位模块安装在每辆车R_i、R_j前端中间位置用于实时采集所述车的位 置信息且同步时间。

所述无线通信模块包括无线网卡和无线路由器用于与所述车队中的其他车 辆建立网络连接。

用于实现通信和应用协议的所述中央处理单元模块通过无线通信模块接收 车辆的速度、位置信息并处理。

第二步骤中，红外传感器(5)检测前方是否存在障碍物，如果存在障碍物， 执行第三步骤，如果不存在障碍物，执行第四步骤；

第三步骤中，所述中央处理单元模块(4)建立坐标系，并通过公式(F-1) 计算获得跟随车辆(R_j)的线速度(v_j)和角速度(ω_j)，公式(F-1)如下：

$\left(\begin{array}{c}{v}_j\\ {\omega }_j\end{array}\right)=\left(\begin{array}{c}\frac{{\stackrel{·}{l}}_{\mathrm{ij}}\cos {\gamma }_{\mathrm{kj}}+{\stackrel{·}{\delta }}_{\mathrm{jk}}\sin {\gamma }_{\mathrm{ij}}+v_i\cos {\psi }_{\mathrm{ij}}\cos {\gamma }_{\mathrm{kj}}}{\cos ({\gamma }_{\mathrm{kj}}-{\gamma }_{\mathrm{ij}})}\\ \frac{{\stackrel{·}{l}}_{\mathrm{ij}}\sin {\gamma }_{\mathrm{kj}}-{\stackrel{·}{\delta }}_{\mathrm{jk}}\cos {\gamma }_{\mathrm{ij}}+v_i\cos {\psi }_{\mathrm{ij}}\sin {\gamma }_{\mathrm{kj}}}{d\cos ({\gamma }_{\mathrm{kj}}-{\gamma }_{\mathrm{ij}})}\end{array}\right)---(F-1),$

其中，γ_ij＝θ_i+ψ_ij-θ_j、γ_kj＝θ_k-θ_j、α₁、α₂为常数，l_ij为领航车辆(R_i)和跟随车辆(R_j)之间的相对距离，d表 示跟随车辆(R_j)质心到轴心的距离，其中定义虚拟车辆R_k，它以恒定线速度v_k相切于障碍物的方向θ_k沿着障碍物运动，即θ_k的运动方向v_k始终垂直于R_j与虚 拟车辆R_k的连线δ_jk，δ_jk表示跟随车辆(R_j)与障碍物间的最近距离，θ_k为虚 拟车辆R_k相切于障碍物的方向，θ_i、θ_j分别是领航车辆(R_i)和跟随车辆(R_j) 的角度，v_i、v_j分别是领航车辆(R_i)和跟随车辆(R_j)的线速度，ψ_ij和l_ij分 别表示领航车辆(R_i)和跟随车辆(R_j)间相对角度和距离，跟随车辆(R_j) 通过CAN总线将中央处理单元模块(4)获得的线速度(v_j)和角速度(ω_j) 发送到车辆控制器(7)进行调整跟随车辆(R_j)的线速度(v_j)和角速度(ω_j) 从而确保车队在编队的情况下进行避障；

第四步骤中，所述中央处理单元模块(4)通过公式(F-2)计算获得领航车 辆(R_i)根据参考轨迹所需控制的线速度(v_i)和角速度(ω_i)，公式(F-2) 如下：

$\left(\begin{array}{c}{v}_i\\ {\omega }_i\end{array}\right)=\left(\begin{array}{c}{y}_{\mathrm{ie}}{\omega }_i+v_r\cos {\theta }_{\mathrm{ie}}+(ϵ{\left|s_1\right|}^a\frac{s_1}{\left|s_1\right|+\sigma }+k_2{s}_1)y_{\mathrm{ie}}^2+ϵ{\left|s_1\right|}^a\frac{s_1}{\left|s_1\right|+\sigma }+k_2{s}_1\\ \frac{{\omega }_r+\frac{\partial \alpha }{\partial v_r}{\stackrel{·}{v}}_r+\frac{\partial \alpha }{\partial y_{\mathrm{ie}}}\left(v_r\sin {\theta }_{\mathrm{ie}}\right)+ϵ{\left|s_2\right|}^a\frac{s_2}{\left|s_2\right|+\sigma }+k_3{s}_2}{1+\frac{\partial \alpha }{\partial y_{\mathrm{ie}}}{x}_{\mathrm{ie}}}\end{array}\right)---(F-2),$

其中，v_r、ω_r分别为参考轨迹的线速度和角速度，ε、k₂、k₃均为正常数， 0＜a＜1，位姿误差e_i＝[x_ie，y_ie，θ_ie]^T收敛到零，$s=\left(\begin{array}{c}{s}_1\\ s_2\end{array}\right)=\left(\begin{array}{c}{x}_{\mathrm{ie}}\\ {\theta }_{\mathrm{ie}}+\arctan \left(v_r{y}_{\mathrm{ie}}\right)\end{array}\right),$

所述中央处理单元模块(4)通过公式(F-3)计算获得跟随车辆(R_j)根 据参考轨迹所需控制的线速度((v_j)和角速度(ω_j)，公式(F-3)如下：

$\left(\begin{array}{c}{v}_j\\ {\omega }_j\end{array}\right)=\left(\begin{array}{c}{v}_i\cos e_{j3}+k_4{e}_{j1}\\ {\omega }_i+(v_i+k_v)k_5{e}_{j2}+(v_i+k_v)k_6\sin e_{j3}\end{array}\right)+\left(\begin{array}{c}{\gamma }_{\mathrm{vj}}\\ {\gamma }_{\mathrm{\omega j}}\end{array}\right)---(F-3)$

γ_vj＝-ω_il_ijdsin(ψ_ijd+e_j3)、${\gamma }_{\mathrm{\omega j}}=-\frac{\left|e_{j2}\right|({\omega }_i(d_j+l_{\mathrm{ijd}})+k_6(v_i+k_v)d_j+k_v)}{1/k_5+\left|e_{j2}\right|d_j}$

其中，k₄、k₅、k₆、k_v均为正实数，ψ_ijd和l_ijd分别表示领航车辆(R_i)和 跟随车辆(R_j)期望的角度和车间距，

位姿误差$e_j=\left(\begin{array}{c}{e}_{j1}\\ e_{j2}\\ e_{j3}\end{array}\right)=\left(\begin{array}{c}{l}_{\mathrm{ijd}}\cos ({\psi }_{\mathrm{ijd}}+e_{j3})-l_{\mathrm{ij}}\cos ({\psi }_{\mathrm{ij}}+e_{j3})\\ l_{\mathrm{ijd}}\sin ({\psi }_{\mathrm{ijd}}+e_{j3})-l_{\mathrm{ij}}\sin ({\psi }_{\mathrm{ij}}+e_{j3})\\ {\theta }_i-{\theta }_j\end{array}\right).$

本发明公开的基于信息物理网的车队编队控制装置及控制方法考虑实际道 路障碍物情况，增强了车辆编队的避障能力，使得车队保持相对稳定行驶。本发 明可以取得如下有益效果：

将编队控制问题简化为独立的跟踪问题，简化了队形间的合作问题，解决了 对通信质量要求高的缺点；

在智能车编队保持相对稳定行驶的前提下，考虑实际道路障碍物情况，增强 了车辆编队的避障能力；

车辆间通信部分借助于物理信息网络，是交通运输和网络领域的一场伟大技 术变革，可以提高交通安全，缓解交通拥塞，提高行驶效率和舒适度。而车联网 技术对智能车编队行驶提供了保障。

附图说明

图1是根据本发明实施例的基于信息物理网的车队编队控制装置的控制整 体框图；

图2是根据本发明实施例的基于信息物理网的车队编队控制装置的示意性 避障位置关系坐标图；

图3是根据本发明实施例的基于信息物理网的车队编队控制装置的示意性 编队位置关系坐标图；

图4是根据本发明实施例的基于信息物理网的车队编队控制方法的流程 图。

以下结合附图和实施例对本发明作进一步的解释。

具体实施方式

本发明的实施例描述了一种基于信息物理网的车队编队控制装置，其中，所 述车队包括领航车辆R_i和跟随车辆R_j。根据如图1所示的基于信息物理网的车 队编队控制装置的控制整体框图，设在车队中的每辆车R_i、R_j上的所述车队编 队控制装置包括车外传感器模块1、GPS定位模块2、无线通信模块3和中央处 理单元模块4。

车外传感器模块1包括用于检测前方障碍物的红外传感器5和用于检测车行 驶速度的速度传感器6。

GPS定位模块2安装在每辆车R_i、R_j前端中间位置用于实时采集所述车的 位置信息且同步时间。

所述无线通信模块3包括无线网卡和无线路由器用于与所述车队中的其他 车辆建立网络连接。

用于实现通信和应用协议的所述中央处理单元模块4通过无线通信模块3 接收车辆的速度、位置信息并处理，当红外传感器5检测前方存在障碍物，所述 中央处理单元模块4通过公式F-1计算获得跟随车辆R_j的线速度v_j和角速度ω_j。

如图2所示的是根据本发明实施例的基于信息物理网的车队编队控制装置 的示意性避障位置关系坐标图，建立平面坐标系{O，X，Y}，其中X轴代表东向 位置，Y轴代表北向位置，(x_i，y_i，θ_i，v_i，ω_i)和(x_j，y_j，θ_j，v_j，ω_j)分别表示领航车辆R_i和跟随车辆R_j的位置、角度、线速度和角速度，ψ_ij和l_ij分别表示领航车辆R_i和 跟随车辆R_j间相对角度和距离，d表示车质心到轴心的距离，δ表示跟随车辆 与障碍物间的最近距离。我们定义一个虚拟车辆R_k，它以恒定线速度v_k相切于 障碍物的方向θ_k沿着障碍物运动，即θ_k的运动方向v_k始终垂直于R_j与虚拟车辆 R_k的连线δ。

根据领航车辆R_i、跟随车辆R_j和虚拟车辆R_k建立速度关系公式

$\left(\begin{array}{c}{\stackrel{·}{l}}_{\mathrm{ij}}=v_j\cos {\gamma }_{\mathrm{ij}}-v_i\cos {\psi }_{\mathrm{ij}}+d{\omega }_j\sin {\gamma }_{\mathrm{ij}}\\ \stackrel{·}{\delta }=v_j\cos {\gamma }_{\mathrm{kj}}-v_k\cos {\psi }_{\mathrm{kj}}+d{\omega }_j\sin {\gamma }_{\mathrm{kj}}\end{array}\right)$(公式1)

其中γ_ij＝θ_i+ψ_ij-θ_j，γ’_kj＝θ_k+ψ_kj-θ_j.由于v_k方向始终垂直于δ_jk方向，即 故(公式1)可以写成

$\left(\begin{array}{c}{\stackrel{·}{l}}_{\mathrm{ij}}=v_j\cos {\gamma }_{\mathrm{ij}}-v_i\cos {\psi }_{\mathrm{ij}}+d{\omega }_j\sin {\gamma }_{\mathrm{ij}}\\ \stackrel{·}{\delta }=v_{j}s{\mathrm{in\gamma }}_{\mathrm{kj}}-d{\omega }_j\cos {\gamma }_{\mathrm{kj}}\end{array}\right)$(公式2)

其中γ_kj＝θ_k-θ_j。由(公式2)可得跟随车辆R_j的线速度、角速度，即公 式F-1如下：

$\left(\begin{array}{c}{v}_j\\ {\omega }_j\end{array}\right)=\left(\begin{array}{c}\frac{{\stackrel{·}{l}}_{\mathrm{ij}}\cos {\gamma }_{\mathrm{kj}}+{\stackrel{·}{\delta }}_{\mathrm{jk}}\sin {\gamma }_{\mathrm{ij}}+v_i\cos {\psi }_{\mathrm{ij}}\cos {\gamma }_{\mathrm{kj}}}{\cos ({\gamma }_{\mathrm{kj}}-{\gamma }_{\mathrm{ij}})}\\ \frac{{\stackrel{·}{l}}_{\mathrm{ij}}\sin {\gamma }_{\mathrm{kj}}-{\stackrel{·}{\delta }}_{\mathrm{jk}}\cos {\gamma }_{\mathrm{ij}}+v_i\cos {\psi }_{\mathrm{ij}}\sin {\gamma }_{\mathrm{kj}}}{d\cos ({\gamma }_{\mathrm{kj}}-{\gamma }_{\mathrm{ij}})}\end{array}\right),$其中，

γ_ij＝θ_i+ψ_ij-θ_j、ψ_kj＝θ_k-θ_j、${\stackrel{·}{l}}_{\mathrm{ij}}={\alpha }_1(l_{\mathrm{ij}}^d-l_{\mathrm{ij}}),{\stackrel{·}{\delta }}_{\mathrm{jk}}={\alpha }_2({\delta }_{\mathrm{jk}}^d-{\delta }_{\mathrm{jk}}),$α₁、α₂为常数，l_ij为领航车辆R_i和跟随车辆R_j之间的相对距离，d表示跟随车辆R_j质 心到轴心的距离，其中定义虚拟车辆R_k，它以恒定线速度v_k相切于障碍物的方 向θ_k沿着障碍物运动，即θ_k的运动方向v_k始终垂直于R_j与虚拟车辆R_k的连线 δ_jk，δ_jk表示跟随车辆R_j与障碍物间的最近距离，θ_k为虚拟车辆R_k相切于障碍 物的方向，θ_i、θ_j分别是领航车辆R_i和跟随车辆R_j的角度，v_i、v_j分别是领航 车辆R_i和跟随车辆R_j的线速度，ψ_ij和l_ij分别表示领航车辆R_i和跟随车辆R_j间相 对角度和距离。

跟随车辆R_j通过CAN总线8将中央处理单元模块1获得的线速度v_j和角速 度ω_j发送到车辆控制器7进行调整跟随车辆R_j的线速度v_j和角速度ω_j从而确 保车队在编队的情况下进行避障。

如图3所示的根据本发明实施例的基于信息物理网的车队编队控制装置的 示意性编队位置关系坐标图，当红外传感器5检测前方不存在障碍物，建立领航 车辆R_i的运动学模型由以下微分方程描述

${\stackrel{·}{q}}_i=\left(\begin{array}{c}{x}_i\\ y_i\\ {\theta }_i\end{array}\right)=\left(\begin{array}{cc}\cos {\theta }_i& d\sin {\theta }_i\\ \sin {\theta }_i& -d\cos {\theta }_i\\ 0& 1\end{array}\right)\left(\begin{array}{c}{v}_i\\ {\omega }_i\end{array}\right)$(公式4)

领航车辆R_i在平面内的位姿误差为

$e_i=T(q_r-q_i)=\left(\begin{array}{c}{x}_{\mathrm{ie}}\\ y_{\mathrm{ie}}\\ {\theta }_{\mathrm{ie}}\end{array}\right)=\left(\begin{array}{ccc}\cos {\theta }_i& \sin {\theta }_i& 0\\ -\sin {\theta }_i& \cos {\theta }_i& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}{x}_r-x_i\\ y_r-y_i\\ {\theta }_r-{\theta }_i\end{array}\right)$(公式5)

${\stackrel{·}{x}}_r=v_r\cos {\theta }_r,{\stackrel{·}{y}}_r=v_r\sin {\theta }_r,{\stackrel{·}{\theta }}_r={\omega }_r$

q_r＝[x_r，y_r，θ_r]^T为虚拟四轮车在参考轨迹上某一点的位置坐标和航向；v_r、ω_r分别为参考轨迹的线速度和角速度。其位姿误差微分方程为

${\stackrel{·}{e}}_i=\left(\begin{array}{c}{\stackrel{·}{x}}_{\mathrm{ie}}\\ {\stackrel{·}{y}}_{\mathrm{ie}}\\ {\stackrel{·}{\theta }}_{\mathrm{ie}}\end{array}\right)=\left(\begin{array}{c}{\omega }_i{y}_{\mathrm{ie}}-v_i+v_r\cos {\theta }_{\mathrm{ie}}\\ -{\omega }_i{x}_{\mathrm{ie}}+v_r\sin {\theta }_{\mathrm{ie}}\\ {\omega }_r-{\omega }_i\end{array}\right)$(公式6)

设计了变结构控制的切换函数为

$s=\left(\begin{array}{c}{s}_1\\ s_2\end{array}\right)=\left(\begin{array}{c}{x}_{\mathrm{ie}}\\ {\theta }_{\mathrm{ie}}+\arctan \left(v_r{y}_{\mathrm{ie}}\right)\end{array}\right)$(公式7)

由于此滑动模态是渐进稳定的，因而在滑动运动下系统最终将实现位姿误差 e_i＝[x_ie，y_ie，θ_ie]^T收敛到零。要想使系统存在滑动模态，必须满足到达条件 采用幂次趋近律

$\stackrel{·}{s}=-ϵ{\left|s\right|}^a\mathrm{sgn}\left(s\right)-k_{1}s$(公式8)

其中，ε＞0，k₁＞0，0＜a＜1，-ε|s|^asgn(s)保证在有限时间内全局到达 的。假设初始值s(0)＞0，由(8)式可得

$s^{1-a}=C{\exp }^{-(1-a)\mathrm{kt}}-\frac{ϵ}{k_1}$(公式9)

其中那么从滑动模态s(0)到s＝0可得到可达时间为

$t=\frac{1}{k_1(1-a)}\ln [1+\frac{k_1{\left|s\left(0\right)\right|}^{1-a}}{ϵ}]$(公式10)

为了削弱抖振问题，符号函数可用下面连续函数代替得

$\left(\begin{array}{cc}\mathrm{sgn}\left(s_i\right)=\frac{s_i}{\left|s_i\right|+\sigma }& (i=\mathrm{1,2})\end{array}\right)$(公式11)

令α＝arctan(v_ry_ie)，则

$\frac{\partial \alpha }{\partial v_r}=\frac{y_{\mathrm{ie}}}{1+{\left(v_r{y}_{\mathrm{ie}}\right)}^2}$(公式12)

$\frac{\partial \alpha }{\partial y_{\mathrm{ie}}}=\frac{v_r}{1+{\left(v_r{y}_{\mathrm{ie}}\right)}^2}$

$\stackrel{·}{s}=\left(\begin{array}{c}{\omega }_i{y}_{\mathrm{ie}}-v_i+v_r\cos {\theta }_{\mathrm{ie}}\\ {\omega }_r-{\omega }_i+\frac{\partial \alpha }{\partial v_r}{\stackrel{·}{v}}_r+\frac{\partial \alpha }{\partial y_{\mathrm{ie}}}(-{\omega }_i{x}_{\mathrm{ie}}+v_r\sin {\theta }_{\mathrm{ie}})\end{array}\right)$(公式13)

假设s₁→0，s₂→0，所述中央处理单元模块4通过公式(F-2)计算获得 领航车辆(R_i)根据参考轨迹所需控制的线速度(v_i)和角速度(ω_i)，公式(F-2) 如下：

$\left(\begin{array}{c}{v}_i\\ {\omega }_i\end{array}\right)=\left(\begin{array}{c}{y}_{\mathrm{ie}}{\omega }_i+v_r\cos {\theta }_{\mathrm{ie}}+(ϵ{\left|s_1\right|}^a\frac{s_1}{\left|s_1\right|+\sigma }+k_2{s}_1)y_{\mathrm{ie}}^2+ϵ\frac{s_1}{\left|s_1\right|+\sigma }+k_2{s}_1\\ \frac{{\omega }_r+\frac{\partial \alpha }{\partial v_r}{\stackrel{·}{v}}_r+\frac{\partial \alpha }{\partial y_{\mathrm{ie}}}\left(v_r\sin {\theta }_{\mathrm{ie}}\right)+ϵ{\left|s_2\right|}^a\frac{s_2}{\left|s_2\right|+\sigma }+k_3{s}_2}{1+\frac{\partial \alpha }{\partial y_{\mathrm{ie}}}{x}_{\mathrm{ie}}}\end{array}\right)$(F-2)，其中，

v_r、ω_r分别为参考轨迹的线速度和角速度，ε、k₂、k₃均为正常数， 0＜a＜1，

位姿误差e_i＝[x_ie，y_ie，θ_ie]^T收敛到零，$s=\left(\begin{array}{c}{s}_1\\ s_2\end{array}\right)=\begin{array}{}\end{array}\left(\begin{array}{c}{x}_{\mathrm{ie}}\\ {\theta }_{\mathrm{ie}}+\arctan \left(v_r{y}_{\mathrm{ie}}\right)\end{array}\right),$

考虑如下Lyapunov函数

$V_s=\frac{1}{2}{s}^{T}s$(公式15)

对Lyapunov函数求导得：

$\left(\begin{array}{c}{\stackrel{·}{V}}_s=x_{\mathrm{ie}}({\omega }_i{y}_{\mathrm{ie}}-v_i+v_r\cos {\theta }_{\mathrm{ie}})+({\theta }_{\mathrm{ie}}+\arctan \left(v_r{y}_{\mathrm{ie}}\right))({\stackrel{·}{\theta }}_{\mathrm{ie}}+\frac{\partial \alpha }{\partial v_r}{\stackrel{·}{v}}_r+\frac{\partial \alpha }{\partial y_{\mathrm{ie}}}{\stackrel{·}{y}}_{\mathrm{ie}})\\ =-k_2{x}_{\mathrm{ie}}^2-k_2{x}_{\mathrm{ie}}^2{y}_{\mathrm{ie}}^2-{({\theta }_{\mathrm{ie}}+\arctan \left(v_r{y}_{\mathrm{ie}}\right))}^2\le 0\end{array}\right)$

(公式16)

因此，切换函数能够在有限的时间内收敛到零。当s→0， θ_ie→α＝：-arctan(v_ry_ie)，那么系统位姿误差收敛到零。

所述中央处理单元模块4对跟随车辆R_j的实际位置和角度为

x_j＝x_i-dcosθ_i+l_ijcos(ψ_ij+θ_i)

y_j＝y_i-dsinθ_i+l_ijsin(ψ_ij+θ_i)

θ_j＝θ_j(公式17)

而期望的跟随车辆R_j位置为

x_jr＝x_i-dcosθ_i+l_ijdcos(ψ_ijd+θ_i)

y_jr＝y_i-dsinθ_i+l_ijdsin(ψ_ijd+θ_i)

θ_jr＝θ_i(公式18)

其中ψ_ijd和l_ijd分别表示领航车辆R_i与跟随车辆R_j间期望的角度和车间距， 由式(公式17)和(公式18)得到位姿误差方程为

$e_j=\left(\begin{array}{c}{e}_{j1}\\ e_{j2}\\ e_{j3}\end{array}\right)=\left(\begin{array}{c}{l}_{\mathrm{ijd}}\cos ({\psi }_{\mathrm{ijd}}+e_{j3})-l_{\mathrm{ij}}\cos ({\psi }_{\mathrm{ij}}+e_{j3})\\ l_{\mathrm{ijd}}\sin ({\psi }_{\mathrm{ijd}}+e_{j3})-l_{\mathrm{ij}}\sin ({\psi }_{\mathrm{ij}}+e_{j3})\\ {\theta }_i-{\theta }_j\end{array}\right)$(公式19)

为了进一步分析(公式19)式的误差方程，假设ψ_ijd、l_ijd为常数，将车间距 l_ij分解到水平方向和垂直方向得：

$l_{\mathrm{ijx}}=x_{i_{\mathrm{front}}}-x_{j_{\mathrm{front}}}=x_i-x_j-d\cos {\theta }_j$

$l_{\mathrm{ijy}}=y_{i_{\mathrm{front}}}-y_{j_{\mathrm{front}}}=y_i-y_j-d\cos {\theta }_j$(公式20)

对(公式20)式进行微分得：

${\stackrel{.}{l}}_{\mathrm{ijx}}=v_i\cos {\theta }_i-v_j\cos {\theta }_j+d{\omega }_j\sin {\theta }_j$

${\stackrel{.}{l}}_{\mathrm{ijy}}=v_i\sin {\theta }_i-v_j\sin {\theta }_j-d{\omega }_j\cos {\theta }_j$(公式21)

有$l_{\mathrm{ij}}^2=l_{\mathrm{ijx}}^2+l_{\mathrm{ijy}}^2,{\psi }_{\mathrm{ij}}=\arctan \left(\frac{l_{\mathrm{ijy}}}{l_{\mathrm{ijx}}}\right)-{\theta }_i+\pi .$由运动学分析得：

${\stackrel{.}{l}}_{\mathrm{ij}}=v_j\cos {\gamma }_j-v_i\cos {\psi }_{\mathrm{ij}}+d{\omega }_j\sin {\gamma }_j$

${\stackrel{.}{\psi }}_{\mathrm{ij}}=(v_i\sin {\psi }_{\mathrm{ij}}-v_j\sin {\gamma }_j+d{\omega }_j\cos {\gamma }_j-l_{\mathrm{ij}}{\omega }_i)/l_{\mathrm{ij}}$(公式22)

其中γ_j＝ψ_ij+e_j3。联立式(公式19)、(公式22)得动态误差微分方程为

${\stackrel{·}{e}}_j=\left(\begin{array}{c}{\stackrel{·}{e}}_{j1}\\ {\stackrel{·}{e}}_{j2}\\ {\stackrel{·}{e}}_{j3}\end{array}\right)=\left(\begin{array}{c}-v_j+v_i\cos e_{j3}+{\omega }_j{e}_{j2}-{\omega }_i{l}_{\mathrm{ijd}}\sin ({\psi }_{\mathrm{ijd}}+e_{j3})\\ -{\omega }_j{e}_{j1}+v_i\sin e_{j3}-{\mathrm{d\omega }}_j+{\omega }_i{l}_{\mathrm{ijd}}\cos ({\psi }_{\mathrm{ijd}}+e_{j3})\\ {\omega }_i-{\omega }_j\end{array}\right)$

(公式23)

中央处理单元模块4运用backstepping方法设计了跟随车辆R_j的控制关系通 过公式(F-3)获得跟随车辆(R_j)根据参考轨迹所需控制的线速度((v_j)和 角速度(ω_j)，公式(F-3)如下：

$\left(\begin{array}{c}{v}_j\\ {\omega }_j\end{array}\right)=\left(\begin{array}{c}{v}_i\cos e_{j3}+k_4{e}_{j1}\\ {\omega }_i+(v_i+k_v)k_5{e}_{j2}+(v_i+k_v)k_6\sin e_{j3}\end{array}\right)+\left(\begin{array}{c}{\gamma }_{\mathrm{vj}}\\ {\gamma }_{\mathrm{\omega j}}\end{array}\right)---(F-3)$

γ_vj＝-ω_il_ijdsin(ψ_ijd+e_j3)、${\gamma }_{\mathrm{\omega j}}=-\frac{\left|e_{j2}\right|({\omega }_j(d_j+l_{\mathrm{ijd}})+k_6(v_i+k_v)d_j+k_v)}{1/k_5+\left|e_{j2}\right|d_j}$

其中k₄、k₅、k₆、k_v均为正实数，ψ_ijd和l_ijd分别表示领航车辆(R_i)和跟 随车辆(R_j)期望的角度和车间距，

位姿误差$e_j=\left(\begin{array}{c}{e}_{j1}\\ e_{j2}\\ e_{j3}\end{array}\right)=\left(\begin{array}{c}{l}_{\mathrm{ijd}}\cos ({\psi }_{\mathrm{ijd}}+e_{j3})-l_{\mathrm{ij}}\cos ({\psi }_{\mathrm{ij}}+e_{j3})\\ l_{\mathrm{ijd}}\sin ({\psi }_{\mathrm{ijd}}+e_{j3})-l_{\mathrm{ij}}\sin ({\psi }_{\mathrm{ij}}+e_{j3})\\ {\theta }_i-{\theta }_j\end{array}\right).$

参数k_v是为了保证即使v_i＝0时系统的渐进稳定性。考虑下面的Lyapunov 函数

$V_j=\frac{1}{2}(e_{j1}^2+e_{j2}^2)+\frac{1}{k_5}(1-\cos e_{j3})$(公式27)

V_j≥0且当且仅当e_j＝0时V_j＝0才成立。对(公式27)式微分并结合(公式 23)、(公式24)、(公式25)、(F-3)得

$\left(\begin{array}{c}{\stackrel{.}{V}}_j=e_{j1}(-v_j+v_i\cos e_{j3}+{\omega }_j{e}_{j2}-{\omega }_i{l}_{\mathrm{ind}}\sin ({\psi }_{\mathrm{ijd}}+e_{j3}))+e_{j2}({-\omega }_j{e}_{j1}\\ +v_i\sin e_{j3}-d_j{\omega }_j+{\omega }_i{l}_{\mathrm{ind}}\cos ({\psi }_{\mathrm{ind}}+e_{j3}))+\frac{1}{k_2}({\omega }_i-{\omega }_j)\sin e_{j3}\\ ={-k}_4{e}_{j1}^2-k_5{d}_j(v_i+k_v)e_{j2}^2-\frac{k_6}{k_5}(v_i+k_v){\sin }^2{e}_{j3}-e_{j2}{d}_j({\omega }_i+k_6(v_i+k_v)\sin e_{j3}+{\gamma }_{\mathrm{\omega j}})\\ +e_{j2}{\omega }_i{l}_{\mathrm{ind}}\cos ({\psi }_{\mathrm{ijd}}+e_{j3})-k_v{e}_{j2}\sin e_{j3}-\frac{\sin e_{j3}}{k_5}{\gamma }_{\mathrm{\omega j}}\\ ={-k}_4{e}_{j1}^2-k_5{d}_j(v_i+k_v)e_{j2}^2-\frac{k_6}{k_5}(v_i+k_v){\sin }^2{e}_{j3}-e_{j2}{d}_j{\omega }_i-e_{j2}\sin e_{j3}(k_v+k_6(v_i+k_v)d_j)\\ {-e}_{j2}{d}_j{\gamma }_{\mathrm{\omega j}}+e_{j2}\cos ({\psi }_{\mathrm{ijd}}+e_{j3}){\omega }_i{l}_{\mathrm{ijd}}-\frac{\sin e_{j3}}{k_5}{\gamma }_{\mathrm{\omega j}}\\ \le {-k}_4{e}_{j1}^2-k_5{d}_j(v_i+k_v)e_{j2}^2-\frac{k_6}{k_5}(v_i+k_v){\sin }^2{e}_{j3}\\ +\left|e_{j2}\right|({\omega }_i(d_j+l_{\mathrm{ind}})+k_v+k_6(v_i+k_v)d_j+(d_j+\frac{1}{k_5\left|e_{j2}\right|}){\gamma }_{\mathrm{\omega j}})\\ \le {-k}_4{e}_{j1}^2-k_5{d}_j(v_i+k_v)e_{j2}^2-\frac{k_6}{k_5}(v_i+k_v){\sin }^2{e}_{j3}\le 0\end{array}\right)$

对于所有的v_i≥0有式(24)、(25)、(F-3)的速度控制输入可以使 式(19)、(23)系统动态误差达到渐进稳定。

在一个实施例中，车外传感器模块1在车辆R_i、R_j左前侧和右前侧分别安 装红外传感器5用于检测前方障碍物，在车轮轴上安装速度传感器6用于检测车 辆R_i、R_j行驶的速度。

在一个实施例中，GPS定位模块2含有GPS天线及转接线，其供电电压为 5V。

在一个实施例中，所述无线通信模块3采用FPV5.8G200MW发送接收套 装，其供电电压均为12V。

在一个实施例中，所述中央处理单元模4块为嵌入式DSP处理单元。

在另一个实施例中，所述车外传感器模块1还包括可视测量装置用于监测障 碍物。

如图4所述的根据本发明实施例的基于信息物理网的车队编队控制方法的 流程图，实例还详细说明了一种基于信息物理网的车队编队控制方法。

第一步骤S1中，进行车队编队，其中所述车队包括领航车辆R_i和跟随车辆 R_j，设在车队中的每辆车R_i、R_j上的所述车队智能避障装置包括车外传感器模 块1、GPS定位模块2、无线通信模块3和中央处理单元模块4。

车外传感器模块1包括用于检测前方障碍物的红外传感器5和用于检测车行 驶速度的速度传感器6。

GPS定位模块2安装在每辆车R_i、R_j前端中间位置用于实时采集所述车的 位置信息且同步时间。

所述无线通信模块3包括无线网卡和无线路由器用于与所述车队中的其他 车辆建立网络连接。

用于实现通信和应用协议的所述中央处理单元模块4通过无线通信模块3 接收车辆的速度、位置信息并处理。

第二步骤S2中，红外传感器5检测前方是否存在障碍物，如果存在障碍物， 执行第三步骤，如果不存在障碍物，执行第四步骤；

第三步骤S3中，所述中央处理单元模块4建立坐标系，并通过公式(F-1) 计算获得跟随车辆(R_j)的线速度(v_j)和角速度(ω_j)，公式(F-1)如下：

$\left(\begin{array}{c}{v}_j\\ {\omega }_j\end{array}\right)=\left(\begin{array}{c}\frac{i_{\mathrm{ij}}\cos {\gamma }_{\mathrm{kj}}+{\stackrel{·}{\delta }}_{\mathrm{jk}}\sin {\gamma }_{\mathrm{ij}}+v_i\cos {\psi }_{\mathrm{ij}}\cos {\gamma }_{\mathrm{kj}}}{\cos ({\gamma }_{\mathrm{kj}}-{\gamma }_{\mathrm{ij}})}\\ \frac{i_{\mathrm{ij}}\sin {\gamma }_{\mathrm{kj}}-{\stackrel{·}{\delta }}_{\mathrm{jk}}\cos {\gamma }_{\mathrm{ij}}+v_i\cos {\psi }_{\mathrm{ij}}\sin {\gamma }_{\mathrm{kj}}}{d\cos ({\gamma }_{\mathrm{kj}}-{\gamma }_{\mathrm{ij}})}\end{array}\right)$(F-1)，其中，

γ_ij＝θ_i+ψ_ij-θ_j、γ_kj＝θ_k-θ_j、α₁、α₂为常数，l_ij为领航车辆(R_i)和跟随车辆(R_j)之间的相对距离，d表示跟随 车辆(R_j)质心到轴心的距离，其中定义虚拟车辆R_k，它以恒定线速度v_k相切 于障碍物的方向θ_k沿着障碍物运动，即θ_k的运动方向v_k始终垂直于R_j与虚拟车 辆R_k的连线δ_jk，δ_jk表示跟随车辆(R_j)与障碍物间的最近距离，θ_k为虚拟车 辆R_k相切于障碍物的方向，θ_i、θ_j分别是领航车辆(R_i)和跟随车辆(R_j)的 角度，v_i、v_j分别是领航车辆(R_i)和跟随车辆(R_j)的线速度，ψ_ij和l_ij分别 表示领航车辆(R_i)和跟随车辆(R_j)间相对角度和距离，跟随车辆(R_j)通 过CAN总线将中央处理单元模块4获得的线速度(v_j)和角速度(ω_j)发送到 车辆控制器7进行调整跟随车辆(R_j)的线速度(v_j)和角速度(ω_j)从而确 保车队在编队的情况下进行避障；

第四步骤S4中，所述中央处理单元模块4通过公式(F-2)计算获得领航车 辆(R_i)根据参考轨迹所需控制的线速度(v_i)和角速度(ω_i)，公式(F-2) 如下：

$\left(\begin{array}{c}{v}_i\\ {\omega }_i\end{array}\right)=\left(\begin{array}{c}{y}_{\mathrm{ie}}{\omega }_i+v_r\cos {\theta }_{\mathrm{ie}}+(ϵ{\left|s_1\right|}^a\frac{s_1}{\left|s_1\right|+\sigma }+k_2{s}_1)y_{\mathrm{ie}}^2+ϵ{\left|s_1\right|}^a\frac{s_1}{\left|s_1\right|+\sigma }+k_2{s}_1\\ \frac{{\omega }_r+\frac{\partial \alpha }{{\partial v}_r}{\stackrel{·}{v}}_r+\frac{\partial \alpha }{{\partial y}_{\mathrm{ie}}}\left(v_r\sin {\theta }_{\mathrm{ie}}\right)+ϵ{\left|s_2\right|}^a\frac{s_2}{\left|s_2\right|+\sigma }+k_3{s}_2}{1+\frac{\partial \alpha }{{\partial y}_{\mathrm{ie}}}{x}_{\mathrm{ie}}}\end{array}\right)---(F-2),$其中，

v_r、ω_r分别为参考轨迹的线速度和角速度，ε、k₂、k₃均为正常数， 0＜a＜1，

位姿误差e_i＝[x_ie，y_ie，θ_ie]^T收敛到零，$s=\left(\begin{array}{c}{s}_1\\ s_2\end{array}\right)=\left(\begin{array}{c}{x}_{\mathrm{ie}}\\ {\theta }_{\mathrm{ie}}+\arctan \left(v_r{y}_{\mathrm{ie}}\right)\end{array}\right),$

所述中央处理单元模块4通过公式(F-3)计算获得跟随车辆(R_j)根据参 考轨迹所需控制的线速度((v_j)和角速度(ω_j)，公式(F-3)如下：

$\left(\begin{array}{c}{v}_j\\ {\omega }_j\end{array}\right)=\left(\begin{array}{c}{v}_i\cos e_{j3}+k_4{e}_{j1}\\ {\omega }_i+(v_i+k_v)k_5{e}_{j2}+(v_i+k_v)k_6\sin e_{j3}\end{array}\right)+\left(\begin{array}{c}{\gamma }_{\mathrm{vj}}\\ {\gamma }_{\mathrm{\omega j}}\end{array}\right)---(F-3)$

γ_vj＝-ω_il_ijdsin(ψ_ijd+e_j3)、${\gamma }_{\mathrm{\omega j}}=-\frac{\left|e_{j2}\right|({\omega }_i(d_j+l_{\mathrm{ijd}})+k_6(v_i+k_v)d_j+k_v)}{1/k_5+\left|e_{j2}\right|d_j}$

其中，k₄、k₅、k₆、k_v均为正实数，ψ_ijd和l_ijd分别表示领航车辆(R_i)和 跟随车辆(R_j)期望的角度和车间距，

位姿误差$e_j=\left(\begin{array}{c}{e}_{j1}\\ e_{j2}\\ e_{j3}\end{array}\right)=\left(\begin{array}{c}{l}_{\mathrm{ijd}}\cos ({\psi }_{\mathrm{ijd}}+e_{j3})-l_{\mathrm{ij}}\cos ({\psi }_{\mathrm{ij}}+e_{j3})\\ l_{\mathrm{ijd}}\sin ({\psi }_{\mathrm{ijd}}+e_{j3})-l_{\mathrm{ij}}\sin ({\psi }_{\mathrm{ij}}+e_{j3})\\ {\theta }_i-{\theta }_j\end{array}\right).$

尽管以上结合附图对本发明的实施方案进行了描述，但本发明并不局限于上 述的具体实施方案和应用领域，上述的具体实施方案仅仅是示意性的、指导性的， 而不是限制性的。本领域的普通技术人员在本说明书的启示下和在不脱离本发明 权利要求所保护的范围的情况下，还可以做出很多种的形式，这些均属于本发明 保护之列。