CN111483455B - 一种无人车的自主平滑转弯控制系统及其控制方法 - Google Patents

Abstract

一种无人车的自主平滑转弯控制系统，其特征在于：包括RTK定位模块、惯性测量模块、电机驱动器、ECU控制器、VCU控制器，ECU控制器获取RTK定位模块、惯性测量模块与VCU控制器的数据，结合路径路线，通过融合算法计算无人车的位置、航向、速度数据，从而控制电机驱动器，使无人车在自主模式下沿路线进行运行；自主模式是指，地面站完成路线规划后，将路径路线通过数据链路传输到无人车，无人车按规划的路线自动运行，在转弯时所有运行动作均由无人车自主完成，实现平稳、平滑的转弯动作。

Description

一种无人车的自主平滑转弯控制系统及其控制方法

技术领域

本发明涉及无人车控制系统，是一种无人车的自主平滑转弯控制系统及其控制方法。

背景技术

无人车的平稳转弯控制是无人车领域的一个难点，尤其是锐角的拐点。一般的无人车转弯控制通过L1路径控制方法。这种方法的问题有：

1、由于位置误差的原因，拐弯时动作猛烈

2、拐弯完成后，在航线上左右摆动。

3、剧烈摆动，对差速轮物理结构有损伤。

目前无人车自主转弯主流方案是采用L1控制器，通过位置计算无人车需要移动的横向量，它可以实现转弯，但是由于其算法的限制，转弯时就会导致车辆摆动。

发明内容

本发明为了克服现有技术方案的不足，为实现稳定、平滑、精确的转弯，提供了一种无人车的自主平滑转弯控制系统及其控制方法的技术方案。

为了实现上述目的，本发明的技术方案为：

一种无人车的自主平滑转弯控制系统，包括RTK定位模块、惯性测量模块、电机驱动器、ECU控制器、VCU控制器，

RTK定位模块通过UART接口与ECU控制器电连接，惯性测量模块通过SPI接口与ECU控制器电连接，VCU控制器通过CAN接口与ECU控制器电连接，电机驱动器通过PWM接口与ECU控制器电连接，电机驱动器的动力输出轴与无人车的车轮机械连接，ECU控制器获取RTK定位模块、惯性测量模块与VCU控制器的数据，结合路径路线，通过融合算法计算无人车的位置、航向、速度数据，从而控制电机驱动器，使无人车在自主模式下沿路线进行运行；

自主模式是指，地面站完成路线规划后，将路径路线通过数据链路传输到无人车，无人车按规划的路线自动运行，在转弯时所有运行动作均由无人车自主完成，实现平稳、平滑的转弯动作。

一种无人车的自主平滑转弯控制方法，其特征在于包括如下步骤：

步骤1，初始化，无人车各个模块连接正常；

步骤2，通过测绘获得运行区域坐标后，地面站根据坐标数据规划无人车路线，将路径路线通过数据链路上传到无人车；

步骤3，无人车解锁后由操作人员通过遥控切换到自主模式，无人车将按照路线自动运行；

步骤4，ECU控制器读取RTK定位模块与惯性测量单元数据，通过融合算法获得无人车的位置和航向信息；

步骤5，通过平滑转弯控制算法计算系统输出；

步骤6，根据步骤5得到的系统输出u(t)产生控制量传入差速控制处理单元控制车轮旋转：

本发明与现有技术的有益效果体现在：

1、本发明在各种拐点情况，实现最佳的转弯效果

2、本发明通过RTK航向与惯导数据融合，实现获取稳定准确的位置和航向。

3、本发明通过根据速度与位置实时计算航向控制数据算法，实现航线过程中航向平稳，航迹精度更高。

附图说明

图1为本发明的系统结构图；

图2为本发明的转弯控制器算法原理图；

具体实施方式

下面结合附图与实施例对本发明作进一步的说明。

如图1-2所示，一种无人车的自主平滑转弯控制系统，包括RTK定位模块、惯性测量模块、电机驱动器、ECU控制器、VCU控制器，

RTK定位模块通过UART接口与ECU控制器电连接，惯性测量模块通过SPI接口与ECU控制器电连接，VCU控制器通过CAN接口与ECU控制器电连接，电机驱动器通过PWM接口与ECU控制器电连接，电机驱动器的动力输出轴与无人车的车轮机械连接，ECU控制器获取RTK定位模块、惯性测量模块与VCU控制器的数据，结合路径路线，通过融合算法计算无人车的位置、航向、速度数据，从而控制电机驱动器，使无人车在自主模式下沿路线进行运行。

自主模式是指，地面站完成路线规划后，将路径路线通过数据链路传输到无人车，无人车按规划的路线自动运行，在转弯时所有运行动作均由无人车自主完成，实现平稳、平滑的转弯动作。

RTK定位模块用于获取无人车的位置坐标，计算无人车当前速度；

惯性测量模块用于获取无人车的加速度和角速度，计算无人车的当前欧拉角；

VCU控制器用于获取无人车车轮的转速；

ECU控制器是无人车的控制核心部件，根据所有外部的数据进行数据融合，并根据控制算法计算执行机构的控制量控制无人车稳定工作。本发明的高精度高可靠自主航线运行算法即运行在无人车ECU控制器中。

一种无人车的自主平滑转弯控制方法，其特征在于包括如下步骤：

步骤1，初始化，无人车各个模块连接正常；

步骤2，通过测绘获得运行区域坐标后，地面站根据坐标数据规划无人车路线，将路径路线通过数据链路上传到无人车；

步骤3，无人车解锁后由操作人员通过遥控切换到自主模式，无人车将按照路线自动运行；

步骤4，ECU控制器读取RTK定位模块与惯性测量单元数据，通过融合算法获得无人车的位置和航向信息，具体为：

步骤4.1，预估无人车的当前航向和位置，

z_GPS＝H_GPS·x+R_GPS

R_GPS为位置观测的不确定度，z_GPS为位置观测值，H_GPS为观测矩阵，x状态估计矩阵；

其中，

为位置观测值的协方差矩阵,[]^T为转置，ν_N为北向速度，ν_E为东向速度，ν_D为对地速度，p_N为北向位置，p_E为东向位置。

根据最优估计更新公式：

P_k+1|k+1＝[1-K·H_GPS]·P_k+1|k，

其中，S为协方差，P_k+1|k和P_k+1|k+1为更新的协方差估计，k为当前时间，K转换系数，x_k+1|k+1和x_k+1|k为更新状态估计；

H_GPS·x_k+1|k表示无人车的位置和速度的预测值，根据以上更新公式，计算出最优当前位置；

步骤4.2，根据RTK模块航向数据通过惯导数据处理计算当前的航向：

其中，ψ为测量点航向；m_x2、m_y2、m_z2分别为惯导融合的航向X轴、Y轴、Z轴的数据，φ、θ为俯仰和滚转角度；

步骤4.3，通过导航系统计算目标航向角度ψ_T，

off_x＝next_pos.lng-curr_lng，

ψ_T＝atan2f(-off_y，off_x)*5729.57795f

其中，next_pos.lng和next_pos.lat分别为目标经度、纬度，curr_lng和curr_pos.lat分别为当前经度和纬度；off_x和off_y为位置偏移量，_scale为位置转换比例因子，5729.57795f为单位转换比例因子，ψ_T为目标航向角度，atan2f为三角函数；

步骤5，通过平滑转弯控制算法计算系统输出；

当无人车进入转弯时，其控制算法如下：

t_rate(t)＝(ψ_T-ψ)*0.2，

其中，t_rate(t)为目标航向角速率，0.2为转换系数；

e(t)＝t_rate(t)-c_rate(t)，

其中，e(t)为航向角速率误差，c_rate(t)当前时刻航向；

其中，u(t)为系统输出，K_p、K_i、K_d分别为比例、积分和微分系数；

此外，对于数字控制系统，可将PID控制器离散化，得到离散结构形式：

其中，

为所有误差值累加之和，Δ为等效微分运算，Δe(t)＝e(t)-e(t-1)，当期望值在相邻的采样周期保持不变时，e(t)＝e(t-1)，Δe(t)即为系统输出的变化量，经过离散的处理后u(t)更平滑；

步骤6，根据步骤5得到的系统输出u(t)产生控制量传入差速控制处理单元控制车轮旋转：

无人车为差速无人车，通过控制两侧车轮转速实现无人车的前进和转向的，在转弯阶段，通过使两个车轮产生转速差，实现转弯。对差速车辆建模，模型将左右轮的控制量进行合理分配，建模公式如下：

thr_scaled油门比例系数，yaw_scaled航向比例系数，motor_left左电机输出混控总比例系数，motor_right右电机输出混控总比例系数；motor_left_out左电机最终控制量，motor_right_out右电机最终控制量。

以上所述实施方式仅表达了本发明的一种实施方式，但并不能因此而理解为对本发明范围的限制。应当指出，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

Claims (7)

1.一种无人车的自主平滑转弯控制系统，其特征在于：包括RTK定位模块、惯性测量模块、电机驱动器、ECU控制器、VCU控制器，

RTK定位模块通过UART接口与ECU控制器电连接，惯性测量模块通过SPI接口与ECU控制器电连接，VCU控制器通过CAN接口与ECU控制器电连接，电机驱动器通过PWM接口与ECU控制器电连接，电机驱动器的动力输出轴与无人车的车轮机械连接，ECU控制器获取RTK定位模块、惯性测量模块与VCU控制器的数据，结合路径路线，通过融合算法计算无人车的位置、航向、速度数据，从而控制电机驱动器，使无人车在自主模式下沿路线进行运行；

其中，自主模式是指，地面站完成路线规划后，将路径路线通过数据链路传输到无人车，无人车按规划的路线自动运行，在转弯时所有运行动作均由无人车自主完成，实现平稳、平滑的转弯动作。

2.根据权利要求1所述的一种无人车的自主平滑转弯控制系统，其特征在于：

RTK定位模块用于获取无人车的位置坐标，计算无人车当前速度；

惯性测量模块用于获取无人车的加速度和角速度，计算无人车的当前欧拉角；

VCU控制器用于获取无人车车轮的转速；

ECU控制器是无人车的控制核心部件，根据所有外部的数据进行数据融合，并根据控制算法计算执行机构的控制量控制无人车稳定工作。

3.一种根据权利要求1或2所述的无人车的自主平滑转弯控制系统的自主平滑转弯控制方法，其特征在于包括如下步骤：

步骤1，初始化，无人车各个模块连接正常；

步骤2，通过测绘获得运行区域坐标后，地面站根据坐标数据规划无人车路线，将路径路线通过数据链路上传到无人车；

步骤3，无人车解锁后由操作人员通过遥控切换到自主模式，无人车将按照路线自动运行；

步骤4，ECU控制器读取RTK定位模块与惯性测量单元数据，通过融合算法获得无人车的位置和航向信息；

步骤5，通过平滑转弯控制算法计算系统输出；

步骤6，根据步骤5得到的系统输出产生控制量传入差速控制处理单元控制车轮旋转，实现转弯。

4.根据权利要求3所述的自主平滑转弯控制方法，其特征在于步骤4具体为：

步骤4.1，预估无人车的当前航向和位置，

z_GPS＝H_GPS·x+R_GPS，

其中，R_GPS为位置观测的不确定度，z_GPS为位置观测值，H_GPS为观测矩阵，x状态估计矩阵；

则，

其中，

为位置观测值的协方差矩阵,[]^T为转置，ν_N为北向速度，ν_E为东向速度，ν_D为对地速度，p_N为北向位置，p_E为东向位置

根据最优估计更新公式：

P_k+1|k+1＝[1-K·H_GPS]·P_k+1|k，

其中，S为协方差，P_k+1|k和P_k+1|k+1为更新的协方差估计，k为当前时间，K转换系数，x_k+1|k+1和x_k+1|k为更新状态估计；

H_GPS·x_k+1|k表示无人车的位置和速度的预测值，根据以上更新公式，计算出最优当前位置；

步骤4.2，根据RTK模块航向数据通过惯导数据处理计算当前的航向：

其中，ψ为测量点航向；m_x2、m_y2、m_z2分别为惯导融合的航向X轴、Y轴、Z轴的数据，φ、θ为俯仰和滚转角度；

步骤4.3，通过导航系统计算目标航向角度ψ_T，

off_x＝next_pos.lng-curr_lng，

ψ_T＝atan2f(-off_y，off_x)*5729.57795f

其中，next_pos.lng和next_pos.lat分别为目标经度、纬度，curr_lng和curr_pos.lat分别为当前经度和纬度；off_x和off_y为位置偏移量，_scale为位置转换比例因子，5729.57795f为单位转换比例因子，ψ_T为目标航向角度，atan2f为三角函数。

5.根据权利要求3所述的自主平滑转弯控制方法，其特征在于步骤5具体为：当无人车进入转弯时，其控制算法如下：

t_rate(t)＝(ψ_T-ψ)*0.2，

其中，t_rate(t)为目标航向角速率，0.2为转换系数；

e(t)＝t_rate(t)-c_rate(t)，

其中，e(t)为航向角速率误差，c_rate(t)当前时刻航向；

通过PID控制器计算输出，

其中，u(t)为系统输出，K_p、K_i、K_d分别为比例、积分和微分系数。

6.根据权利要求5所述的自主平滑转弯控制方法，其特征在于为：

对于数字控制系统，将PID控制器离散化，得到离散结构形式：

其中，

为所有误差值累加之和，Δ为等效微分运算，Δe(t)＝e(t)-e(t-1)，当期望值在相邻的采样周期保持不变时，e(t)＝e(t-1)，Δe(t)即为系统输出的变化量，经过离散的处理后u(t)更平滑。

7.根据权利要求5或6所述的自主平滑转弯控制方法，其特征在于为步骤6具体为：

无人车为差速无人车，通过控制两侧车轮转速实现无人车的前进和转向的，在转弯阶段，通过使两个车轮产生转速差，实现转弯，对差速车辆建模，模型将左右轮的控制量进行合理分配，建模公式如下：

thr_scaled油门比例系数，yaw_scaled航向比例系数，motor_left左电机输出混控总比例系数，motor_right右电机输出混控总比例系数；motor_left_out左电机最终控制量，motor_right_out右电机最终控制量。

Images