CN116165958A - 一种两栖特种无人平台的自动驾驶系统 - Google Patents

Abstract

本发明提供了一种两栖特种无人平台的自动驾驶系统，属于自动驾驶领域。该系统的工作过程为：控制器首先通过网口、串口、CAN总线获取激光雷达点云数据、摄像头图片信息以及车辆底盘反馈信息等，将得到的点云数据与位置信息通过ndt融合定位算法对数据进行融合与校准，得到特种车辆精确位置；然后控制器调用决策控制模块输出宏观的决策指令，最后控制系统将根据环境传感器识别特种车辆所行驶的环境对车辆进行控制，同时通过调用CAN总线通讯模块实时下发车辆转向与油门等控制指令。本发明是结合水陆两栖环境设计的自适应自动驾驶控制系统，解决了目前两栖特种车自动驾驶水陆综合控制等问题。

Description

一种两栖特种无人平台的自动驾驶系统

技术领域

本发明涉及自动驾驶领域，具体涉及一种两栖特种无人平台的自动驾驶系统。

背景技术

随着自动驾驶技术在物流、巡检、接驳等应用场景的快速落地，进而激发了对水陆两栖特定场景的自动驾驶技术研究。但是目前主要研究限于对陆地、空中、水域单方面应用场景，其不同应用领域的控制系统互不相容，水面环境对传感器的感知影响巨大，不能实现陆地自动驾驶控制系统的快速移植。因此，融合陆地和水域的水陆两栖特种无人平台在环境感知、决策控制等方面提出了更高要求。目前自动驾驶技术主要功能是通过激光雷达等传感器实现对地面的环境感知，由于水面光的反射，激光雷达在检测水面障碍物时准确率低、可靠性差；其控制系统并没有融合两栖特种车在水面的运动特点并不能实现水陆两栖协同控制功能。所以，在现有技术中没有一种控制系统能够实现水陆两栖环境感知，也没有一种控制系统能够在水面等地点实现精确自动控制，同时能够精确采集两栖特种车车身姿态与加速度数据信息以判定其在水面真实运动状态。

发明内容

本发明要解决的技术问题是：现有自动驾驶控制系统无法精确识别水面障碍物、水面自动控制精度低、水面定位性能差。

为实现以上技术目的，本发明采用以下技术方案：

一种两栖特种无人平台的自动驾驶系统，包括多源传感器时间同步模块、环境感知定位模块、自主决策模块、运动控制模块；

其中，控制器通过网口、串口以及CAN总线实时获得传感器数据，原始数据首先进入多源传感器时间同步模块；

时间同步后的传感器数据被输入环境感知与定位模块；

在完成环境感知与定位后，自主决策模块根据传感器数据判断两栖特种无人平台当前所行驶的工作环境是水中航行、近岸登陆或陆上行驶，据此作出车辆控制策略；

运动控制模块根据所述车辆控制策略控制两栖特种无人平台行驶。

作为优选，时间同步模块基于ROS中的message_filters，以激光雷达为传感器，每次收到一次雷达数据，便以当前雷达数据时间戳作为要插入的时间点，该时间点另一传感器IMU的数据通过插值获得。

作为优选，插值的流程是先获得主传感器的时间戳，然后根据索引结果获得这一同步时间前后的两帧数据，根据前后两帧数据的采集时刻，以及要插入的时刻，根据比例获得权重得到另一传感器在同步时间的结果。

作为优选，时间同步后的传感器数据被输入环境感知与定位模块以后，视觉感知通过工业级单目摄像头获得实时视频流，基于SSD算法对视频流进行物体检测与跟踪；点云感知模块采用16线激光雷达通过欧式聚类算法获得障碍物信息，采用16线激光雷达ndt点云匹配算法进行定位。

作为优选，自主决策模块使用包含动作短语的有限状态机来实现，有限状态机从一个基础状态出发，将根据不同的驾驶场景跳转到不同的动作状态，将动作短语传递给下层的动作规划层。

作为优选，所述有限状态机的运行流程包括：

1）特种车从环境中观测到高维观测状态，并利用深度学习识别观测信息的特征；

2）通过设定的回报函数评估状态-动作值函数，并把当前策略反馈到环境中；

3）随动作变化而导致特种车与环境交互得到的观测信息发生变化，进入三者的循环最终得出目标的最优策略。

作为优选，运动控制模块在陆地上运动依靠独立驱动电机进行差速驱动，实现直线行驶和转向；在水域依靠喷水推进系统实现前进与转向；通过控制叶轮转速实现纵向运动，通过控制喷口出口处的水流喷出方向实现横向控制；纵向控制通过传感器实时读取车辆当前运动速度以及规划的目标速度作PID闭环控制，输出加速度信息，通过转化模块转化成叶轮转速控制指令。

作为优选，运动控制模块在横向控制采用纯跟踪算法，通过计算角速度控制两栖特种无人平台从当前位置移到其前方的某个预瞄点，该算法利用几何关系计算特种车路径跟踪时的期望航向，通过航向传感器反馈到运动控制器，控制车辆的运动航向实时跟踪期望航向，两栖特种无人平台的位置也不断向规划的路径点靠近，直到路径的终点；

所述纯跟踪算法包括：

1）确定距离目标路点的前视距离

，将/>

设计为特种车纵向速度/>

的线性函数，在不同工况下选择不同的前视距离：/>

为比例系数；

2）计算车身和目标路径点的夹角

；

3）计算圆弧的曲率kappa：

；

4）计算t时刻前轮转角：

,其中L为特种车轴距，/>

为特种车当前姿态和目标点在横向上的误差。

作为优选，运动控制模块针对不同环境中两栖特种无人平台的不同运动方式具有对应的底盘运动控制策略；首先在陆地上通过CAN总线控制转向电机实现两栖特种无人平台转向，控制集成在电机内的驱动器控制电机转速在水域通过CAN总线控制喷水电机喷水口方向实现水中转向，对于电机均采用模糊自适应PID控制方法实现闭环控制。

作为优选，模糊自适应PID控制器的设计方法包括：

1）确定语言变量及变量的模糊化，将特种车运动时的航向偏差e及其变化率e_c作为输入，相应的语言变量定义为{负大（NB）,负中（NM），负小（NS），零（ZO），正小（PS），正中（PM），正大（PB）}，增量参数为

；

2）确定参数自适应的模糊控制规则，特种车在航向控制过程中，根据航向偏差e及其变化率e_c确定Kp、Ki、Kd系数；

其中，所述自适应模糊控制规则包括：

Kp的取值范围为[0，0.3]，Ki 的取值范围为[0，0.06]，Kd 的取值范围为[0，6]；

当航向偏差绝对值|θ|≥10°时，Kp、Ki和Kd取值分别为0.3、0.01和0，若偏差值变化率e_c与航向偏差值e同号，则在取值范围内增大Kd加快系统响应，反之则在取值范围内减小Kd值，单次变化范围为0.05；

航向偏差绝对值5°≤|θ|≤10°时，Kp、Ki和Kd取值分别为0.15、0.03和0，若ec与e异号时，则在取值范围内增大K_d值减小超调，若ec与e同号时，则在取值范围内减小K_d防止偏差值继续增大，单次变化范围为0.05；

航向偏差绝对值|θ|≤5°时，Kp、Ki和Kd取值分别为0.2、0.01和0，若ec与e异号时，则在取值范围内减小K_d值，若ec与e同号时，则在取值范围内增大K_d值，单次变化范围为0.02。

其中，K_p项是误差变化率，K_i项是误差，K_d项是变化率的变化率。

本发明系统基于Linux-ROS操作环境开发，以英伟达计算单元T2X为中央控制器，建立多进程、多线程、多任务的应用架构，外围电路通过搭载CAN总线实现控制系统与特种车线控底盘的通信并完成控制程序指令下发。整个自动驾驶控制系统由车端、控制器、上位机三部分组成，其系统的工作过程为控制器首先通过网口、串口、CAN总线获取激光雷达点云数据、组合导航模块接收到的车辆位置信息、摄像头图片信息以及车辆底盘反馈信息等，将得到的点云数据与位置信息通过ndt融合定位算法对数据进行融合与校准，得到特种车量的精确位置；然后控制器调用决策控制模块输出宏观的决策指令，最后控制系统将根据环境传感器识别特种车辆所行驶的环境对车辆进行控制，控制系统采用模糊PID控制方法无需构建运动学模型即可实现两栖特种车的自主运动控制，两栖特种车底盘采用履带和推进器结合的驱动方式，通过调用CAN总线通讯模块实时下发车辆转向与油门等控制指令，实现两栖特种车在陆地、水面、水中以及水下等环境中执行任务；另外，特种车辆通过CAN总线通讯模块，将特种车辆状态信息实时上传至自动驾驶控制系统中，控制系统对车身数据进行解析处理，通过启动线路和控制线路实现车身冗余控制以消除故障信息。

本发明提供了一种两栖特种无人平台的自动驾驶系统，在功能上实现两栖特种车环境感知、多传感器时间同步、自主决策与控制、线控底盘执行等功能。与现有技术相比，本发明的有益效果为：本发明结合水陆两栖环境设计的自适应自动驾驶控制系统，能够实现两栖特种车在陆地航行、水面航行、近岸登陆等行驶过程，解决了目前两栖特种车自动驾驶水陆综合控制等问题；该系统基于Linux-ROS操作系统，建立分布式、点对点的任务机制，每个节点可分别运行，便于模块化的修改和升级，提高了系统的容错能力。

附图说明

图1为本发明的特种车自动驾驶系统框架图；

图2为本发明的激光雷达ndt定位算法流程图；

图3为本发明的有限状态机流程图。

具体实施方式

以下将对本发明的具体实施方式进行详细描述。为了避免过多不必要的细节，在以下实施例中对属于公知的结构或功能将不进行详细描述。以下实施例中所使用的近似性语言可用于定量表述，表明在不改变基本功能的情况下可允许数量有一定的变动。除有定义外，以下实施例中所用的技术和科学术语具有与本发明所属领域技术人员普遍理解的相同含义。

一种两栖特种无人平台的自动驾驶系统。该系统基于Linux-ROS操作环境开发，以英伟达计算单元T2X为中央控制器，建立多进程、多线程、多任务的自动驾驶系统应用架构，具体实施分为四个部分。附图1为特种车自动驾驶系统框架图：

自动驾驶系统包括多源传感器时间同步模块、环境感知定位模块、自主决策模块、运动控制模块。

第一步，通过网口、串口以及CAN总线实时获得传感器数据，原始数据首先进入多源传感器时间同步模块。特种车自动驾驶控制系统中接入多个传感器，由于传感器之间的采集频率不同，导致无法保证传感器数据同步。时间同步模块基于ROS中的message_filters，以激光雷达为核心传感器，每次收到一次雷达数据，便以当前雷达数据时间戳作为要插入的时间点，该时刻另一传感器IMU的数据通过插值获得。插值的流程是先获得主传感器的时间戳，然后根据索引结果获得这一同步时间前后的两帧数据，根据前后两帧数据的采集时刻，以及要插入的时刻，根据比例获得权重得到另一传感器在同步时间的结果。

第二步，将时间同步后的传感器数据输入环境感知与定位模块。首先，视觉感知通过工业级单目摄像头获得实时视频流，基于SSD算法对视频流进行物体检测与跟踪，对于陆地上的物体目标（比如车辆，行人），全景分割输出其分割Mask，类别和实例ID，对于水面上的非物体目标（比如水中垃圾，其他船只等），则只输出其分割Mask和类别，该模块具体实现流程为：

1）通过opencv将视频转换成图片格式，输入一幅图片，让图片经过卷积神经网络（CNN）提取特征，并生成feature map；

2）抽取其中六层的feature map，然后再feature map 的每个点上生成defaultbox；

3）将生成的所有default box 集合，全部输入到 NMS（极大值抑制）中，输出筛选后的default box。

其次，点云感知模块采用16线激光雷达通过欧式聚类算法获得障碍物信息，其欧式聚类流程如图2所示，首先通过点云降采样的方法减少点云的密度，再使用Kd-tree作为抽取算法的搜索方法：

1）为输入的点云数据集P创建Kd-tree的表示；

2）设置一个空的聚类列表C，以及一个需要被检查的点云队列Q；

3）然后对P中的每一个点p_i进行如下步骤；

4）将p_i添加到当前队列Q；

5）对每个Q中的 p_i 进行如下步骤；

6）设置半径 r<d_th，在此范围内搜索 p_i 的 最近邻 点云集 P_i_k；

7）对于每个最近邻点云集 P_i_k ,检查其中的点是否被处理过，如果没有被处理过，就添加到Q中；

8）当Q中列表中的所有点都被处理完成，把Q添加到聚类列表C中，并将Q清空；

9）当数据集P中的所有点都被处理过，并且成为聚类列表中的一部分。

根据距离的不同使用不同的聚类半径阈值，对聚类完成后的一个点云簇，进一步计算其形心并作为障碍物的中心，同时计算出点云簇的长宽高，输出三维Bounding Box。

特种车采用16线激光雷达ndt点云匹配算法进行定位，结合图2：首先获取激光雷达的两帧点云数据P1,P2，对P1计算正态分布变换；然后初始化估计参数R,t设置为0，根据参数R,t将P2中的所有样本点投影到对应的坐标系下，并确定每一个映射点对应的正态分布P2属于P1中的哪一个栅格，最后评估每个映射点的分布对结果求和来确定参数的得分，通过优化分数来计算新的参数估计，直到满足收敛条件。

第三步，在两栖特种车完成环境感知与定位后，自动驾驶系统中的自主决策模块将根据传感器感知特种车所处行驶环境，判断特种车当前所行驶的工作环境是水中航行、近岸登陆还是陆上行驶三个阶段，作出下一步特种车应该执行的车辆控制策略，自主决策模块使用包含大量动作短语的复杂有限状态机（Finite State Machine，FSM）来实现，有限状态机从一个基础状态出发，将根据不同的驾驶场景跳转到不同的动作状态，将动作短语传递给下层的动作规划层，具体实施如图3所示：

1）特种车从环境中观测到高维观测状态，并利用深度学习识别观测信息的特征；

2）通过设定的回报函数评估状态-动作值函数，并把当前策略反馈到环境中；

3）随动作变化而导致特种车与环境交互得到的观测信息发生变化，进入三者的循环最终得出目标的最优策略。

第四步，考虑到特种车在不同环境下运动时受到各种力或力矩的影响，本专利将设计特种车在不同环境下的运动控制模块。基于两栖特种车辆运动特征，对两栖特种车辆模型进行近似简化，两栖特种车在陆地上运动依靠独立驱动电机进行差速驱动，实现特种车的直线行驶和转向；两栖特种车在水域依靠喷水推进系统实现前进与转向。通过研究其运动特性，控制叶轮的转速可实现特种车辆纵向运动，控制喷口出口处的水流喷出方向实现特种车横向控制。纵向控制通过传感器实时读取车辆当前运动速度以及规划的目标速度作PID闭环控制，输出加速度信息，通过转化模块转化成叶轮转速控制指令；

针对两栖特种车的路径跟踪控制功能，本专利在横向控制采用纯跟踪算法，通过计算角速度控制特种车从当前位置移到特种车前方的某个预瞄点，该算法利用几何关系计算特种车路径跟踪时的期望航向，通过航向传感器反馈到运动控制器，控制车辆的运动航向实时跟踪期望航向，特种车的位置也不断向规划的路径点靠近，直到路径的终点。纯跟踪算法具体设计流程：

1）确定距离目标路点的前视距离

，将/>

设计为特种车纵向速度/>

的线性函数，在不同工况下选择不同的前视距离：/>

为比例系数；

2）计算车身和目标路径点的夹角

；

3）计算圆弧的曲率kappa：

；

4）计算t时刻前轮转角：

,其中L为特种车轴距，/>

为特种车当前姿态和目标点在横向上的误差。

针对不同环境中两栖特种车的不同运动方式设计对应的底盘运动控制策略，首先在陆地上通过CAN总线控制转向电机实现特种车转向，控制集成在电机内的驱动器控制电机转速在水域通过CAN总线控制喷水电机喷水口方向实现水中转向，对于电机均采用模糊自适应PID控制方法实现闭环控制，只需更改控制器的参数就可实现水面的运动控制需求。模糊自适应PID控制器具体设计流程：

1）确定语言变量及变量的模糊化，将特种车运动时的航向偏差e及其变化率e_c作为输入，相应的语言变量定义为{负大（NB）,负中（NM），负小（NS），零（ZO），正小（PS），正中（PM），正大（PB）}，增量参数为

；

2）确定参数自适应的模糊控制规则，特种车在航向控制过程中，根据航向偏差e及其变化率e_c确定Kp、Ki、Kd系数；

其中，所述自适应模糊控制规则包括：

Kp的取值范围为[0，0.3]，Ki 的取值范围为[0，0.06]，Kd 的取值范围为[0，6]；

当航向偏差绝对值|θ|≥10°时，Kp、Ki和Kd取值分别为0.3、0.01和0，若偏差值变化率e_c与航向偏差值e同号，则在取值范围内增大Kd加快系统响应，反之则在取值范围内减小Kd值，单次变化范围为0.05；

航向偏差绝对值5°≤|θ|≤10°时，Kp、Ki和Kd取值分别为0.15、0.03和0，若ec与e异号时，则在取值范围内增大K_d值减小超调，若ec与e同号时，则在取值范围内减小K_d防止偏差值继续增大，单次变化范围为0.05；

航向偏差绝对值|θ|≤5°时，Kp、Ki和Kd取值分别为0.2、0.01和0，若ec与e异号时，则在取值范围内减小K_d值，若ec与e同号时，则在取值范围内增大K_d值，单次变化范围为0.02。

以上对本发明的实施例进行了详细说明，但所述内容仅为本发明的较佳实施例，并不用以限制本发明。凡在本发明的申请范围内所做的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种两栖特种无人平台的自动驾驶系统，其特征在于，包括多源传感器时间同步模块、环境感知定位模块、自主决策模块、运动控制模块；

其中，控制器通过网口、串口以及CAN总线实时获得传感器数据，原始数据首先进入多源传感器时间同步模块；

时间同步后的传感器数据被输入环境感知与定位模块；

在完成环境感知与定位后，自主决策模块根据传感器数据判断两栖特种无人平台当前所行驶的工作环境是水中航行、近岸登陆或陆上行驶，据此作出车辆控制策略；

运动控制模块根据所述车辆控制策略控制两栖特种无人平台行驶。

2.根据权利要求1所述的一种两栖特种无人平台的自动驾驶系统，其特征在于，时间同步模块基于ROS中的message_filters，以激光雷达为传感器，每次收到一次雷达数据，便以当前雷达数据时间戳作为要插入的时间点，该时间点另一传感器IMU的数据通过插值获得。

3.根据权利要求2所述的一种两栖特种无人平台的自动驾驶系统，其特征在于，插值的流程是先获得主传感器的时间戳，然后根据索引结果获得这一同步时间前后的两帧数据，根据前后两帧数据的采集时刻，以及要插入的时刻，根据比例获得权重得到另一传感器在同步时间的结果。

4.根据权利要求1所述的一种两栖特种无人平台的自动驾驶系统，其特征在于，时间同步后的传感器数据被输入环境感知与定位模块以后，视觉感知通过工业级单目摄像头获得实时视频流，基于SSD算法对视频流进行物体检测与跟踪；点云感知模块采用16线激光雷达通过欧式聚类算法获得障碍物信息，采用16线激光雷达ndt点云匹配算法进行定位。

5.根据权利要求1所述的一种两栖特种无人平台的自动驾驶系统，其特征在于，自主决策模块使用包含动作短语的有限状态机来实现，有限状态机从一个基础状态出发，将根据不同的驾驶场景跳转到不同的动作状态，将动作短语传递给下层的动作规划层。

6.根据权利要求5所述的一种两栖特种无人平台的自动驾驶系统，其特征在于，所述有限状态机的运行流程包括：

1）特种车从环境中观测到高维观测状态，并利用深度学习识别观测信息的特征；

2）通过设定的回报函数评估状态-动作值函数，并把当前策略反馈到环境中；

3）随动作变化而导致特种车与环境交互得到的观测信息发生变化，进入三者的循环最终得出目标的最优策略。

7.根据权利要求1所述的一种两栖特种无人平台的自动驾驶系统，其特征在于，运动控制模块在陆地上运动依靠独立驱动电机进行差速驱动，实现直线行驶和转向；在水域依靠喷水推进系统实现前进与转向；通过控制叶轮转速实现纵向运动，通过控制喷口出口处的水流喷出方向实现横向控制；纵向控制通过传感器实时读取车辆当前运动速度以及规划的目标速度作PID闭环控制，输出加速度信息，通过转化模块转化成叶轮转速控制指令。

8.根据权利要求1所述的一种两栖特种无人平台的自动驾驶系统，其特征在于，运动控制模块在横向控制采用纯跟踪算法，通过计算角速度控制两栖特种无人平台从当前位置移到其前方的某个预瞄点，该算法利用几何关系计算特种车路径跟踪时的期望航向，通过航向传感器反馈到运动控制器，控制车辆的运动航向实时跟踪期望航向，两栖特种无人平台的位置也不断向规划的路径点靠近，直到路径的终点；

所述纯跟踪算法包括：

1）确定距离目标路点的前视距离

，将/>

设计为特种车纵向速度/>

的线性函数，在不同工况下选择不同的前视距离：/>

为比例系数；

2）计算车身和目标路径点的夹角

；

3）计算圆弧的曲率kappa：

；

4）计算t时刻前轮转角：

,其中L为特种车轴距，/>

为特种车当前姿态和目标点在横向上的误差。

9.根据权利要求1所述的一种两栖特种无人平台的自动驾驶系统，其特征在于，运动控制模块针对不同环境中两栖特种无人平台的不同运动方式具有对应的底盘运动控制策略；首先在陆地上通过CAN总线控制转向电机实现两栖特种无人平台转向，控制集成在电机内的驱动器控制电机转速在水域通过CAN总线控制喷水电机喷水口方向实现水中转向，对于电机均采用模糊自适应PID控制方法实现闭环控制。

10.根据权利要求9所述的一种两栖特种无人平台的自动驾驶系统，其特征在于，模糊自适应PID控制器的设计方法包括：

1）确定语言变量及变量的模糊化，将特种车运动时的航向偏差e及其变化率e_c作为输入，相应的语言变量定义为{负大NB,负中NM，负小NS，零ZO，正小PS，正中PM，正大PB}，增量参数为

；

2）确定参数自适应的模糊控制规则，特种车在航向控制过程中，根据航向偏差e及其变化率e_c确定Kp、Ki、Kd系数；

其中，所述自适应模糊控制规则包括：

Kp的取值范围为[0，0.3]，Ki 的取值范围为[0，0.06]，Kd 的取值范围为[0，6]；

当航向偏差绝对值|θ|≥10°时，Kp、Ki和Kd取值分别为0.3、0.01和0，若偏差值变化率e_c与航向偏差值e同号，则在取值范围内增大Kd加快系统响应，反之则在取值范围内减小Kd值，单次变化范围为0.05；

航向偏差绝对值5°≤|θ|≤10°时，Kp、Ki和Kd取值分别为0.15、0.03和0，若ec与e异号时，则在取值范围内增大K_d值减小超调，若ec与e同号时，则在取值范围内减小K_d防止偏差值继续增大，单次变化范围为0.05；

航向偏差绝对值|θ|≤5°时，Kp、Ki和Kd取值分别为0.2、0.01和0，若ec与e异号时，则在取值范围内减小K_d值，若ec与e同号时，则在取值范围内增大K_d值，单次变化范围为0.02。

Images