CN107521496B - 一种无人车辆的横纵向协调控制轨迹跟随控制方法 - Google Patents

Abstract

本发明公开了一种无人车辆的横纵向协调控制轨迹跟随控制方法，其具体方法为：控制方法中的控制***包括：信息接收模块，计算求解模块和控制命令发送模块，具体步骤为：步骤一、信息的接收，通过信息接收模块进行信息的接收，步骤二、求解计算：利用计算求解模块进行求解；步骤三、控制命令发送模块首先将前轮转角和车轮驱动/制动力矩转化为执行机构的直接控制量；有益效果：采用横纵向协调控制，建立了无人车辆横纵向运动关系，相对于横向和纵向单独控制，消除了横纵向运动耦合带来的控制误差，增加了控制***的鲁棒性。

Description

一种无人车辆的横纵向协调控制轨迹跟随控制方法

技术领域

本发明涉及无人车辆控制技术领域，具体涉及一种无人车辆的横纵向协调控制轨迹跟随控制方法。

背景技术

目前，无人驾驶车辆理论上具有降低交通事故率、降低交通事故死亡率，提高能源利用率，缓解交通拥堵、提高道路通行量，减少个人通勤时间，给更多人提供方便，提高社会生产力等优点，成为国内外各大研究机构和科技企业的研究热点。无人驾驶车辆可以分为感知、决策和控制三个层面，在控制层中，需要将决策层输出的车辆轨迹转化为控制量从而使车辆能够精确跟随规划的轨迹。一般的横向或纵向控制多采用PID控制方法，虽然结构简单易于实现，但是存在依赖工程经验，需要调试过程等缺点，对于无人车辆来说显然存在很大的不合理，由于缺乏具体的模型，PID控制也很难做到控制的精确；PID控制的控制效果严重依赖于参数，因此对于控制车辆车速变化具有较差的鲁棒性。此外，现有的大多控制一般在控制层的策略为横向和纵向独立控制，但是车辆的横向和纵向运动具有较强的耦合性，忽略车辆自身横纵向运动的耦合进行独立控制往往也降低了控制的精确性，因此在独立控制中存在控制精度较差等问题。

发明内容

本发明的目的是为了解决在无人驾驶车辆控制过程中所存在的诸多问题而提供的一种无人车辆的横纵向协调控制轨迹跟随控制方法。

本发明提供的无人车辆的横纵向协调控制轨迹跟随控制方法，其具体方法如下：

控制方法中的控制***包括：信息接收模块，计算求解模块和控制命令发送模块，具体步骤如下：

步骤一、信息的接收，通过信息接收模块进行信息的接收，信息接收模块分为两部分，一部分为无人车未来行驶轨迹接收部分，提供和决策层的接口，接受无人车辆决策***输出的未来行驶的轨迹，然后将接收到的包含速度和相对大地坐标系位置、航向角的连续轨迹数据根据控制周期进行离散化后发送给计算求解模块；另一部分为车辆当前状态信息接收模块，接收传感器自身获取并发送的车辆相对大地坐标系的实际位置、车辆横纵向速度、横纵向加速度、车辆相对大地坐标系的航向角、车辆横摆角速率，然后将接收到的这些传感器信息利用卡尔曼滤波器滤除噪声后，传送给计算求解模块；

步骤二、计算求解：利用计算求解模块进行求解，利用经过简化的车辆两轮动力学模型和模型预测控制算法将信息接收模块传递来的轨迹信息和车辆状态信息，经过车辆状态预测、优化求解、反馈机制后求出控制时域内的最优解：前轮转角和车轮所需制动/驱动力矩，然后将求出的控制时域内的下一时刻的前轮转角增量和车轮驱动/制动力矩增量进行下一步计算，计算求解模块主要分为三部分，第一部分根据信息接收模块传递的当前车辆状态信息和车辆两轮模型的动力学分析建立准确的车辆动力学模型，根据所建立的动力学模型计算出预测时域内的车辆状态；第二部分根据信息接收模块传递的决策轨迹和预测的车辆状态做时间线对齐后计算出预测的控制偏差；第三部分为求解部分，根据上述得出的控制偏差和车辆动力学模型以及车辆自身的运动学和动力学约束利用二次规划进行求解，得出车辆横向控制所需的控制时域内的前轮转角增量序列和纵向控制所需的车轮驱动/制动转矩增量序列，之后将求出的控制时域内的下一时刻的前轮转角增量和车轮驱动/制动力矩增量传递给控制命令发送模块；

步骤三、控制命令发送模块首先将前轮转角增量和车轮驱动/制动力矩增量累加得到前轮转角和车轮驱动/制动绝对量，然后将前轮转角和车轮驱动/制动的绝对量转化为执行机构的直接控制量：方向盘转角和发动机扭矩或制动缸压力，然后打包通过控制局域网络发送给执行机构，从而通过对车辆横纵方向的协调控制实现对规划决策的轨迹准确跟随，通过各模块的协同工作，使得车辆能够准确跟踪决策层输出的轨迹。

上述方法中的信息接收模块，计算求解模块和控制命令发送模块均为现有设备的组装，因此，具体型号和规格没有进一步进行赘述。

本发明的有益效果：

本发明采用车辆两轮动力学模型对无人车辆进行精确的***建模，相对与传统的PID控制算法因为拥有了较为精准的模型，减少了大量的参数整定过程，节约了大量的人力物力，采用模型预测控制算法求解控制时域内的最优解，增加了控制***的精确度，采用横纵向协调控制，建立了无人车辆横纵向运动关系，相对于横向和纵向单独控制，消除了横纵向运动耦合带来的控制误差，增加了控制***的鲁棒性。

附图说明

图1为本发明所述控制方法流程图。

图2为无人车辆两轮简化模型示意图。

具体实施方式

请参阅图1至图2所示：

首先为信息的接收；信息接收模块分为两部分，一部分为无人车未来行驶轨迹接收部分，提供和决策层的接口，接受无人车辆决策***输出的未来行驶的轨迹，然后将接收到的包含速度和相对大地坐标系位置、航向角的连续轨迹数据根据控制周期进行离散化后发送给计算求解模块；另一部分为车辆当前状态信息接收模块，接收传感器自身获取并发送的车辆相对大地坐标系的实际位置、车辆横纵向速度、横纵向加速度、车辆相对大地坐标系的航向角、车辆横摆角速率，然后将接收到的这些传感器信息利用卡尔曼滤波器滤除噪声后，传送给计算求解模块。

然后是计算求解；首先以前轮驱动车辆两轮模型做动力学分析。采用车辆两轮模型的前提条件是做如下假设：(1)忽略悬架的作用，认为汽车车厢只作平行于地面的平面运动，即采用附图1中的车辆坐标系，汽车沿z轴的位移，绕y轴的俯仰和绕x轴的侧倾角均为零；(2)忽略转向***的影响，以前轮转角作为输入；(3)车辆的侧向加速度在0.4g范围内，轮胎的侧偏特性处于线性区域内；(4)忽略纵向和横向的空气动力学；(5)车辆的行驶速度变化缓慢，忽略前后轴的载荷转移。

在上述假设下做车辆两轮如附图2。图中坐标系oxyz固定于车身，原点o固定于车辆质心位置，xoz处于车辆左右对称的平面内，x轴为车辆纵轴，y轴与车辆纵轴垂直，z轴满足右手准则。坐标系OXY为大地坐标系。前后轮分别建立各自的xy轴。在附图2中进行简化后的前后车轮进行受力分析：

F_fl、F_fc、F_fx、F_fy分别为车辆前轮坐标系中的纵向、侧向、x轴、y轴方向上的受力；

F_rl、F_rc、F_rx、F_ry分别为车辆后轮坐标系中的纵向、侧向、x轴、y轴方向上的受力；

δ为前轮转角，α_f、α_r分别为前后轮的轮胎侧偏角；

v_f、v_r分别为前后轮的速度；

x′、y′分别为车辆x轴、y轴方向上的速度；

a、b分别为车辆质心到前后轴的距离。

在车辆x轴方向上，

在车辆y轴方向上，

在车辆z轴方向上，

上式中，I_z为车辆绕z轴的转动惯量。

在轮胎坐标系中，x轴方向、y轴方向上的力与纵向力、侧向力的转换关系为：

F_fx＝F_flcosδ-F_fcsinδ (公式4)

F_fy＝F_flsinδ+F_fccosδ (公式5)

F_rx＝F_rlcosδ+F_rcsinδ (公式6)

F_ry＝F_rccosδ-F_rlsinδ (公式7)

上述公式中，δ为对应车轮x轴与车辆x轴的夹角，即车轮的转角。

轮胎的纵向力、侧向力可以表示为轮胎侧偏角、滑移率、路面系数和垂直载荷等参数的复杂函数：

F_l＝f_l(α,s,μ,F_z) (公式8)

F_c＝f_c(α,s,μ,F_z) (公式9)

上述公式中中，α为轮胎侧偏角，s为滑移率，μ为路面摩擦系数，F_z为轮胎所受到的垂直载荷。在车辆两轮模型的简化中，忽略前后轴质量的转移，则前后轴的垂直载荷为：

F_fz＝(bmg)/(a+b) (公式10)

F_rz＝(amg)/(a+b) (公式11)

Pacejka提出的魔术公式表述了轮胎的纵向力F_l，侧向力F_c，回正力矩M_z，翻转力矩M_x，阻力矩M_y与侧偏角α，滑移率s之间的定量关系，以及纵向力、侧向力的联合作用工况，能够表达不同驱动情况时的轮胎特性。魔术公式的一般表达式为：

Y(x)＝Dsin{Ctan^-1[Bx-E(tan^-1(Bx))]} (公式12)

上式中，系数B、C、D由轮胎的垂直载荷和外倾角确定；Y为输出变量，可以为纵向力F_l或侧向力F_c或回正力矩；x为输入变量，在不同的情况下分别表示轮胎的侧偏角α或纵向滑移率s；B为刚度因子，C为形状因子；D为峰值因子，E为曲率因子。在实际应用中，由于帘布层转向效应、侧偏力作用或滚动阻力会引起便宜，通常还会引入垂直偏移S_v和水平偏移S_h。

轮胎纵向力计算方法：

F_l＝D_lsin[C_ltan^-1(B_lx_l-E_l(B_lx_l-tan^-1(B_lx_l)))]+S_v (公式13)

上式中，x_l＝s+S_h，s为纵向滑移率，计算方法见(公式16)。S_h为曲线的水平方向漂移，S_v为曲线的垂直方向的漂移。C_l为曲线的形状因子，C_l＝B₀；D_l为曲线峰值因子，表示曲线的最大值，其中F_z为轮胎受到的垂直载荷；B_l为刚度因子，E_l为曲线的曲率因子，表示曲线最大值附近的形状，S_h为曲线水平方向的漂移：S_h＝B₉F_z+B₁₀；S_v为曲线垂直方向漂移：S_v＝0。

轮胎侧向力计算方法：

F_c＝D_c sin[C_ctan^-1(B_cx_c-E_c(B_cx_c-tan^-1(B_cx_c)))]+S_v (公式14)

上式中，x_c＝α+S_h,α为轮胎侧偏角。S_h为曲线的水平方向漂移，S_v为曲线的垂直方向的漂移。C_c为曲线的形状因子，C_c＝A₀；D_c为曲线峰值因子，表示曲线的最大值，其中F_z为轮胎受到的垂直载荷；B_c为刚度因子，B_c＝A₃sin(2tan^-1F_z/A₄)*(1-A₅|γ)/(C_c*D_c)，其中γ为轮胎外倾角；E_c为曲线的曲率因子，S_h为曲线水平方向的漂移：S_h＝A₉F_z+A₁₀+A₈γ；S_v为曲线垂直方向漂移：S_v＝A₁₁F_zγ+A₁₂F_z+A₁₃。A₀-A₁₃和B₁-B₁₀为常数。

轮胎侧偏角的计算公式为：

α＝tan^-1(v_c/v_l) (公式15)

纵向滑移率的计算：

上式中，v_l为纵向车轮速度，r和ω分别为车轮半径和角速度。

对车轮进行受力分析得到：

I_dfω′_f＝-F_flr+T_f-C_wfω_f (公式17)

I_drω_r′＝-F_rlr+T_r-C_wrω_r (公式18)

上式中，I_df为前轮的转动惯量，T_f为作为驱动轮的前轮的驱动力矩或制动力矩，C_wf为前轮及传动系的阻尼系数；I_dr为后轮的转动惯量，T_r为作为从动轮的后轮的制动力矩(T_r≤0)，C_wf为后轮及半轴的阻尼系数。

在(公式15)和(公式16)中，v_l和v_c为纵向和侧向车轮速度，可以表示为：

v_l＝v_ysinδ+v_xcosδ (公式19)

v_c＝v_ycosδ-v_xsinδ (公式20)

在车辆模型中，

v_fx＝x′ v_rx＝x′ (公式22)

在大地坐标系中存在如下关系：

Y′＝x′sinψ+y′cosψ (公式23)

X′＝x′cosψ-y′sinψ (公式24)

根据(公式1)-(公式24)，建立基于车辆两轮模型的动力学状态方程：

ξ′(t)＝f[ξ(t),u(t)] (公式25)

状态控制量u＝[δ_f,T_f,T_r]。

输出方程：

η(t)＝C(t)ξ(t) (公式26)

输出状态量η＝[Y,X,ψ]。

根据上述建立的车辆动力学非线性方程采用模型预测算法对车辆进行横纵向协调控制。

受限于现有计算机的计算能力，对于非线性模型的模型预测控制算法计算周期远远大于无人车跟踪控制所需的控制周期，因此，对上述非线性动力学模型进行线性化，得到线性时变方程：

ξ′(t)＝A(t)ξ(t)+B(t)u(t) (公式27)

式中，

在计算机中计算需要对上述线性连续方程进行离散化处理，得到：

ξ(k+1)＝A(k)ξ(k)+B(k)u(k) (公式28)

式中，A(k)＝I+TA(t)，B(k)＝TB(t)。

在模型预测控制的优化求解中需要建立约束条件：

(1)动力学模型为前驱车辆，所以后轮驱动力矩为0，制动力矩限定在车辆制动性能范围内，即：

-T_t,bmax≤T_r≤0 (公式29)

(2)前轮转角约束量：

δ_f,min≤δ_f≤δ_f,max (公式30)

(3)前轮转角增量限制：

δ_min≤δ≤δ_max (公式31)

(4)前轮制动/驱动力矩限制：

-T_f,bmax≤T_f≤T_f,dmax (公式32)

建立目标函数：

上式中，N_p为预测时域，N_c为控制时域，ρ为权重系数，ε为松弛因子。

综合上述目标函数和约束条件，基于车辆两轮模型的横纵向协调控制轨迹跟随***在每个控制周期内需要解决的问题为如下二次型优化问题：

s.t.:ΔU_min≤ΔU≤ΔU_max

完成上述求解后，得到控制时域内的一系列控制输入增量：

将上述控制序列中的第一个元素作为当前周期的实际控制量传递给命令发送模块，命令发送模块经过打包后发送给执行器，从而作用于车辆。

进入下一个控制周期后，重复求解上述优化问题，执行循环从而实现对期望轨迹的横纵向协同跟踪控制。

命令发送模块接收到的控制量为u(k)＝[δ_f,T_f,T_r]，δ_f为前轮转角，经过转向***反向计算后得出方向盘转角：

δ_w＝i_sδ_f (公式37)

上式中，i_s为转向***的传动比。

T_f为前轴的驱动(T_f＞0)或制动(T_f＜0)力矩，经过传动***的解算后得到动力机(发动机或电机)扭矩，或前轴制动缸压力

上式中，i_d为传动***的传动比，k_f为前轴制动缸压力换算系数。

T_f为后轴的制动力矩，

P_cyr＝k_rT_f (公式39)

上式中k_r为前轴制动主缸压力换算系数。

命令发送模块通过控制局域网络将方向盘转角δ_f、动力机请求扭矩T_req或制动缸压力P_cyr、P_cyf发送给对应的执行机构，完成一个控制周期的控制过程。

Claims (1)

1.一种无人车辆的横纵向协调控制轨迹跟随控制方法，其特征在于：其具体方法如下：

控制方法中的控制***包括：信息接收模块，计算求解模块和控制命令发送模块，具体步骤如下：

步骤一、信息的接收，通过信息接收模块进行信息的接收，信息接收模块分为两部分，一部分为无人车未来行驶轨迹接收部分，提供和决策层的接口，接受无人车辆决策***输出的未来行驶的轨迹，然后将接收到的包含速度和相对大地坐标系位置、航向角的连续轨迹数据根据控制周期进行离散化后发送给计算求解模块；另一部分为车辆当前状态信息接收模块，接收传感器自身获取并发送的车辆相对大地坐标系的实际位置、车辆横纵向速度、横纵向加速度、车辆相对大地坐标系的航向角、车辆横摆角速率，然后将接收到的这些传感器信息利用卡尔曼滤波器滤除噪声后，传送给计算求解模块；

步骤二、计算求解：利用计算求解模块进行求解，利用经过简化的车辆两轮动力学模型和模型预测控制算法将信息接收模块传递来的轨迹信息和车辆状态信息，经过车辆状态预测、优化求解、反馈机制后求出控制时域内的最优解：前轮转角和车轮所需制动/驱动力矩，然后将求出的控制时域内的下一时刻的前轮转角增量和车轮驱动/制动力矩增量进行下一步计算，计算求解模块主要分为三部分，第一部分根据信息接收模块传递的当前车辆状态信息和车辆两轮模型的动力学分析建立准确的车辆动力学模型，根据所建立的动力学模型计算出预测时域内的车辆状态；第二部分根据信息接收模块传递的决策轨迹和预测的车辆状态做时间线对齐后计算出预测的控制偏差；第三部分为求解部分，根据上述得出的控制偏差和车辆动力学模型以及车辆自身的运动学和动力学约束利用二次规划进行求解，得出车辆横向控制所需的控制时域内的前轮转角增量序列和纵向控制所需的车轮驱动/制动转矩增量序列，之后将求出的控制时域内的下一时刻的前轮转角增量和车轮驱动/制动力矩增量传递给控制命令发送模块；

步骤三、控制命令发送模块首先将前轮转角增量和车轮驱动/制动力矩增量累加得到前轮转角和车轮驱动/制动绝对量，然后将前轮转角和车轮驱动/制动的绝对量转化为执行机构的直接控制量：方向盘转角和发动机扭矩或制动缸压力，然后打包通过控制局域网络发送给执行机构，从而通过对车辆横纵方向的协调控制实现对规划决策的轨迹准确跟随，通过各模块的协同工作，使得车辆能够准确跟踪决策层输出的轨迹。