CN113978467A - 一种基于正弦函数的车辆自动变道控制方法 - Google Patents

Abstract

本发明涉及汽车电子技术领域，具体的说，是涉及一种基于正弦函数的车辆自动变道控制方法。本案是基于实际行车工况中，当驾驶员左变道，方向盘控制是先左转、再回正，在此过程中，方向盘角度变化和车辆转向变化趋式一致，且近似于“S”形这一出发点，设计出的基于正弦函数的自动变道控制方法。本发明通过距离传感器和视觉传感器实时测量前方车辆信息，侧后方来车信息、以及车道信息等，然后基于本专利的控制方法，规划出合适的行车轨迹，并根据行车环境，实时动态修正，最终实现安全、舒适地变道。

Description

一种基于正弦函数的车辆自动变道控制方法

技术领域

本发明涉及汽车电子技术领域，具体的说，是涉及一种基于正弦函数的车辆自动变道控制方法。

背景技术

当前高精度传感器、高算力芯片、物联网、云计算、大数据、人工智能为代表的信息技术广泛运用，正加速社会的智能化发展，汽车交通领域智能化的进程亦随之演化，其中ADAS/AD(高级辅助驾驶***/自动驾驶)是汽车行业的重点创新领域。

在ADAS(高级辅助驾驶***)进化到AD(自动驾驶)过程中，横向和纵向的深度耦合，是高阶功能实现的基础，而自动变道辅助(ALC)是具体应用之一。

目前，大部分自动变道辅助(ALC)实现策略，如等速偏移变道、圆弧变道、梯形加速度变道等，不符合实际驾驶***滑性差，体验不友好；S型函数变道策略，运算量较大，且切入和切出需要特殊处理；多项式函数变道策略，运算量大，边界条件设置复杂。因此，本专利以正弦函数为基础，设计一种自动变道辅助(ALC)实现策略，以消除或者改善前述各策略的不足之处。

本专利涉及的自动变道辅助(ALC)实现策略，是通过距离传感器(如雷达)和视觉传感器(如摄像头)实时测量前方车辆信息(距离、速度)，侧后方来车信息、以及车道信息等，然后基于本专利的策略，规划出合适的行车轨迹，并根据行车环境，实时动态修正，最终实现安全、舒适地变道。

发明内容

本发明的目的在于克服现有技术的不足，适应现实需要，提供一种基于正弦函数的车辆自动变道控制方法。

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

一种基于正弦函数的车辆自动变道控制方法，包括如下步骤：

S1.基础模型搭建：

在实际行车工况中，当驾驶员左变道，方向盘控制是先左转、再回正，在此过程中，方向盘角度变化和车辆转向变化趋式一致，且近似于“S”形；

S11.基本轨迹模型：以正弦函数为车辆运行轨迹模型，

y＝sin(x)x∈[-π/2,π/2] (式1)；

式1的一阶导数，

该式算出正弦曲线上的各处斜率；

S12.依据现实环境变形后的轨迹模型：

将基础模型进行扩展变形，以涵盖关键参数，如下：

y＝Asin(ωx+θ)+B(式3)

其中，A为正弦函数的幅值，定义SLW为本车道宽度，LLW为左车道宽度，正弦函数的幅值

以车辆行进方向为纵向；

y为本车变道过程中的横向距离，开始变道时，y为0；

x为本车变道过程中的纵向距离，与行进方向一致，开始变道时，x为0；

车辆轨迹投影在X轴上的距离为Dmax，为完成变道所需最大的纵向距离，Dmax＝V*T；

ω为转换系数，以车辆行驶即X轴方向的纵向距离为参数(X)，代入正弦函数，需用公式

转换为角度，即

B＝A，sin曲线沿Y正轴平移A，确保变道起始时，y＝0；

θ＝-π/2，sin曲线沿X正轴平移π/2，确保变道起始时，x＝0；

公式(3)的一阶导数为：y’＝Aωcos(ωx+θ)(式4)

S2.约束条件

S21.变道车速设定：

当前车速v_x大于变道使能车速V_enable，且稳定追随前车，与前车的速度差ΔV<ΔV_thr，且与前车的安全距离差ΔD<ΔD_thr,持续一段时间t,t≥3s，***将以当前车速v_x作为纵向车速开始变道，实际车速v在变道过程中，由正弦函数求解；其中，ΔV_thr为启动自动变道时与前车的速度差的最大值，ΔD_thr为启动自动变道时与前车的安全距离的最大值；

S22.变道产生的最大的纵向距离D_max：

变道完成时间T_end与预期的纵向车速v_x乘积为变道产生的最大纵向距离D_max，即

D_max＝v_x×T_end，正弦函数变量x∈[0,D_max]；

其中，变道完成时间T_end为5秒～8秒，为标定量；

S23.变道转向轨迹的最大斜率k_max：

k_max＝Aω，不同速度区间设定上限值k_limit，当k_max>k_limit，禁止变道；

S24.本车车道线和左车车道线可见，宽度∈(3.75m+Δe)，曲率半径大于设定阈值L_{max_radius}；

S25.左车道，前方无车或者左前车与本车的车距满足安全车间时距，允许变道；

车间时距算法为D_w＝T_h*Vs_s-K*(V_f-V_s)+D₀，

其中，D_W为当前车间距离，即前方或左方或左前方车辆与本车的距离；

V_s为本车车速；

T_h为车间时距，即本车与前方或左方或左前方车辆相距的时间，即以本车当前速度到达前方或左方或左前方车辆现在位置所需要的时间。

V_f＝前车车速；

D₀为最小安全车距，是标定量；K为调节系数，为标定量；

S26.左车道，后方、左后方无车或者左侧后车与本车预碰撞时间大于预设阈值RCT_TTC，允许变道；RCT_TTC是标定量；

上述条件S21-S26均满足，则进入步骤S3，开始变道；

S3.假定整个工况是无损耗的理想状态，车辆纵向速度v_x恒定，即可根据T的时刻状态，结合正弦函数，推导出T+1状态，以及所需要控制量；

T时刻，纵向车速为vx_t＝v_x，纵向距离X_t，可预期T+1时，纵向距离和横向距离：

X_t+1＝X_t+vx_t*ΔT (5)，

Yt₊₁＝Asin(ω*X_t+1+θ)+B (6)

将式(6)代入式(4)，得T+1时，正弦曲线的切线斜率，即车辆形式轨迹的切线斜率：

进而可推导出切线与X轴的夹角，即T+1时刻横摆角γ_t+1：

已设定纵向速度恒定为v_x，实际车速v_r分解为沿X轴的纵向车速v_x和沿Y轴的横向车速v_y，在整个变道过程，策略要求v_x恒定不变，则v_r会随着横摆角变化而变化则在T+1 时刻，车速(vr_t+1)为：

进而可推导出速度控制量ACC，如下：

vr_t为T时刻的车速；

Δγ＝γ_t+1-γ_t，其中，γ_t为车辆在T时刻的横摆角，

即

δ为转向车轮偏角，L为车辆轴距；

T+1时刻，方向盘角度：S＝map(δ)，即得到转向控制量。

还包括如下补偿修正步骤：

T时刻，根据车道线信息，得当前横向距离γr_t，同时由反正弦函数得纵向距离:

由式(5)，可在T-1时刻，预期T时刻的纵向距离Xp_t，结合式(10)，得纵向距离误差ΔX＝(Xr_t-Xp_t)。

当X_hth>ΔX＝(Xr_t-Xp_t)>X_lth时，需要进行补偿操作；

X_lth为差误阈值；X_hth为有效误差阈值，当ΔX>X_hth可认为差误不可信；

为补偿ΔX，需T至T+1时刻，预期的纵向位移应该为ΔX_t+1＝ΔX+vx_t*ΔT，

推导出应需的纵向速度：

由式(11)结合式(7)，式(8)，式(9)，得到新的速度控制量ACC和转向控制量S。

本发明的有益效果：

通过距离传感器(如雷达)和视觉传感器(如摄像头)实时测量前方车辆信息(距离、速度)，侧后方来车信息、以及车道信息等，然后基于本专利的控制方法，规划出合适的行车轨迹，并根据行车环境，实时动态修正，最终实现安全、舒适地变道。

附图说明

图1是本发明的***控制框图；

图2是车辆变道过程中的运行轨迹模型示意图；

图3是车辆变道过程中的相关参数示意图；

图4是开始变道时正弦曲线沿X、Y轴的平移示意图；

图5为变道车辆的运动学模型示意图；

图6为本发明基于正弦函数的车辆自动变道控制方法的逻辑流程图；

图7为实施例1的实车变道测试图；

图8为实施例2的实车变道测试图；

图9为实施例3的实车变道测试图。

具体实施方式

本发明公开了一种基于正弦函数的车辆自动变道控制方法，包括如下步骤：

S1.基础模型搭建：

在实际行车工况中，当驾驶员左变道，方向盘控制是先左转、再回正，在此过程中，方向盘角度变化和车辆转向变化趋式是一致，且近似于“S”形。

S11.基本轨迹模型：本专利以正弦函数为本车运行轨迹模型，如图2所示，其从形态上符合上述“S”形，同时具备曲率连续，无阶跃现象，运算开销少，实时性好等优点。

y＝sin(x)x∈[-π/2,π/2](式1)；

式1的一阶导数，

该式算出正弦曲线上的各处斜率；

S12.依据现实环境变形后的轨迹模型：

在现实环境中，本车变道运行轨迹会受本车状态，前车状态，车道线等影响，因此将基础模型进行扩展变形，以涵盖关键参数，如下：

y＝Asin(ωx+θ)+B(式3)

其中，A为正弦函数的幅值，定义SLW为本车道宽度，LLW为左车道宽度，如图4所示，正弦波只在(半个左车道+半个本车道)范围内变化，因此，正弦函数的幅值

以车辆行进方向为纵向，在该公式(3)中A代表车辆在两车道之间的横移的距离。实质上，车的横向变化就是从本车道中心横向移动到左车道中心。

y为本车变道过程中的横向距离，开始变道时，y为0。

x为本车变道过程中的纵向距离，与行进方向一致，开始变道时，x为0。

车辆轨迹投影在X轴上的距离为Dmax，为完成变道所需最大的纵向距离，其可以由变道的车速V和变道完成需要的时间需T计算得到：Dmax＝V*T，

ω为转换系数，本实施例中，以车辆行驶即X轴方向的纵向距离为参数(X)，代入正弦函数，需用公式

转换为角度，即

B＝A，sin曲线沿Y正轴平移A，确保变道起始时，y＝0；

θ＝-π/2，sin曲线沿X正轴平移π/2，确保变道起始时，x＝0。正弦曲线沿X、Y 轴的平移示意图如图4所示。

公式(3)的一阶导数为：y’＝Aωcos(ωx+θ) (式4)

相关参数示意图请见图3；

S2.约束条件

S21.变道车速设定：

对于车辆自动变道的车速条件之一，用户或者主机厂设定，要求车速大于某一临界值时允许自动变道，该临界值定义为变道使能车速V_enable。例如，要求车速大于60KPH时，才允许使用自动变道，该车的变道使能车速V_enable即为60KPH。

其中，ΔV_thr、ΔD_thr分别是设计时进行标定的两个已知量，ΔV_thr为启动自动变道时与前车的速度差的最大值，ΔD_thr为启动自动变道时与前车的安全距离的最大值，例如，若本车和前车的速度差ΔV，始终允许在3KPH范围内波动，同时由本车及前车速度，可由安全策略机制得到理论上的安全距离D，如果实际两车距离Dreal与D之间的差ΔD一直小2米，则认为已经稳定跟车，上述的ΔV_th为3KPH，ΔD_thr为2米。当前车速v_x大于变道使能车速V_enable，且稳定追随前车，与前车的速度差ΔV<ΔV_thr，且与前车的安全距离差ΔD<Δ D_thr,持续一段时间t,t≥3s，***将以当前车速v_x作为纵向车速开始变道，实际车速v 在变道过程中，由正弦函数求解。

S22.变道产生的最大的纵向距离D_max：

变道完成时间T_end与预期的纵向车速v_x乘积为变道产生的最大纵向距离D_max，即

D_max＝v_x×T_end，事实上，正弦函数变量x∈[0,D_max]；

其中，变道完成时间T_end为5秒～8秒，可标定；

S23.变道转向轨迹的最大斜率k_max：

由式4可得，当ωx＝-θ，时，斜率最大，k_max＝Aω。k_max决定变道转向行为的剧烈度。不同速度区间设定上限值k_limit，当k_max>k_limit，禁止变道。

S24.本车车道线和左车车道线可见，宽度∈(3.75m+Δe)，曲率半径大于设定阈值L_{max_radius}(如2000m)，定义为阀值定为1500m；Δe是误差，这里可定义为正负0.2m。

S25.左车道，前方无车或者左前车与本车的车距满足安全车间时距，允许变道。

车间时距算法为D_w＝T_h*V_s-K*(V_f-V_s)+D₀，

其中，D_w为当前车间距离，即前方或左方或左前方车辆与本车的距离；

V_s为本车车速；

T_h为车间时距，即本车与前方或左方或左前方车辆相距的时间，即以本车当前速度到达前方或左方或左前方车辆现在位置所需要的时间，而ACC就会按照这个时间来保持相应距离。

V_f＝前车车速；

D₀为最小安全车距，是标定量，通常取3.5米；K为调节系数，也标定量，在项目中，通常取0.45。

S26.左车道，后方、左后方无车或者左侧后车与本车预碰撞时间大于预设阈值RCT_TTC，允许变道；其中，RCT_TTC是标定量，通常取2秒；

上述条件S21-S26均满足，则进入步骤S3，开始变道；

S3.假定整个工况是无损耗的理想状态，本车纵向速度v_x恒定，即可根据T的时刻状态，结合正弦函数，推导出T+1状态，以及所需要控制量。

T时刻，纵向车速为vx_t＝v_x，纵向距离X_t，可预期T+1时，纵向距离和横向距离：

X_t+1＝X_t+vx_t*ΔT (5)，

Y_t+1＝Asin(ω*X_t+1+θ)+B (6)

将式(6)代入式(4)，可得T+1时，正弦曲线的切线斜率，也即车辆形式轨迹的切线斜率：

进而可推导出切线与X轴的夹角，即T+1时刻横摆角γ_t+1：

已设定纵向速度恒定为v_x，实际车速v_r分解为沿X轴的纵向车速v_x和沿Y轴的横向车速v_y，在整个变道过程，策略要求v_x恒定不变，则v_r会随着横摆角变化而变化则在T+1 时刻，车速(vr_t+1)应该为：

进而可推导出速度控制量ACC，如下：

注vr_t为T时刻的车速。

根据图5中的车辆运动学模型，可推导出以下公式：

Δγ＝γ_t+1-γ_t，其中，γ_t为车辆在T时刻的横摆角，

即

δ为转向车轮偏角，L为车辆轴距。

根据表1的方向盘转角传动比映射表，不同的速度下，车辆的横摆角和方向盘的转角比是不一样的，结合式9，可得到T+1时刻，方向盘角度：S＝map(δ)，即得到转向控制量。

表1方向盘转交传动比映射表

车速KPH	10	20	40	60	80	100	140
								传动比	15.00977	9.299805	5.399902	4.299805	3.5	3.5	3

多域控制器是以20ms为周期发出速度控制量、转向控制量等相关控制信息，所以在***计算中，ΔT＝0.02，单位为秒。

本专利涉及的自动变道辅助(ALC)实现策略，如图1所示，是通过距离传感器(如雷达)和视觉传感器(如摄像头)实时测量前方车辆信息(距离、速度)，侧后方来车信息、以及车道信息等，本实施例中，包括短距离雷达SRR、中距离雷达MRR、智能摄像头 IPM，雷达和摄像头捕捉到的侧方目标信息传递至多域控制器MDC，然后通过多域控制器 MDC给方向盘发出转角指令，电动助力转向***EPS与车辆转向控制***VSC通信，同时多域控制器MDC向车身电子稳定***ESP发出加\减速度信号，将油门\刹车控制车速量传递至车辆的电子控制单元ECU，然后基于本专利的策略，规划出合适的行车轨迹，并根据行车环境，实时动态修正，最终实现安全、舒适地变道。

本案中，多域控制器(MDC)以20ms为周期，获取外部信息——车道信息(车道宽度，曲率，本车在车道中横向位置DY)，前车/左前车/左侧后车信息(速度，距离)，本车信息(车速)。

多域控制器(MDC)以20ms为周期，发出控制信息——加/减速度ACC，以控制车速；方向盘转角S，以控制车辆转向。

在实际工况中，受行车环境和自身状态影响，相关控制量无法保证车辆轨迹按预期运行，***需要实时监控和按如下方式进行补偿修正。

在本***中，变道为首要考量点，所以以本车与车道线的横向距离DY作为参考量。

T时刻，根据车道线信息，可得当前横向距离Yr_t，同时由反正弦函数可得纵向距离:

由式(5)，可在T-1时刻，预期T时刻的纵向距离Xp_t，结合式(10)，可得纵向距离误差ΔX＝(Xr_t-Xp_t)。

当X_hth>ΔX＝(Xr_t-Xp_t)>X_lth时，需要进行补偿操作。注：X_lth为差误阈值；X_hth为有效误差阈值——当ΔX>X_hth可认为差误不可信。

为补偿ΔX，需T至T+1时刻，预期的纵向位移应该为

ΔX_t+1＝ΔX+vx_t*ΔT，

进而可推导出应需的纵向速度：

由式(11)结合式(7)，式(8)，式(9)，得到新的速度控制量ACC和转向控制量S。

安全避撞：

在变道过程中，需实时监控前车，左前车(如果存在)情况，通过预碰撞时间

作为变道过程退出，由驾驶员接管的条件，碰撞时间的阈值可根据不同速度区间分别设定。

实施例1.见图7。

本车道前车车速为100KPH(27.78m/s),本车设定巡航车速为120KPH(33.33m/s)，车道宽为3.75m。

当本车稳定追随前车后，本车以大约100KHP纵向速度变道，用户设定需要6秒完成变道，则整个过程本车行驶的纵向距离为27.78*6＝166.68m。变道完成后，本车以设定巡航车速行驶。相关数据如图7所示，符合预期。

实施例2.见图8。

本车道前车车速为50KPH(13.89m/s),本车设定巡航车速为80KPH(22.22m/s)，车道宽为3.75m。

当本车稳定追随前车后，本车以大约50KHP纵向速度变道，用户设定需要6秒完成变道，则整个过程本车行驶的纵向距离为13.89*6＝83.34m。变道完成后，本车以设定巡航车速行驶。相关数据如图8所示，符合预期。

实施例3.见图9。

本车道前车车速为50KPH(13.89m/s),本车设定巡航车速为80KPH(22.22m/s)，车道宽为3.75m。当本车稳定追随前车后，本车以大约50KHP纵向速度变道，用户设定需要 8秒完成变道，则整个过程本车行驶的纵向距离为13.89*8＝111.12m。变道完成后，本车以设定巡航车速行驶。相关数据如图8所示，符合预期。

由各实施例实测可确认，本专利涉及的自动变道(ALC)控制方法符合设计预期，可满足使用要求。

应当理解的是，以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解，依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求范围当中。

Claims (2)

1.一种基于正弦函数的车辆自动变道控制方法，其特征在于，包括如下步骤：

S1.基础模型搭建：

在实际行车工况中，当驾驶员左变道，方向盘控制是先左转、再回正，在此过程中，方向盘角度变化和车辆转向变化趋式一致，且近似于“S”形；

S11.基本轨迹模型：以正弦函数为车辆运行轨迹模型，

y＝sin(x)x∈[-π/2,π/2] (式1)；

式1的一阶导数，

该式算出正弦曲线上的各处斜率；

S12.依据现实环境变形后的轨迹模型：

将基础模型进行扩展变形，以涵盖关键参数，如下：

y＝Asin(ωx+θ)+B(式3)

其中，A为正弦函数的幅值，定义SLW为本车道宽度，LLW为左车道宽度，正弦函数的幅值

以车辆行进方向为纵向；

y为本车变道过程中的横向距离，开始变道时，y为0；

x为本车变道过程中的纵向距离，与行进方向一致，开始变道时，x为0；

车辆轨迹投影在X轴上的距离为Dmax，为完成变道所需最大的纵向距离，Dmax＝V*T；

ω为转换系数，以车辆行驶即X轴方向的纵向距离为参数(X)，代入正弦函数，需用公式

转换为角度，即

B＝A，sin曲线沿Y正轴平移A，确保变道起始时，y＝0；

θ＝-π/2，sin曲线沿X正轴平移π/2，确保变道起始时，x＝0；

公式(3)的一阶导数为：y’＝Aωcos(ωx+θ)(式4)

S2.约束条件

S21.变道车速设定：

当前车速v_x大于变道使能车速V_enable，且稳定追随前车，与前车的速度差ΔV<ΔV_thr，且与前车的安全距离差ΔD<ΔD_thr,持续一段时间t,t≥3s，***将以当前车速v_x作为纵向车速开始变道，实际车速v在变道过程中，由正弦函数求解；其中，ΔV_thr为启动自动变道时与前车的速度差的最大值，ΔD_thr为启动自动变道时与前车的安全距离的最大值；

S22.变道产生的最大的纵向距离D_max：

变道完成时间T_end与预期的纵向车速v_x乘积为变道产生的最大纵向距离D_max，即D_max＝v_x×T_end，正弦函数变量x∈[0,D_max]；

其中，变道完成时间T_end为5秒～8秒，为标定量；

S23.变道转向轨迹的最大斜率k_max：

k_max＝Aω，不同速度区间设定上限值k_limit，当k_max>k_limit，禁止变道；

S24.本车车道线和左车车道线可见，宽度∈(3.75m+Δe)，曲率半径大于设定阈值L_{max_radius}；

S25.左车道，前方无车或者左前车与本车的车距满足安全车间时距，允许变道；

车间时距算法为D_w＝T_h*V_s-K*(V_f-V_s)+D₀，

其中，D_w为当前车间距离，即前方或左方或左前方车辆与本车的距离；

V_s为本车车速；

T_h为车间时距，即本车与前方或左方或左前方车辆相距的时间，即以本车当前速度到达前方或左方或左前方车辆现在位置所需要的时间；

V_f＝前车车速；

D₀为最小安全车距，是标定量；K为调节系数，为标定量；

S26.左车道，后方、左后方无车或者左侧后车与本车预碰撞时间大于预设阈值RCT_TTC，允许变道；RCT_TTC是标定量；

上述条件S21-S26均满足，则进入步骤S3，开始变道；

S3.假定整个工况是无损耗的理想状态，车辆纵向速度v_x恒定，即可根据T的时刻状态，结合正弦函数，推导出T+1状态，以及所需要控制量；

T时刻，纵向车速为vx_t＝v_x，纵向距离X_t，可预期T+1时，纵向距离和横向距离：

X_t+1＝X_t+vx_t*ΔT (5)，

Y_t+1＝Asin(ω*X_t+1+θ)+B (6)

将式(6)代入式(4)，得T+1时，正弦曲线的切线斜率，即车辆形式轨迹的切线斜率：

进而可推导出切线与X轴的夹角，即T+1时刻横摆角γ_t+1：

已设定纵向速度恒定为v_x，实际车速v_r分解为沿X轴的纵向车速v_x和沿Y轴的横向车速v_y，在整个变道过程，策略要求v_x恒定不变，则v_r会随着横摆角变化而变化则在T+1时刻，车速(vr_t+1)为：

进而可推导出速度控制量ACC，如下：

vr_t为T时刻的车速；

Δγ＝γ_t+1-γ_t，其中，γ_t为车辆在T时刻的横摆角，

即

δ为转向车轮偏角，L为车辆轴距；

T+1时刻，方向盘角度：S＝map(δ)，即得到转向控制量。

2.根据权利要求1所述的基于正弦函数的车辆自动变道控制方法，其特征在于，还包括如下补偿修正步骤：

T时刻，根据车道线信息，得当前横向距离Yr_t，同时由反正弦函数得纵向距离:

由式(5)，可在T-1时刻，预期T时刻的纵向距离Xp_t，结合式(10)，得纵向距离误差ΔX＝(Xr_t-Xp_t)；

当X_hth>ΔX＝(Xr_t-Xp_t)>X_lth时，需要进行补偿操作；

X_lth为差误阈值；X_hth为有效误差阈值，当ΔX>X_hth可认为差误不可信；

为补偿ΔX，需T至T+1时刻，预期的纵向位移应该为ΔX_t+1＝ΔX+vx_t*ΔT，推导出应需的纵向速度：

由式(11)结合式(7)，式(8)，式(9)，得到新的速度控制量ACC和转向控制量S。

Images