佑驾创新控制算法面经

动力学控制需要考虑车辆的质量分布、轮胎与地面的摩擦力、空气阻力等因素。动力学模型通常用牛顿第二定律表示，即F=ma。在车辆动力学中，常见的模型包括纵向动力学（研究加速和减速）、侧向动力学（研究转弯）和垂直动力学（研究悬架系统的振动）。例如，在侧向动力学中，车辆的侧向加速度取决于轮胎的侧向力和车辆的重心位置。动力学控制常用于高级驾驶辅助系统（ADAS），如电子稳定程序（ESP）和牵引力控制系统（TCS）。

（3）数学模型对比：

运动学模型通常是线性的，可以用简单的微分方程表示。例如，一个二维平面内的车辆运动学模型可以写为dx/dt=vcos(theta)，dy/dt=vsin(theta)，dtheta/dt=omega，其中v是线速度，omega是角速度，theta是车辆的方向角。
而动力学模型则可能是非线性的，需要考虑更多的物理参数。例如，车辆的侧向动力学方程可以写为mvdtheta/dt=Fy，其中m是车辆质量，Fy是轮胎提供的侧向力。

4）扩展知识：

（1）应用场景：

运动学控制适用于无人机、移动机器人等需要精确路径规划的场景。动力学控制则广泛应用于汽车工业，尤其是在自动驾驶和智能交通领域。

（2）仿真工具：

常用的车辆动力学仿真工具有CarSim、TruckSim和MATLAB/Simulink。这些工具可以帮助工程师验证控制算法的有效性和安全性。

（3）伪代码示例：

cpp 复制代码

function kinematic_model(v, omega, theta)
    dx = v * cos(theta)
    dy = v * sin(theta)
    dtheta = omega
    return dx, dy, dtheta

function dynamic_model(m, Fx, Fy, v, theta)
    a_longitudinal = Fx / m
    a_lateral = Fy / m
    dv = a_longitudinal
    dtheta = a_lateral / v
    a_mag = sqrt(a_longitudinal^2 + a_lateral^2)
    return a_mag, dtheta

动力学模型解析：

平面运动里，把加速度分解成：

切向：改变速度大小 v
法向：改变速度方向（也就是航向角 θ）

法向（横向）加速度和转向角速度（航向角变化率）满足：

所以：

也就是dtheta公式的由来：

✅ 这个是标准的点质体/自行车模型里常用关系。

二、LQR在有曲率和无曲率情况下如何消除稳态误差

1）曲率概念

无曲率：κ=0，轨迹是直线，不需要持续转向。

有曲率 ：κ≠0，轨迹是弯道，需要持续转向/持续yaw rate（横摆角速度）

航向角：ψ（车头朝向）

yaw rate：r=dψ/dt

r=0：车头方向不变（直线走）

r≠0：车头在转（在拐弯）

2）为什么 LQR 会有稳态误差？

LQR 本质是：

它只最小化状态与控制的二次代价，不天然保证"对常值扰动/参考输入"零稳态误差。

直线（κ=0）：理想平衡点常常就是 xe=0,u=0x_e=0, u=0xe=0,u=0，容易回零。
弯道（κ≠0）：零误差平衡点通常需要 u≠0u\neq 0u=0（持续转向），你不给前馈，弯道就等价成"常值偏置扰动"，误差只能留着去"顶住"它 → 出现稳态偏差。

3）无曲率怎么消稳态误差？

首选：LQI（加积分） ，直线（κ=0）时如果还出现稳态误差，通常来自模型不准/外扰/执行器偏置

做法：把误差积分当成新状态：

增广系统后做 LQR（就叫 LQI），得到：

这样可以把稳态误差"积掉"。

4）有曲率怎么消稳态误差？

核心不是"更强的LQR"，而是"把弯道的稳态输入补上"：

（1）曲率前馈（让"零误差"成为可达平衡点）

常见自行车模型前馈：

（2）"前馈"怎么来的？（核心公式）

弯道的曲率 κ 与转弯半径 R：

走圆弧时，航向角变化率（yaw rate）：

另一方面，自行车模型给：

两式相等（要跟上这条曲线）：

这就是你看到的 常见自行车模型前馈。

直线（κ=0）：δff = 0
弯道（κ≠0）：δff ≠ 0（需要持续打一个角度）

小角度近似（δ 不大时）：

整体控制结构：

（3）再用 LQI 抵抗外扰（可选但很工程）

如果还有风/坡度/轮胎参数不准等常值扰动导致残差：

这就是弯道跟踪里最常见的组合：FF + LQR/LQI。

你原文提到"非线性 + 线性化 + 自适应/鲁棒"也没错，但面试里更建议先讲 前馈+误差LQR（或LQI），再补一句"强非线性/大范围工况用增益调度/TV-LQR/MPC"。

5）总结

LQR 是线性系统最优状态反馈，本身不保证对常值参考/扰动零稳态误差。

"无曲率（κ=0）时，参考输入通常为零，若仍存在稳态误差，多来自模型偏差或常值扰动，可通过 积分增广（LQI）"消除。
有曲率（κ≠0）时，弯道跟踪需要非零稳态输入（如 δff=arctan⁡(Lκ)，因此要采用曲率前馈 + 误差LQR ；若仍有残差，再加入 LQI/扰动观测 提升鲁棒性。

三、给定轨迹位置，期望速度，期望加速度，输出油门踏板如何控制？

解题思路

1）正确答案：

油门踏板的控制可以通过将期望速度和期望加速度映射到油门开度来实现。具体来说，需要根据当前车辆的速度、期望速度以及期望加速度，通过一个**控制器（如PID控制器）**计算出合适的油门开度，并将其转换为油门踏板的实际物理位置或信号值。

2）解答思路：

首先明确问题中的输入参数：轨迹位置（表示车辆在路径上的当前位置）、期望速度（目标速度）和期望加速度（目标加速度）。
根据当前速度与期望速度的偏差，结合期望加速度，设计一个控制器来计算油门开度。
使用PID控制器作为核心算法，其能够基于误差（期望速度与实际速度之差）调整输出，确保系统快速响应且稳定。
将控制器输出的油门开度映射到油门踏板的具体控制信号上。

3）深度知识讲解：

PID控制器原理 ：PID控制器是一种常用的反馈控制系统，由比例（P）、积分（I）和微分（D）三个部分组成。其输出可以表示为：
复制代码
```
u(t) = Kp * e(t) + Ki * integral(e(t)) + Kd * derivative(e(t))
```
其中，e(t)是期望值与实际值之间的误差，Kp、Ki和Kd分别是比例、积分和微分项的增益系数。
- 比例项（P）：根据当前误差直接调整输出，影响系统的响应速度。
- 积分项（I）：消除稳态误差，确保系统最终达到期望值。
- 微分项（D）：预测误差变化趋势，抑制系统震荡。
期望速度与加速度的关系：期望加速度决定了速度的变化率。如果期望加速度为正，则需要增加油门开度；反之则减少油门开度。
油门踏板控制：油门踏板的物理位置或电信号通常与发动机节气门开度成正比关系。因此，控制器输出的油门开度可以直接转化为踏板控制信号。

4）伪代码：

复制代码

function calculate_throttle(current_speed, target_speed, target_acceleration, dt)
    # 计算速度误差
    speed_error = target_speed - current_speed

    # PID控制器参数
    Kp = 0.5  # 比例增益
    Ki = 0.1  # 积分增益
    Kd = 0.3  # 微分增益

    # 更新积分项
    integral += speed_error * dt

    # 计算微分项
    derivative = (speed_error - previous_error) / dt

    # PID输出
    throttle = Kp * speed_error + Ki * integral + Kd * derivative + target_acceleration

    # 限制油门范围
    if throttle > 1.0:
        throttle = 1.0
    elif throttle < 0.0:
        throttle = 0.0

    # 更新变量
    previous_error = speed_error

    return throttle

5）扩展知识：

在实际应用中，除了油门控制外，还需要考虑制动系统的协同工作，特别是在减速或停车时。
轨迹位置可以用于预测未来的速度需求，从而提前调整油门开度，优化能耗和驾驶体验。
更复杂的系统可能采用模型预测控制（MPC）等高级算法，综合考虑多种约束条件（如燃油经济性、排放要求等）。

四、LQR离线还是在线，有哪些区别？

解题思路

1）正确答案：

LQR（Linear Quadratic Regulator，线性二次型调节器）可以分为离线和在线两种实现方式。
离线LQR指的是在系统运行前预先计算出最优控制律并存储下来，而在系统运行时直接调用该控制律进行控制。
在线LQR则是在系统运行过程中实时计算最优控制输入。

2）解答思路：

首先需要理解LQR的基本概念，它是一种基于状态空间模型的最优控制方法，目标是最小化一个由状态和控制输入构成的二次型性能指标。
对于离线LQR，由于系统的动态特性和性能指标通常是已知且固定的，因此可以在运行前通过求解代数Riccati方程得到最优反馈增益矩阵，并据此确定控制律。
而对于在线LQR，当系统的动态特性或性能指标随时间变化时，就需要在每个采样时刻重新计算最优控制输入。

五、超前和滞后矫正如何设计？

解题思路

1）正确答案：

超前和滞后矫正是控制理论中用于改善系统动态性能的技术。
超前矫正通过增加系统的相位裕度来提高稳定性，
而滞后矫正则通过降低增益来减少高频噪声的影响。
设计时需要根据系统的传递函数选择合适的补偿器参数。

2）解答思路：

首先分析系统的开环传递函数，确定其频率响应特性。
对于超前矫正，设计一个具有适当零点和极点的超前网络，以在特定频率范围内增加相位。
对于滞后矫正，设计一个滞后网络，以降低高频增益而不影响低频性能。
然后使用波特图或奈奎斯特图验证矫正效果。

3）深度知识讲解：

超前矫正主要通过增加系统的相位裕度来提高稳定性，通常适用于不稳定或接近不稳定的系统。它通过引入一个零点和一个极点，使得在关键频率处相位提前。
滞后矫正则是为了减少高频噪声的影响，同时保持低频性能不变。滞后网络通过引入一个极点和一个零点，使得高频增益下降。
这两种方法都需要仔细调整零点和极点的位置以达到最佳效果。

六、控制上层路径规划有那些数据发到下层路径跟踪上？

解题思路

1）正确答案：

在机器人或自动驾驶系统的路径规划与跟踪架构中
上层路径规划模块 通常会将目标路径、关键点坐标、速度曲线以及可能的加速度约束等数据发送到下层路径跟踪模块。
这些数据帮助下层模块精确地控制车辆或机器人沿规划路径行驶。

2）解答思路：

首先明确问题背景：路径规划分为全局路径规划（高层）和局部路径跟踪（底层）。高层负责生成整体可行路径，而底层负责实时调整以确保系统能够沿着规划路径移动。
确定高层需要传递给底层的关键信息。这些信息包括但不限于路径点集合、期望的速度曲线、时间参数化、曲率限制等。
分析这些信息如何影响底层算法的设计和实现。

3）深度知识讲解：

路径点集合：这是最基础的数据，表示从起点到终点的一系列离散点。这些点可以是二维平面中的(x, y)坐标，也可以是三维空间中的(x, y, z)坐标。对于某些复杂场景，还可以包含方向角（如theta），即每个路径点对应一个特定的方向。
速度曲线：高层路径规划不仅要提供位置信息，还需要给出每个路径点上的期望速度v(t)。这有助于底层控制器计算合适的加速度和制动力，从而平稳地完成任务。
时间参数化：如果路径被赋予了时间戳，则可以进一步指导底层控制器按照预定的时间表执行动作。例如，在t=5秒时应该到达某个特定点。
曲率和加速度约束：为了保证运动的安全性和舒适性，高层路径规划可能会附加一些物理约束条件，比如最大允许曲率（避免急转弯）、最大加速度或减速度等。这些约束对底层控制器至关重要，因为它需要根据实际情况调整控制策略。
误差容忍度：有时高层路径规划还会定义容许的偏差范围，告知底层控制器在多大范围内偏离规划路径是可以接受的。
扩展知识：在实际应用中，不同类型的系统对上下层交互的要求有所不同。例如，在自动驾驶汽车中，高层路径规划可能基于高精地图进行，考虑交通规则、障碍物等因素；而在无人机导航中，高层路径规划更注重避开空域限制和动态障碍物。此外，现代系统常采用反馈机制，让底层控制器将当前状态反馈给高层，以便动态调整规划结果。

4）伪代码示例：

下面是一个简单的高层路径规划向底层路径跟踪传输数据的伪代码示例：

复制代码

function sendPathToTracker(pathPlannerOutput)
    pathPoints = pathPlannerOutput['path_points']  // 路径点集合 [(x1,y1), (x2,y2), ...]
    speedProfile = pathPlannerOutput['speed_profile']  // 速度曲线 [v1, v2, ...]
    curvatureLimits = pathPlannerOutput['curvature_limits']  // 曲率限制 [k1, k2, ...]
    accelerationConstraints = pathPlannerOutput['acceleration_constraints']  // 加速度约束 [a_min, a_max]

    for point in pathPoints:
        tracker.addPoint(point)

    for speed in speedProfile:
        tracker.setSpeed(speed)

    tracker.setCurvatureLimits(curvatureLimits)
    tracker.setAccelerationConstraints(accelerationConstraints)

    tracker.startTracking()

七、纯跟踪算法前视怎么找的，用了那些方法？

解题思路

1）正确答案：

纯跟踪算法中的前视（lookahead）通常通过预测模型或轨迹规划方法来实现。具体来说，可以使用基于几何的路径规划、卡尔曼滤波器、粒子滤波器等技术来估计目标的未来位置。这些方法结合传感器数据和运动学模型，预测目标在下一时刻的位置和状态。

2）解答思路：

纯跟踪算法的核心是实时调整控制输入以使系统跟踪目标。为了实现这一点，需要预测目标的未来位置（即前视）。以下是常见的实现步骤：

获取当前目标的状态信息（如位置、速度、加速度等），这通常来自传感器数据。

使用运动学或动力学模型预测目标在未来某一时间点的位置。

根据预测的位置调整控制器参数，例如方向盘角度或电机转速。这些步骤可能涉及多种数学工具和技术，包括微分方程、概率论以及优化算法。

3）深度知识讲解：

以下是一些常用的前视方法及其背后的原理：

基于几何的路径规划 ：假设目标沿一条特定的轨迹移动，例如直线或圆弧。通过简单的几何关系计算出未来某时刻的目标位置。这种方法适用于简单的场景，但对复杂环境的适应性较差。
- 示例：如果目标以恒定速度v沿直线移动，则t秒后的位置为(x + vt, y)。
卡尔曼滤波器 ：这是一种递归的状态估计方法，能够融合多源传感器数据并预测目标的未来状态。它假设系统的动态行为可以用线性方程表示，并且噪声服从高斯分布。
- 公式：状态更新公式为x_k = F * x_(k-1) + B * u_k + w_k，其中F是状态转移矩阵，B是控制输入矩阵，w是过程噪声。
粒子滤波器 ：当系统动态行为非线性或噪声非高斯时，卡尔曼滤波器可能失效。此时可以使用粒子滤波器，通过一系列随机样本（粒子）近似概率分布。
- 原理：每个粒子代表一种可能的状态，通过重采样和权重更新逐步逼近真实状态。
轨迹优化 ：对于复杂的动态系统，可以通过优化算法直接求解最优轨迹。这种方法通常将问题建模为一个约束优化问题，目标是最小化某种代价函数（如能量消耗或跟踪误差）。
- 实现：定义代价函数J(x,u)，其中x是状态向量，u是控制输入向量。然后使用梯度下降或其他优化算法求解。

八、MPC和LQR区别？

解题思路

1）正确答案：

MPC（Model Predictive Control，模型预测控制）和LQR（Linear Quadratic Regulator，线性二次调节器）都是常用的控制方法，但它们在目标、计算方式和适用场景上存在显著区别。
MPC通过优化一个有限时间范围内的成本函数来生成控制输入，并考虑了系统的约束条件。
而LQR是一种基于状态空间的最优控制方法，适用于线性系统，通过求解代数黎卡提方程得到最优反馈增益矩阵K，从而实现闭环控制。

2）解答思路：

首先明确两者的定义和核心思想。MPC是一种基于模型的控制方法，它在每个采样时刻重新计算未来的控制序列，以最小化一个预定义的成本函数，同时满足系统约束。
LQR则是一种静态反馈控制器设计方法，其目标是找到一个固定的反馈增益矩阵K，使得系统的状态能够快速收敛到期望值，同时最小化一个二次型性能指标。

九、最小二乘法

解题思路

1）正确答案：

最小二乘法是一种数学优化技术，通过最小化误差的平方和来寻找数据的最佳函数匹配 。它通常用于线性回归模型中，以找到一条直线或曲线，使得该直线或曲线与给定的数据点之间的误差平方和最小。

2）解答思路：

假设我们有一组数据点 (x1, y1), (x2, y2), ..., (xn, yn)，我们希望找到一条直线 y = ax + b，使得这条直线能够尽可能好地拟合这些数据点。为了实现这一点，我们定义误差为每个数据点的实际值与预测值之间的差值，并且我们试图最小化这些误差的平方和。具体步骤如下：

定义误差函数 E(a, b) = Σ(yi - (axi + b))^2。

对误差函数分别对a和b求偏导数，并令其等于零，得到两个方程。

解这两个方程组成的线性方程组，求出a和b的最优值。