基于轨迹规划的自动驾驶车辆避障控制方法及系统

摘要

本发明公开了一种基于轨迹规划的自动驾驶车辆避障控制方法及系统，该方法包括S1，获取静态障碍物环境下的避障轨迹及其关于时间的方程组；S2，获取障碍物的坐标信息及体积信息，并根据障碍物的坐标信息及体积信息设置约束条件；S3，根据所述避障轨迹对应的参数方程及约束条件，求出最优避障路径；S4，求出最优避障路径的曲率公式，并根据该曲率公式求出曲率半径关于时间的参数方程；S5，将曲率半径关于时间的参数方程作为转弯半径关于时间的参数方程，求解各个车轮的转向角及各个车轮的目标转速；S6，分别控制各个车轮按对应的转向角和目标转速转动。本发明能够使智能车辆自动驾驶转弯避障或者轨迹跟踪时达到好的控制效果。

权利要求

1.一种基于轨迹规划的自动驾驶车辆避障控制方法，所述自动驾驶车辆为四轮独立控制车辆，其特征在于：包括以下步骤：S1，获取静态障碍物环境下的避障轨迹及其关于时间的方程组；

S2，获取障碍物的坐标信息及体积信息，并根据障碍物的坐标信息及体积信息设置约束条件；

S3，根据所述避障轨迹对应的参数方程及约束条件，求出最优避障路径；

S4，求出最优避障路径的曲率公式，并根据该曲率公式求出曲率半径关于时间的参数方程；

S5，将曲率半径关于时间的参数方程作为转弯半径关于时间的参数方程，求解各个车轮的转向角及各个车轮的目标转速；

S6，分别控制各个车轮按对应的转向角和目标转速转动。

2.根据权利要求1所述的基于轨迹规划的自动驾驶车辆避障控制方法，其特征在于：步骤S1中的避障轨迹为多项式拟合曲线。

3.根据权利要求2所述的基于轨迹规划的自动驾驶车辆避障控制方法，其特征在于：步骤S1中的避障轨迹对应的关于时间的参数为5次多项式；

步骤S1中避障轨迹及其关于时间的方程组为

其中，x、y为关于时间t的函数；A、B、C、D、E、G、H、I、J、K，F、L均为系数。

4.根据权利要求3所述的基于轨迹规划的自动驾驶车辆避障控制方法，其特征在于：步骤S2中，约束条件的数量与障碍物的数量相等；

所述约束条件为：

(x-x1)2+(y-y1)2≥L12

其中，x1和y1为分别为某一障碍物的横坐标和纵坐标；L1为车辆外接圆的半径与障碍物的外接圆半径之和。

5.根据权利要求4所述的基于轨迹规划的自动驾驶车辆避障控制方法，其特征在于：步骤S3包括如下具体步骤，

S31，建立最优轨迹优化模型，其中，最优轨迹优化模型为：S32，建立任意时刻的避障约束条件，其中，约束条件为：其中，(xn,yn)为第n个障碍物的中心点的坐标，Ln为第n个障碍物的外接圆半径与车辆的外接圆半径之和；max[y1,y2,......yn]表示各障碍物中心坐标中，Y向坐标的最大值；max[x1,x2,......xn]表示各障碍物中心坐标中，X向坐标的最大值；max[A1,A2,......An]表示各障碍物外接圆半径中最大值；t0为避障时间；

S33，根据最优轨迹优化模型和避障约束条件，求解最优短轨迹对应的参数A、B、C、D、E、G、H、I、J、K；

S34，根据S33中的结果得到最优轨迹对应的参数方程。

6.根据权利要求5所述的基于轨迹规划的自动驾驶车辆避障控制方法，其特征在于：步骤S4具体包括，S41，求出最优避障路径的曲率公式，其中，该曲率公式为：其中，K表示出最优避障路径的曲率；

S42，根据步骤S41中的曲率公式，求出最优避障路径对应的曲率半径公式，该公式为：其中，ρ为最优避障路径上各点对应的轨迹曲率半径。

7.一种基于轨迹规划的自动驾驶车辆避障控制系统，其特征在于：包括，视觉传感器、DGPS传感器、障碍物位置提取模块、车辆坐标定位提取模块、车载电脑端的labview算法控制模块、避障轨迹规划模块、Myrio控制输出模块、四轮独立控制转向模块和四轮独立控制驱动模块；

所述视觉传感器用于获取车辆周围的图像信息；

所述障碍物位置提取模块与所述视觉传感器电性连接，所述障碍物位置提取模块用于从所述视觉传感器获取的图像信息中提取障碍物信息；

所述车载电脑端的labview算法控制模块与所述障碍物位置提取模块电性连接，所述车载电脑端的labview算法控制模块用于根据所述障碍物信息计算出当前障碍物的相对位置信息；

所述DGPS传感器用于获取车辆的运行状态信息；

所述车辆坐标定位提取模块与所述DGPS传感器电性连接，所述车载电脑端的labview算法控制模块与所述车辆坐标定位提取模块电性连接；所述车辆坐标定位提取模块用于提出车辆的运行状态信息并将该运行状态信息输出给车载电脑端的labview算法控制模块；

所述车载电脑端的labview算法控制模块还用于从所述运行状态信息计算出车辆坐标、速度和航向角；

所述避障轨迹规划模块与所述车载电脑端的labview算法控制模块电性连接，所述避障轨迹规划模块用于规划避障路径，并计算出各个车轮的目标转角和目标转速，并将计算结果输出给所述车载电脑端的labview算法控制模块；

所述Myrio控制输出模块与所述车载电脑端的labview算法控制模块电性连接；所述车载电脑端的labview算法控制模块还用于根据各个车轮的目标转角和目标转速输生成控制指令并传输给所述Myrio控制输出模块；

所述四轮独立控制转向模块和所述四轮独立控制驱动模块均与所述Myrio控制输出模块电性连接；所述Myrio控制输出模块用于从所述控制指令中提取出转向控制指令，并输出给所述四轮独立控制转向模块；所述Myrio控制输出模块还用于从所述控制指令中提取出转速控制指令，并输出给所述四轮独立控制驱动模块；

所述四轮独立控制转向模块用于控制各个车轮的转向，所述四轮独立控制驱动模块用于控制各个车轮的转速。

8.根据权利要求7所述的基于轨迹规划的自动驾驶车辆避障控制系统，其特征在于：还包括电池管理模块和车辆安全制动模块；

所述电池管理模块和所述车辆安全制动模块均与所述Myrio控制输出模块电性连接。

9.根据权利要求7所述的基于轨迹规划的自动驾驶车辆避障控制系统，其特征在于：所述车载电脑端的labview算法控制模块还用于根据车辆当前状态信息，计算出当前状态信息与已发出的命令的偏差，并根据该偏差发出修正后的控制指令，并通过所述Myrio控制输出模块、所述四轮独立控制转向模块和所述四轮独立控制驱动模块控制车轮按修正后的控制指令转向和转动。

说明书

基于轨迹规划的自动驾驶车辆避障控制方法及系统

技术领域

[0001] 本发明涉及车辆控制技术领域，特别是一种基于轨迹规划的自动驾驶车辆避障控制方法及系统。

背景技术

[0002] 轮毂电机将电动机、传动系和制动器集成为一体，其体积小，比功率大。由于轮毂电机中集成了传动系，所以电动轮驱动汽车上取消了传统的离合器、变速器、传动轴、差速器、等速万向节和半轴等部件，使汽车底盘结构简化，提高了传动的效率，降低了整车质量。

[0003] 能源转型战略的推进，电能作为清洁能源，必然助长电动汽车的发展。目前，对轮毂电机驱动的汽车的研究主要集中在轮毂电机驱动技术上，而对差动转向系统及其路感控制技术的研究还很少，自主驾驶电动汽车是未来车辆的发展方向，基于轨迹规划的自动驾驶车辆差速转向控制必然成为未来自动驾驶智能车辆的一个重要研究方向。

[0004] 如何控制四轮独立转向汽车避开障碍物，是本领域技术人员亟待解决的重要问题之一。

发明内容

[0005] 本发明的目的是提供一种基于轨迹规划的自动驾驶车辆避障控制方法，以解决现有技术中的技术问题，它能够使智能车辆自动驾驶转弯避障或者轨迹跟踪时达到好的控制效果，增加车辆转向操纵灵敏性的行驶平顺性。

[0006] 本发明提供了一种基于轨迹规划的自动驾驶车辆避障控制方法，所述自动驾驶车辆为四轮独立控制车辆，其中，包括以下步骤：

[0007] S1，获取静态障碍物环境下的避障轨迹及其关于时间的方程组；

[0008] S2，获取障碍物的坐标信息及体积信息，并根据障碍物的坐标信息及体积信息设置约束条件；

[0009] S3，根据所述避障轨迹对应的参数方程及约束条件，求出最优避障路径；

[0010] S4，求出最优避障路径的曲率公式，并根据该曲率公式求出曲率半径关于时间的参数方程；

[0011] S5，将曲率半径关于时间的参数方程作为转弯半径关于时间的参数方程，求解各个车轮的转向角及各个车轮的目标转速；

[0012] S6，分别控制各个车轮按对应的转向角和目标转速转动。

[0013] 如上所述的基于轨迹规划的自动驾驶车辆避障控制方法，其中，优选的是，步骤S1包括如下步骤，

[0014] 步骤S1中的避障轨迹为多项式拟合曲线。

[0015] 如上所述的基于轨迹规划的自动驾驶车辆避障控制方法，其中，优选的是，步骤S1中的避障轨迹对应的关于时间的参数为5次多项式；

[0016] 步骤S1中避障轨迹及其关于时间的方程组为

[0017]

[0018] 其中，x、y为关于时间t的函数；A、B、C、D、E、G、H、I、J、K 均为系数。

[0019] 如上所述的基于轨迹规划的自动驾驶车辆避障控制方法，其中，优选的是，[0020] 步骤S2中，约束条件的数量与障碍物的数量相等；

[0021] 所述约束条件为：

[0022] (x-x1)2+(y-y1)2≥L12

[0023] 其中，和为分别为某一障碍物的横坐标和纵坐标；为车辆外接圆的半径与障碍物的外接圆半径之和。

[0024] 如上所述的基于轨迹规划的自动驾驶车辆避障控制方法，其中，优选的是，[0025] 步骤S3包括如下具体步骤，

[0026] S31，建立最优轨迹优化模型，其中，最优轨迹优化模型为：

[0027]

[0028] S32，建立任意时刻的避障约束条件，其中，约束条件为：

[0029]

[0030] 其中，(xn,yn)为第n个障碍物的中心点的坐标，Ln为第n个障碍物的外接圆半径与车辆的外接圆半径之和；max[y1,y2,......yn]表示各障碍物中心座标中，Y向坐标的最大值；max[x1,x2,......xn]表示各障碍物中心座标中， X向坐标的最大值；max[A1,A2,......An]表示各障碍物外接圆半径中最大值；

[0031] S33，根据最优轨迹优化模型和避障约束条件，求解最优短轨迹对应的参数A、B、C、D、E、G、H、I、J、K；

[0032] S34，根据S33中的结果得到最优轨迹对应的参数方程。

[0033] 如上所述的基于轨迹规划的自动驾驶车辆避障控制方法，其中，优选的是，步骤S4具体包括：

[0034] S41，求出最优避障路径的曲率公式，其中，该曲率公式为：

[0035]

[0036] 其中，K表示出最优避障路径的曲率；

[0037] S42，根据步骤S41中的曲率公式，求出最优避障路径对应的曲率半径公式，该公式为：

[0038]

[0039] 其中，ρ为最优避障路径上各点对应的轨迹曲率半径。

[0040] 如上所述的基于轨迹规划的自动驾驶车辆避障控制方法，其中，优选的是，[0041] 步骤S5中，包括如下具体步骤，

[0042] S51，获取车辆质心坐标、车辆轮距B、质心距前轴的距离a、质心距后轴的距离b、前后轴轴距L、车轮直径d；

[0043] S52，求各车轮转角，且各车轮转角公式如下：

[0044] 右前轮转角：

[0045] 左前轮转角：

[0046] 左后轮转角：

[0047] 右后轮转角：

[0048] 或：

[0049] 右前轮转角：

[0050] 左前轮转角：

[0051] 左后轮转角：

[0052] 右后轮转角：

[0053] S53，求各车轮转向半径，且各车轮的转向半径如下：

[0054] 右前轮转向半径：

[0055] 左前轮转向半径：

[0056] 左后轮转向半径：

[0057] 右后轮转向半径：

[0058] 或

[0059] 右前轮转向半径：

[0060] 左前轮转向半径：

[0061] 左后轮转向半径：

[0062] 右后轮转向半径：

[0063] S54，根据各车轮的转弯半径，计算出各车轮的目标转速；

[0064] 其中，各车轮的目标转速公式如下：

[0065] 右前轮转速：

[0066] 左前轮转速：

[0067] 左后轮转速：

[0068] 右后轮转速：

[0069] 或

[0070] 右前轮转速：

[0071] 左前轮转速：

[0072] 左后轮转速：

[0073] 右后轮转速：

[0074] 本发明还提出了一种基于轨迹规划的自动驾驶车辆避障控制系统，其中，[0075] 包括，视觉传感器、DGPS传感器、障碍物位置提取模块、车辆坐标定位提取模块、车载电脑端的labview算法控制模块、避障轨迹规划模块、Myrio 控制输出模块、四轮独立控制转向模块和四轮独立控制驱动模块；

[0076] 所述视觉传感器用于获取车辆周围的图像信息；

[0077] 所述障碍物位置提取模块与所述视觉传感器电性连接，所述障碍物位置提取模块用于从所述视觉传感器获取的图像信息中提取障碍物信息；

[0078] 所述车载电脑端的labview算法控制模块与所述障碍物提取模块电性连接，所述车载电脑端的labview算法控制模块用于根据所述障碍物信息计算出当前障碍物的相对位置信息；

[0079] 所述DGPS传感器用于获取车辆的运行状态信息；

[0080] 所述车辆坐标定位提取模块与所述DGPS传感器电性连接，所述车载电脑端的labview算法控制模块与所述车辆坐标定位提取模块电性连接；所述车辆坐标定位提取模块用于提出车辆的运行状态信息并将该运行状态信息输出给车载电脑端的labview算法控制模块；所述车载电脑端的labview算法控制模块还用于从所述运行状态信息计算出车辆坐标、速度和航向角；

[0081] 所述避障轨迹规划模块与所述车载电脑端的labview算法控制模块电性连接，所述避障轨迹规划模块用于规划避障路径，并计算出各个车轮的目标转角和目标转速，并将计算结果输出给所述车载电脑端的labview算法控制模块；

[0082] 所述Myrio控制输出模块与所述车载电脑端的labview算法控制模块电性连接；所述车载电脑端的labview算法控制模块还用于根据各个车轮的目标 转角和目标转速输生成控制指令并传输给所述Myrio控制输出模块；

[0083] 所述四轮独立控制转向模块和所述四轮独立控制驱动模块均与所述 Myrio控制输出模块电性连接；所述Myrio控制输出模块用于从所述控制指令中提取出转向控制指令，并输出给所述四轮独立控制转向模块；所述Myrio 控制输出模块还用于从所述控制指令中提取出转速控制指令，并输出给所述四轮独立控制驱动模块；

[0084] 所述四轮独立控制转向模块用于控制各个车轮的转向，所述四轮独立控制驱动模块用于控制各个车轮的转速。

[0085] 如上所述的基于轨迹规划的自动驾驶车辆避障控制系统，其中，优选的是，还包括电池管理模块和车辆安全制动模块；

[0086] 所述电池管理模块和所述车辆安全制动模块均与所述Myrio控制输出模块电性连接。

[0087] 如上所述的基于轨迹规划的自动驾驶车辆避障控制系统，其中，优选的是，[0088] 所述车载电脑端的labview算法控制模块还用于根据车辆当前状态信息，计算出当前状态信息与已发出的命令的偏差，并根据该偏差发出修正后的控制指令，并通过所述Myrio控制输出模块、所述四轮独立控制转向模块和所述四轮独立控制驱动模块控制车轮按修正后的控制指令转向和转动。

[0089] 同现有技术相比，该基于轨迹规划的自动驾驶车辆差速转向控制系统具有超前控制的特性，根据预先生成的控制信号做出相应的控制命令，也可在出现特殊情况时结合系统数据做出合理有效的应变措施，提高自动驾驶车辆转向操纵稳定性、灵活性，提高转向系统的响应速度，对自动驾驶轮毂电机车的四轮转向的研究具有重要意义，有利于自动驾驶车辆尤其是四轮独立控制的轮毂电机车的发展。

附图说明

[0090] 图1为控制系统各功能模块相互关系流程图；

[0091] 图2为二自由度独立控制轮毂电机车转向模型；

[0092] 图3为车辆各功能器件组成及相对位置示意图；

[0093] 图4为自动驾驶车辆的整体结构示意图；

[0094] 图5为本发明提出的自动驾驶车辆的结构示意图。

[0095] 附图标记说明：

[0096] 1-前视觉传感器，2-右前轮转向电机驱动器，3-右前轮转角传感器，4- 右前轮轮毂电机驱动器，5-电动油泵，6-两位三通阀，7-多路继电器控制板， 8-myrio控制输出模块，9-右视觉传感器，10-DGPS传感器通信天线，11-数据采集及控制信号传输线束，12-右后轮轮毂电机驱动器，13-DGPS主天线， 14-右后轮转角传感器，15-右后轮转向电机驱动器，16-后视觉传感器，17- 左前轮转向电机驱动器，18-左后轮转向电机驱动器，19-左后轮转角传感器， 20-左后轮轮毂电机驱动器，21-DGPS传感器，22-左视觉传感器，23-车载电脑，24-DGPS辅天线，25-加速踏板，26-两位两通阀，27-左前轮轮毂电机驱动器，28-左前轮转角传感器，29-制动主油路，30-右前轮制动器，31-液压制动储油箱，32-右前轮，33-高压油管，
34-右后轮制动器，35-右后轮，36-右后轮轮毂电机，37-左后轮轮毂电机，38-左后轮制动器，39-左后轮，40-左前轮，41-左前轮轮毂电机，42-左前轮制动器，43-制动主缸，44-车载电源，45- 左前轮转向电机，46-右前轮转向电机，47-右后轮转向电机，48-左后轮转向电机，49-右前轮轮毂电机。

具体实施方式

[0097] 下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

[0098] 请参照图1到图5，本发明提出了一种基于轨迹规划的自动驾驶车辆避障控制系统，其中包括，视觉传感器、DGPS传感器、障碍物位置提取模块、车辆坐标定位提取模块、车载电脑端的labview算法控制模块、避障轨迹规划模块、Myrio控制输出模块、四轮独立控制转向模块和四轮独立控制驱动模块。

[0099] 其中，视觉传感器包括前视觉传感器1、右视觉传感器9、后视觉传感器 16和左视觉传感器22。其中，前视觉传感器1布置在车辆的前方，右视觉传感器9布置在车辆的右侧，后视觉传感器16布置在车辆的后侧，左视觉传感器22布置在车辆在左侧。本申请中所指的视觉传感器，可以是摄像头。

[0100] 所述视觉传感器用于获取车辆周围的图像信息。

[0101] 所述障碍物位置提取模块与所述视觉传感器电性连接，所述障碍物位置提取模块用于从所述视觉传感器获取的图像信息中提取障碍物信息；

[0102] 所述车载电脑端的labview算法控制模块与所述障碍物提取模块电性连接，所述车载电脑端的labview算法控制模块用于根据所述障碍物信息计算出当前障碍物的相对位置信息；

[0103] 所述DGPS传感器用于获取车辆的运行状态信息；具体地，DGPS传感器21上设有DGPS传感器通信天线10，所述DGPS传感器通信天线10包括 DGPS主天线13和DGPS辅天线24。

[0104] 所述车辆坐标定位提取模块与所述DGPS传感器电性连接，所述车载电脑端的labview算法控制模块与所述车辆坐标定位提取模块电性连接；所述车辆坐标定位提取模块用于提出车辆的运行状态信息并将该运行状态信息输出给车载电脑端的labview算法控制模块；所述车载电脑端的labview算法控制模块还用于从所述运行状态信息计算出车辆坐标、速度和航向角；

[0105] 所述避障轨迹规划模块与所述车载电脑端的labview算法控制模块电性连接，所述避障轨迹规划模块用于规划避障路径，并计算出各个车轮的目标转角和目标转速，并将计算结果输出给所述车载电脑端的labview算法控制模块；

[0106] 所述Myrio控制输出模块8与所述车载电脑端的labview算法控制模块电性连接；所述车载电脑端的labview算法控制模块还用于根据各个车轮的目标转角和目标转速输生成控制指令并传输给所述Myrio控制输出模块。

[0107] 所述四轮独立控制转向模块和所述四轮独立控制驱动模块均与所述 Myrio控制输出模块8电性连接；所述Myrio控制输出模块用于从所述控制指令中提取出转向控制指令，并输出给所述四轮独立控制转向模块；所述 Myrio控制输出模块还用于从所述控制指令中提取出转速控制指令，并输出给所述四轮独立控制驱动模块。所述四轮独立控制转向模块用于控制各个车轮的转向，所述四轮独立控制驱动模块用于控制各个车轮的转速。

[0108] 具体地，所述四轮独立控制转向模块包括右前轮转向电机驱动器2、右后轮转向电机驱动器15、左前轮转向电机驱动器17和左后轮转向电机驱动器18。其中，所述右前轮转向电机驱动器2、所述右后轮转向电机驱动器15、所述左前轮转向电机驱动器17和所述左后轮转向电机驱动器18分别用于控制右前轮、右后轮、左前轮和左后轮的转向角度。

[0109] 具体地，还包括左前轮转向电机45、右前轮转向电机46、右后轮转向电机47、左后轮转向电机48。具体地，所述左前轮转向电机驱动器17通过所述左前轮转向电机45控制左前轮40的转向；所述右前轮转向电机驱动器2 通过右前轮转向电机46控制右前轮32的转向；右后轮转向电机驱动器15 通过右后轮转向电机47控制右后轮35的转向；左后轮转向电机驱动器18 通过左后轮转向电机48控制左后轮39的转向。

[0110] 所述四轮独立控制驱动模块包括右前轮轮毂电机驱动器4、右后轮轮毂电机驱动器12、左后轮轮毂电机驱动器20和左前轮轮毂电机驱动器27；所述右前轮轮毂电机驱动器4、右后轮轮毂电机驱动器12、左后轮轮毂电机驱动器20和左前轮轮毂电机驱动器27用于分别左前轮、右后轮、左后轮和左前轮的转速。具体地，所述四轮独立控制驱动模块分别通过左前轮轮毂电机 41、左后轮轮毂电机37、右后轮轮毂电机36和右前轮轮毂电机49分别控制各个车轮的转速。

[0111] 还包括电池管理模块和车辆安全制动模块；

[0112] 所述电池管理模块和所述车辆安全制动模块均与所述Myrio控制输出模块电性连接。所述车辆安全制动模块包括电动油泵5、液压制动储油箱31、两位三通阀，两位两通阀26、制动主缸43、制动管路、车轮制动器组成，其中，制动管路包括制动主油路29和高压油管
33。具体的作用方法及原理如下所述，当车辆需要减速或者制动时，Myrio控制输出模块对应引脚输出的数字开关信号引入多路继电器控制板7，控制多路继电器的通断，多路继电器被控制端分别接电动油泵5、两位三通阀6、两位两通阀，在Myrio控制信号的作用下，根据工况的不同，控制信号作用效果不同，以适应不同的工况需求。控制逻辑由车辆油路得来，与本试验车相对应。

[0113] 具体地，所述安全制动模块包括右前轮制动器30、右后轮制动器34、左后轮制动器38和左前轮制动器42；所述右前轮制动器30、右后轮制动器34、左后轮制动器38和左前轮制动器42分别用于控制各个车轮的制动。

[0114] 本领域技术人员能够理解的是，汽车上还包括加速踏板25等其他部件，在此不作赘述。

[0115] 所述电源管理模块包含六块12V蓄电池及三块12V锂电池的车载电流 44，电源管理模块的功用是按照各用电模块的额定工况，合理分配电能，在电池能量不足时及时通过数据采集模块相应的引脚给予用户警示。

[0116] 所述车载电脑端的labview算法控制模块还用于根据车辆当前状态信息，计算出当前状态信息与已发出的命令的偏差，并根据该偏差发出修正后的控制指令，并通过所述Myrio控制输出模块、所述四轮独立控制转向模块和所述四轮独立控制驱动模块控制车轮按修正后的控制指令转向和转动。

[0117] 具体实施时，本申请中还包括数据采集及控制信号传输线束11，所述数据采集及控制信号传输线束11被设置为用于传递各种电信号，以实现信息的传递和控制指令的传递。

[0118] 使用时，初始化操作，启动车载电脑23开机程序，由labview算法控制模块进行程序的初始化操作，初始化操作主要有障碍物位置提取模块摄像头参数的初始化操作、车辆坐标定位提取模块基站坐标的初始化操作、避障轨迹规划模块程序的打开及车辆起止点坐标等数据的初始化操作、myrio控制输出模块各参数及硬件引脚的初始化操作。

[0119] 障碍物信息的采集，labview算法控制模块中的传感器数据采集程序通过对应硬件接口接收障碍物位置提取模块中视觉传感器采集的实时的周围环境图像信息，随后传感器数据均基于labview编程环境进行数据处理。传感器数据处理程序依靠labview软件视觉模块实时处理图像信息，具体的处理方法有，对图像信息进行灰度化处理、图像二值化处理、图像中值滤波、获取平均像素、获取像素高度、得到障碍物的距离信息以及大小信息，随后将物体的相关信息通过算法匹配到车辆XY坐标系中，同时将相关计算所得数据打包通过labview数据通信模块发送到matlab轨迹规划模块作为最优路径求解的参量。

[0120] 车辆坐标的定位，此步与第二步同时进行的，首先基站K706基站接收机与卫星通讯，定位自身的基站天线坐标，随后通过基站U30-TRX电台与移动站U30-TRX电台进行数据通信，移动站K728接收机同时与卫星进行通讯，得到自身定位坐标。必须说明的是移动站K728接收机定位天线有两根，可同时得到两根天线的定位坐标，用于车辆横滚角、俯仰角、航向角等的计算。随后移动站根据由移动站U30-TRX电台接收到的基站天线坐标进行差分运算，得到高精度的车辆经纬度信息。随后移动站K728接收机通过数据线将车辆定位信息(报文格式)发送到车载电脑对应的接口，由labview算法控制模块中的传感器数据采集程序通过对应硬件接口接收报文信号，随后传感器数据处理程序分析报文信息，分离出车辆定位数据，分离报文得到的车辆定位数据有，报文$GPTRA中的方向角、俯仰角、及横滚角，报文 $PTNL,PJK中的投影后的X坐标、Y坐标及海拔高度，报文$GPVTG中的运动角度真北参照系、水平运动速度(km/h)，报文$GPNTR输出差分后的X、 Y、H方向平距。

[0121] 具体实施时，还包括右前轮转角传感器3、右后轮转角传感器4、左后轮转角传感器19和左前轮转角传感器28；如此，能够利用所述右前轮转角传感器3、右后轮转角传感器4、左后轮转角传感器19和左前轮转角传感器28 分别获取各个车轮的转角信息，以便于将实际控制结果与控制指令相比较，以实现控制效果的反馈。

[0122] 轨迹规划求解最优路径。通过MATLAB软件基于五次多项式拟合进行车辆行驶最优路径的轨迹规划。通过静态障碍物环境下避障轨迹建模，将障碍物位置提取模块处理得到的障碍物位置及大小等信息及车辆定位坐标提取模块得到车辆自身定位信息作为约束条件导入五次多项式进行轨迹规划的求解，其次还需设定车辆的起始坐标作为轨迹的约束条件。

[0123] 请参照图1和图2，本发明还公开了一种基于轨迹规划的自动驾驶车辆避障控制方法，所述自动驾驶车辆为四轮独立控制车辆，其中，包括以下步骤：

[0124] S1，获取静态障碍物环境下的避障轨迹及其关于时间的方程组；其中，对避障轨迹的获取可以是预先设定的参数待定的曲线拟合方程组，如x和y 方向均为对时间的多项式拟合。

[0125] 步骤S1中的避障轨迹对应的关于时间的参数为5次多项式；

[0126] 步骤S1中避障轨迹及其关于时间的方程组为

[0127]

[0128] 其中，x、y为关于时间t的函数；A、B、C、D、E、G、H、I、J、K 均为系数。

[0129] 以10秒避开障碍物，且max[x1,x2,......xn]+max[A1,A2,......An]等于 150+max[A1,A2,......An]，max[y1,y2,......yn]+max[A1,A2,......An]等于 100+max[A1,A2,......An]为例，即，当车辆的质心处于(150+max[A1,A2,......An]， 100+max[A1,A2,......An])处时，则车辆成功避障。

[0130] 车辆起止点坐标

[0131] x(0)＝0，y(0)＝0,x(10)＝150+max[A1,A2,......An],y(10)＝100+max[A1,A2,......An] ，可得：

[0132]

[0133] 则

[0134] 这样，确定参数A,B,C,D,E及G,H,I,J,K，轨迹便能确定。便可以编写主函数。

[0135] S2，获取障碍物的坐标信息及体积信息，并根据障碍物的坐标信息及体积信息设置约束条件；

[0136] 步骤S2中，约束条件的数量与障碍物的数量相等。

[0137] 所述约束条件为：

[0138] (x-x1)2+(y-y1)2≥L12

[0139] 其中，x1和y1为分别为某一障碍物的横坐标和纵坐标；L1为车辆外接圆的半径与障碍物的外接圆半径之和。

[0140] 以三个体积相等的障碍物为例，三个障碍物的中心坐标位置分别为(12，10)，(50，55)，(150，100)，且三个障碍物的外接圆半径与车辆的外接圆半径之和均为6。此处所指的车辆的外接圆，是指以车辆质心为圆心，以车辆周边上的点到质心的水平距离的最大值为半径所得的圆；障碍物的外接圆，是指以障碍物的几何中心为圆心，以障碍物周边距离该几何中心水平距离最大的点所作的圆。

[0141] 由障碍物位置提取模块计算已知障碍物的坐标信息及体积信息，并通过车载电脑中labview程序导入到MATLAB轨迹规划模块中。

[0142] 一号障碍物坐标及体积约束：(x-12)2+(y-10)2≤36

[0143] 二号障碍物坐标及体积约束：(x-50)2+(y-55)2≤36

[0144] 三号障碍物坐标及体积约束：(x-150)2+(y-100)2≤36

[0145] 因此路径的总长度如下：

[0146] 因为考虑到 与 求出的x与y的表达式是相同的，确定目标函数为

[0147] 约束条件确定了如下约束：

[0148]

[0149] 为了实现避障，任意时刻t，有约束条件如下：

[0150]

[0151] 即

[0152] S3，根据所述避障轨迹对应的参数方程及约束条件，求出最优避障路径；

[0153] 步骤S3包括如下具体步骤，

[0154] S31，建立最优轨迹优化模型，其中，最优轨迹优化模型为：

[0155]

[0156] S32，建立任意时刻的避障约束条件，其中，约束条件为：

[0157]

[0158] 其中，(xn,yn)为第n个障碍物的中心点的坐标，Ln为第n个障碍物的外接圆半径与车辆的外接圆半径之和；max[y1,y2,......yn]表示各障碍物中心座标中，Y向坐标的最大值；max[x1,x2,......xn]表示各障碍物中心坐标中， X向坐标的最大值；max[A1,A2,......An]表示各障碍物外接圆半径中最大值；t0 为避障时间。

[0159] 以以上具体实例为例子，所得的约束条件则为：

[0160]

[0161] S33，根据最优轨迹优化模型和避障约束条件，求解最优短轨迹对应的参数A、B、C、D、E、G、H、I、J、K；具体地，以利用matlab编程求解为例：

[0162] 采用MATLAB里优化工具进行最优化求解。得到A、B、C、D、E、G、 H、I、J、K参数的值。

[0163] 由此可得到最优轨迹的参数方程

[0164] 具体实施时，轨迹路径与车辆定位坐标相结合，将matlab计算求解得到的轨迹数据保存为labview程序可调用的文件，由算法控制程序将轨迹与车辆定位坐标相结合，即将轨迹转换成实际的车辆目标定位坐标，用于后续车辆输出控制及反馈修正。

[0165] S34，根据S33中的结果得到最优轨迹对应的参数方程

[0166] S4，求出最优避障路径的曲率公式，并根据该曲率公式求出曲率半径关于时间的参数方程；

[0167] 步骤S4具体包括：

[0168] S41，求出最优避障路径的曲率公式，其中，该曲率公式为：

[0169]

[0170] 其中，K表示出最优避障路径的曲率；

[0171] S42，根据步骤S41中的曲率公式，求出最优避障路径对应的曲率半径公式，该公式为：

[0172]

[0173] 其中，ρ为最优避障路径上各点对应的轨迹曲率半径。

[0174] 在轨迹上任取一点P(x,y)，此点对应的轨迹曲线的曲率半径便是车辆在该路径下行驶时此坐标下的转弯半径的大小，即，由此计算车辆在二自由度转弯模型下的各各参量，以此为车辆避障及转向时提前生成控制信号，增加车辆的超前控制。

[0175] S5，将曲率半径关于时间的参数方程作为转弯半径关于时间的参数方程，求解各个车轮的转向角及各个车轮的目标转速；

[0176] 本实施例中，不考虑车轮的轮胎由于载荷变化而引起的轮胎特性的变化以及轮胎回正力矩的作用和地面切向力对轮胎侧偏特性的影响。如图2,简化成只受侧向力和横摆力二自由度汽车模型。假定轮胎侧偏刚度极大，可忽略轮胎侧偏角对车辆运行姿态的影响。建立如图的车辆二自由度转向模型。由几何关系约束条件为，车辆质心时刻在轨迹上、车辆坐标系X轴时刻与轨迹相切、各车轮旋转平面时刻与转向圆心到车轮圆心的连线垂直。

[0177] 步骤S5中，包括如下具体步骤，

[0178] S51，获取车辆质心坐标、车辆轮距B、质心距前轴的距离a、质心距后轴的距离b、前后轴轴距L、车轮直径d；

[0179] 在本实施例中，已知车辆的质心为O、前后轮轮距均为B、质心距前轴的距离为a、质心距后轴的距离为b、前后轴轴距为L、车轮直径为d且相等、车辆转弯半径为R、车辆质心对曲率圆圆心的角速度、车辆质心速度，各车轮转向角度δi(i＝1、2、3、4)，各车轮目标速度Vi(i＝1、2、3、4)，各车轮目标转速ni(i＝1、2、3、4)，车辆质心离心力F离，各车轮所受地面侧向反力FYi(i＝1、2、3、4)。

[0180] S52，求各车轮转角，且各车轮转角公式如下：

[0181] 当车辆右转避障时，

[0182] 右前轮转角：

[0183] 左前轮转角：

[0184] 左后轮转角：

[0185] 右后轮转角：

[0186] 当车辆左转避障时：

[0187] 右前轮转角：

[0188] 左前轮转角：

[0189] 左后轮转角：

[0190] 右后轮转角：

[0191] 由此得到各车轮的转向角绝对值，根据各车轮的相对位置合理分配转向控制信号，在车轮转角传感器的反馈信号下将车轮转角稳定控制在理想位置。

[0192] S53，求各车轮转向半径，且各车轮的转向半径如下：

[0193] 当车辆右转避障时，

[0194] 右前轮转向半径：

[0195] 左前轮转向半径：

[0196] 左后轮转向半径：

[0197] 右后轮转向半径：

[0198] 当车辆左转避障时，

[0199] 右前轮转向半径：

[0200] 左前轮转向半径：

[0201] 左后轮转向半径：

[0202] 右后轮转向半径：

[0203] S54，根据各车轮的转弯半径，计算出各车轮的目标转速；

[0204] 其中，各车轮的目标转速公式如下：

[0205] 当车辆右转避障时，

[0206] 右前轮转速：

[0207] 左前轮转速：

[0208] 左后轮转速：

[0209] 右后轮转速：

[0210] 当车辆左转避障时，

[0211] 右前轮转速：

[0212] 左前轮转速：

[0213] 左后轮转速：

[0214] 右后轮转速：

[0215] 在本实施例中，以上计算均基于matlab软件求解，将上诉各车轮转角求解得到的数据及各车轮目标转速的数据保存成生成labview可调用的目标文件，由labview数据通信程序提取相应的目标数据，经labview算法控制程序综合判断，得出相应的运算结果并生成相应的控制命令，通过labview数据通信程序及控制信号输出程序与硬件NIMyrio进行数据通信并传输控制命令。

[0216] labview数据通信程序将labview算法控制程序生成的控制指令导入 myrio控制输出模块，由myrio控制输出模块的NIMyrio接收车载电脑端控制流信号，在myrio内部生成对应的pwm轮毂电机控制信号、pwm转向电机控制信号、制动油路数字开关信号等，通过对应的硬件引脚对外输出控制。

[0217] S6，分别控制各个车轮按对应的转向角和目标转速转动。

[0218] 步骤S6具体包括驱动电机控制和转向电机控制。

[0219] 驱动电机控制，四轮独立控制驱动模块。包含四路独立控制的轮毂电机，每路轮毂电机配备一块轮毂电机驱动板，每路轮毂电机驱动板内都配备有车轮转速传感器输出信号线。现就一路轮毂电机控制方式加以说明，其他三路轮毂电机控制方法与之类似。首先轮毂电机驱动板接收由Myrio控制输出模块对应引脚输出的pwm轮毂电机控制信号，由驱动板内部电路处理并由输出信号线直接控制轮毂电机进行转动，转动的方向由Myrio控制输出模块对应引脚输出的数字开关信号进行判断。控制信号因各车轮的相对位置不同而所要达到的目标转速不同，控制信号根据车辆轨迹坐标随时间变化时刻更新。与此同时车轮转速传感器实时采集当前车轮转速作为反馈控制参量参与运算，控制车轮转速保持理想和稳定。

[0220] 转向电机控制，与第十三步同时进行，四轮独立控制转向模块，包含四路独立控制的转向电机，每路转向电机配备一块电机驱动板及一个车轮转角传感器，用于测量车轮转角作为反馈控制参量。现就一路转向电机控制方式加以说明，其他三路转向电机控制方法与之类似。首先转向电机驱动板接收由Myrio控制输出模块对应引脚输出的pwm控制信号，由驱动板内部电路处理并由输出信号线直接控制转向电机进行转动，转动的方向由Myrio控制输出模块对应引脚输出的数字开关信号进行判断，控制信号因各车轮的相对位置不同而所要达到的目标位置不同，控制信号根据车辆轨迹坐标随时间变化时刻更新。与之同时车轮转角传感器测量的车轮转角信号经由Myrio控制输出模块对应引脚引入，作为反馈控制的参量参与运算，控制车轮稳定保持在理想的位置。

[0221] 具体地，还包括系统的反馈调节一，此时第一次的控制命令已经完成，随后紧接着各传感器采集各自的状态信息，参与系统的反馈调节参量。首先各轮的车速传感器采集各轮的当前转速，各轮的转向角度传感器采集各轮的当前车轮转角，通过myrio硬件接口采集传感器数据，通过myrio内部程序通过数据通信程序导入车载电脑，由车载电脑的传感器数据采集模块及算法控制程序分析当前传感器采集的数据，与目标数据进行对比，当有偏差即当前车轮的执行状态不是目标状态时，算法控制程序通过调节算法如PID算法求得当前输出控制的修正值，修正值与目标控制数据叠加输出，以修正系统偏差。当前车轮位置，稳定在目标位置，算法控制程序直接输出目标控制数据，不需修正，最终使车辆按照预定的轨迹、车速、车身姿态平稳行驶，最终实现车辆四轮独立控制的前提下完成给定的目标任务。

[0222] 系统的反馈调节二，此时第一次的控制命令已经完成，随后紧接着各传感器采集各自的状态信息，参与系统的反馈调节参量。障碍物轨迹规划模块时刻采集周围环境的动态图像，时刻执行第二步所述操作，计算判断障碍物的位置信息并时刻修正目标控制数据，对突发事件如障碍物位置的移动、新增障碍物等突发情况作出相应的警示，控制车辆减速或者停车以规避危险。

[0223] 系统的反馈调节三，此时第一次的控制命令已经完成，随后紧接着各传感器采集各自的状态信息，参与系统的反馈调节参量。车辆定位坐标提取模块时刻采集车辆自身定位信息，时刻执行第三步所述操作，通过算法控制程序计算当前车辆定位坐标与目标车辆定位坐标的偏差值，将偏差带入计算求解出当前输出控制的修正值，修正值与目标控制数据叠加输出，以修正系统偏差。

[0224] 在一次完整的控制过程及反馈调节过程已经完成后，接着重复之前控制命令持续输出对各各执行器的控制指令，同时接受各各传感器的反馈信号，用以修正系统出现的偏差，直至整个系统控制车辆完成预定的目标任务。

[0225] 车辆在完成控制任务的整个过程中，电源管理模块时刻按照各用电模模块的额定工况，合理分配电能，在电池能量不足时及时通过数据采集模块相应的引脚给予用户警示。车辆安全制动模块在当车辆需要减速或者制动时， Myrio控制输出模块对应引脚输出的数字开关信号引入多路继电器控制板，控制多路继电器通断，继电器被控制端分别接电动油泵、两位三通阀、两位两通阀，在Myrio控制信号的作用下，根据工况的不同，控制信号作用效果不同，以适应不同的工况需求。

[0226] 以上依据图式所示的实施例详细说明了本发明的构造、特征及作用效果，以上所述仅为本发明的较佳实施例，但本发明不以图面所示限定实施范围，凡是依照本发明的构想所作的改变，或修改为等同变化的等效实施例，仍未超出说明书与图示所涵盖的精神时，均应在本发明的保护范围内。