apollo control module

Posted on |

纵向控制

通过控制刹车、油门实现对车速的控制。由一个级联控制器和标定表构成。级联控制器包括：位置PID闭环控制器， 速度PID闭环控制器。标定表即速度-加速度-刹车-油门命令 标定表。

1
2
3
4
5
6
Status LonController::ComputeControlCommand(
	const localization::LocalizationEstimate *localization,  
	const canbus::Chassis *chassis, 
	const planning::ADCTrajectory *planning_published_trajectory,
	control::ControlCommand *cmd)

其输出是油门\刹车命令cmd.

avatar

位置PID闭环控制器

输入变量：期望位置 + 当前实际位置

输出变量：速度补偿量

速度PID闭环控制器

输入变量：速度补偿 + 当前位置-速度偏差

输出变量：加速度补偿量

速度-加速度-刹车/油门命令 标定表

输入变量： 加速度补偿量 + 规划驾速度， 车速

输出变量： 油门／刹车控制量

1
2
3
4
void LonController::ComputeLongitudinalErrors(
	const TrajectoryAnalyzer *trajectory_analyzer, 
	const double preview_time ,
	SimpleLongitudinalDebug *debug)

CarSim/Simulink 与 apollo 连结 ？

横向控制

通过调节方向盘转角实现对航向的控制，由一个前馈控制器和反馈控制器组合，实现车辆动力学模型和lqr 模型。

1
2
3
4
5
Status LatController::ComputeControlCommand(
	const localization::LocalizationEstimate *localization,
	const canbus::Chassis *chassis,
	const planning::ADCTrajectory *planning_published_trajectory,
	control::ControlCommand *cmd)

avatar

前馈控制器

输入变量：道路曲率

输出变量：方向盘前馈控制量

前馈控制变量实现补偿道路曲率对稳态误差的影响。

反馈控制器

输入变量： 期望航向角

输出变量： 方向盘反馈控制量

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
// update status matrix 
void LatController::UpdateStateAnalyticalMatching(
	SimpleLateralDebug *debug)
// cal lat error 
double LatController::ComputeLateralErrors(
	const double x, 
	const double y,
	const double theta,
	const double linear_v ,
	const double angular_v, 
	const TrajectoryAnalyzer &trajectory_analyzer,
	SimpleLateralDebug *debug)
void LatController::UpdateMatrix() 
void common::math::SolveLQRProblem()  // gain matrix 
steer_angle =  steer_angle_feedback + steer_angle_feedforward

控制模块更新cmd 后，发送给canbus模块。

MPC 模型

mpc的实现依赖于过程的动态模型。对时域内，每个当前时刻进行优化，求取每个时刻的最优控制解，从而得到整个时域的优化解。

  • 基于线性化的预测

实际系统状态-控制方程（系统状态变量、控制变量）具有时域非线性（二次项等）。首先对该系统进行线性近似，从而可以实现通过当前状态变量、控制变量对下一个时刻系统状态和控制的预测。

  • 滚动优化

设计符合约束条件的目标函数，为状态、控制的离散的能量二次方程。优化的目标就是在每个时刻，寻找最优控制变量，使得目标函数最小。MPC相当于给出每个时刻的局部最优控制值，整个时域的最优控制就是一个控制序列。

  • 控制更新

每个优化的输出即每个时步的控制增量，用于更新当下时步的系统控制变量。

LQR 模型

lqr 给出时域系统的全局最优解， 其目标函数是积分函数。相比较，lqr 相当于加权伽辽金方法，给出弹性体的全局最优解，对应强形式的控制方程，解空间非常狭小，甚至不存在。而mpc 相当于有限元方法，给出弹性体每个单元的局部解，对应弱形式的控制方程，每个单元的解空间span可张。理论上弱形式在极限情况给出的解就是强形式的解。

数值方法上，lqr相当于直接法，mpc相当于迭代法。直接法给出控制方程的唯一真实解；迭代法给出近似解，但近似解可收敛到系统的真实解。

车辆模型

运动学模型给出车辆纵向控制变量，即车辆模型刚体质心参考点及航向角的控制方程；动力学模型给出车辆横向控制变量。

由运动学和动力学模型给出了整车状态-控制方程，采用mpc or lqr 算法求解控制变量，即实现车辆控制。