【10天速通Navigation2】(九）：LQR最优控制器的原理推导与Nav2插件实现

前言

前两期我们实现了三个全局规划器（Hybrid-A*、RRT、RRT*），解决了"从起点到终点该走哪条路"的问题。但拿到路径之后，还有一个同样关键的问题------怎么沿着这条路走好。
打个比方：规划器是导航软件，告诉你"前方 200 米右转"；而控制器是你的脚和手------什么时候踩油门、踩多深、方向盘打多少度，才能既精准又顺滑地跟上导航的指示。
往期内容：
本教材将贯穿nav2的全部内容，使用ROS2和C++实现一些仿真乃至实车中常见的建图和路径规划算法。我们将注重与原理讲解和代码实现，去详细讲解每一步的配置过程和代码复现细节。
本教程使用的环境：
- ROS2 humble
- ubuntu 22.04 LTS
本期我们将写一个 LQR 最优控制器------从误差动力学建模到代数黎卡提方程的手算解析解，再到 C++ 代码实现与 Nav2 插件注册，最终替换掉默认的 DWB 控制器，让小车紧贴全局路径进行高精度跟踪。最终效果如下：

0 一些调整

在写控制器之前，先同步几个配置上的变更------这是在调试规划器和控制器过程中踩的坑，单独拿出来方便需要的人。

0-1 切换全局规划器为 Hybrid-A*

LQR 是一个高精度路径跟踪控制器，它对路径的质量要求比较高------如果路径弯弯绕绕、曲率不连续，控制器会在跟踪过程中来回震荡。因此在三个规划器（Hybrid-A*、RRT、RRT*）中，我们选择 Hybrid-A* 作为配合 LQR 的全局规划器：它的路径由自行车模型扩展的圆弧段组成，曲率连续、运动学可行，LQR 跟踪起来最顺滑。
在 navigation_sim.yaml 中确认规划器为 Hybrid-A*：

yaml 复制代码

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "planner/HybridAStarPlanner"
      wheel_base: 0.24
      arc_length: 0.5
      steering_angle_max: 0.6
      goal_xy_tolerance: 0.3
      goal_theta_tolerance: 0.3

0-2 降频全局重规划

planner_server 默认 expected_planner_frequency: 20.0（每秒重规划 20 次）。对于采样规划器（RRT/RRT*），每轮可能跑几百毫秒，20Hz 重规划会导致上一个规划还没完成就被新的打断。改为 2Hz：

yaml 复制代码

planner_server:
  ros__parameters:
    expected_planner_frequency: 2.0   # 从 20 → 2，降低重规划频率

0-3 AMCL 定位参数调整

在实际调试中，AMCL 存在位姿收敛慢、静止时抖动等问题。以下为针对性调整：

yaml 复制代码

amcl:
  ros__parameters:
    # ---- 里程计噪声：降低对 odom 的信任度，更依赖激光扫描 ----
    alpha1: 0.05
    alpha2: 0.05
    alpha3: 0.05
    alpha4: 0.05
    alpha5: 0.05
    # ---- 激光模型 ----
    laser_model_type: "likelihood_field"     # 似然场模型，比 beam 模型更平滑
    laser_likelihood_max_dist: 1.5           # 似然计算的最大距离 (m)
    laser_max_range: 100.0                   # 激光最大量程
    max_beams: 60                            # 每轮处理的激光束数
    # ---- 粒子滤波 ----
    min_particles: 500
    max_particles: 2000
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.001               # 定位丢失时快速恢复
    recovery_alpha_slow: 0.001
    resample_interval: 1
    # ---- 更新频率控制：减少静止时的位姿抖动 ----
    update_min_d: 0.5                        # 走 0.5m 才更新一次
    update_min_a: 0.3                        # 转 0.3rad 才更新一次
    # ---- TF ----
    transform_tolerance: 2.0                 # TF 查询容忍时间
    base_frame_id: "base_footprint"          # 和机器人底盘 TF 对齐
    odom_frame_id: "odom"
    global_frame_id: "map"
    scan_topic: scan

0-4 调大全局膨胀半径

小车在狭窄走廊里规划出的路径仍然贴墙太近，稍微跑偏就会撞。把 global_costmap 的 inflation_radius 从 0.3 调到 0.5：

yaml 复制代码

global_costmap:
  global_costmap:
    ros__parameters:
      inflation_layer:
        inflation_radius: 0.5       # 膨胀半径从 0.3 → 0.5m
        cost_scaling_factor: 3.0

1 Nav2 控制器接口说明

在 Nav2 中，控制器也是一个插件（plugin），继承 nav2_core::Controller 基类。注意它和规划器的接口完全不同：

cpp 复制代码

class Controller
{
public:
  virtual void configure(...) = 0;
  virtual void cleanup() = 0;
  virtual void activate() = 0;
  virtual void deactivate() = 0;

  // 核心：给定当前位姿 + 速度，输出控制指令
  virtual geometry_msgs::msg::TwistStamped computeVelocityCommands(
    const geometry_msgs::msg::PoseStamped & pose,
    const geometry_msgs::msg::Twist & velocity,
    nav2_core::GoalChecker * goal_checker) = 0;

  // 设置路径：规划器每次计算完路径后调用
  virtual void setPlan(const nav_msgs::msg::Path & path) = 0;

  virtual void setSpeedLimit(const double & speed_limit, const bool & percentage) = 0;
};

方法	作用	对比规划器
`configure()`	加载参数	同规划器
`setPlan()`	接收全局路径------规划器每次算出新路径就推过来	规划器是 `createPlan()` 输出路径
`computeVelocityCommands()`	核心 ------给定当前位置，输出 `Twist`（线速度 + 角速度）	规划器是给定 start + goal，输出 Path
`setSpeedLimit()`	外部限速回调	规划器无

说人话就是：

规划器给你一条路（Path），控制器负责开车（cmd_vel）。setPlan() 收到路，computeVelocityCommands() 以高频（默认 20Hz）被调用，每次回答"现在应该怎么踩油门和打方向盘"。

和规划器一样，需要三个文件注册插件。以下是 controller 包的关键文件：
plugins.xml ------注意基类是 nav2_core::Controller，不是 GlobalPlanner：

xml 复制代码

<class_libraries>
    <library path="controller">
        <class type="controller::LQRController"
               base_class_type="nav2_core::Controller">
            <description>LQR path following controller</description>
        </class>
    </library>
</class_libraries>

package.xml ------同样是 <nav2_core plugin="${prefix}/plugins.xml" /> 声明导出：

xml 复制代码

<package format="3">
  <name>controller</name>
  <version>0.1.0</version>
  <description>Custom Nav2 controllers: LQR, and more.</description>

  <depend>rclcpp</depend>
  <depend>nav2_core</depend>
  <depend>nav2_costmap_2d</depend>
  <depend>pluginlib</depend>
  <!-- ... 其他依赖 ... -->

  <export>
    <build_type>ament_cmake</build_type>
    <nav2_core plugin="${prefix}/plugins.xml" />   <!-- 关键行 -->
  </export>
</package>

CMakeLists.txt ------编译为动态库（SHARED），pluginlib 通过 dlopen 加载 .so：

cmake 复制代码

add_library(controller SHARED src/lqr_controller.cpp)
ament_target_dependencies(controller rclcpp nav2_core nav2_costmap_2d pluginlib ...)
pluginlib_export_plugin_description_file(nav2_core plugins.xml)

项目树结构如下：

ros2_ws/src/
├── controller/ ← 自定义控制器包
│ ├── CMakeLists.txt
│ ├── package.xml
│ ├── plugins.xml
│ ├── include/controller/
│ │ └── lqr_controller.hpp
│ └── src/
│ └── lqr_controller.cpp
└── planner/ ← 自定义规划器包（前两期）
三条文件协作用关系：package.xml 告诉构建系统"导出了 nav2_core 插件"→ plugins.xml 告诉 pluginlib "在 libcontroller.so 里找 controller::LQRController"→ PLUGINLIB_EXPORT_CLASS 宏把这个类注册到 C++ 类型系统。和三期规划器插件的注册逻辑完全一样，只是基类从 GlobalPlanner 换成了 Controller。
编译后在 navigation_sim.yaml 中修改 controller_server 的插件名即可替换。

2 数学冷知识

最优控制器涉及三个核心数学概念：二次型 、线性反馈律 和代数黎卡提方程。本节逐一解释，方便后续阅读。

2-1 二次型（Quadratic Form）

二次型就是 "平方的加权求和" 。比如 x T Q x x^T Q x xTQx：

x T Q x = $x 1 x 2$ $q 1 0 0 q 2$ $x 1 x 2$ = q 1 x 1 2 + q 2 x 2 2 x^T Q x = \begin{bmatrix} x_1 & x_2 \end{bmatrix} \begin{bmatrix} q_1 & 0 \\ 0 & q_2 \end{bmatrix} \begin{bmatrix} x_1 \\ x_2 \end{bmatrix} = q_1 x_1^2 + q_2 x_2^2 xTQx= $x1x2$ $q100q2$ $x1x2$ =q1x12+q2x22

为什么要用它做代价函数？因为平方函数天然有两个好性质：
1. 正负号一视同仁 ： ( − 2 ) 2 = ( + 2 ) 2 (-2)^2 = (+2)^2 (−2)2=(+2)2，偏左偏右同样惩罚，不会出现"正负抵消"
2. 离目标越远惩罚越重：误差从 1 变到 2，代价从 1 变到 4------超线性增长，控制器不敢让误差积累
说人话就是： Q Q Q 对角线上每个数告诉你"这个误差方向有多重要"------ q = 1 q=1 q=1 表示普通在意， q = 100 q=100 q=100 表示这个方向绝对不能偏。

2-2 线性反馈律（Linear Feedback Law）

线性反馈律就是 "看到多少偏差，按比例出多少力"：

u = − K x = − $k 1 k 2$ $x 1 x 2$ = − ( k 1 x 1 + k 2 x 2 ) u = -Kx = -\begin{bmatrix} k_1 & k_2 \end{bmatrix} \begin{bmatrix} x_1 \\ x_2 \end{bmatrix} = -(k_1 x_1 + k_2 x_2) u=−Kx=− $k1k2$ $x1x2$ =−(k1x1+k2x2)

负号表示"反向纠正"： x 1 > 0 x_1 > 0 x1>0（偏右了），就输出负控制量把你往左拉。 k 1 , k 2 k_1, k_2 k1,k2 是增益------决定"纠正力度"。
说人话就是：

开车偏右了 → 方向盘往左打。偏得越远、打的方向越多------这就是线性反馈。增益 K 就是"手劲儿多大"------手劲太小纠正不过来，手劲太大来回摆。

那么什么是最优的 K？看一个具体的例子：同一个系统，三种不同的 K：

K	效果	问题
K = $0.1 , 0.1$ K = $0.1, 0.1$ K= $0.1,0.1$	手劲太小	偏差纠正慢，机器人慢悠悠才回到路径上
K = $100 , 100$ K = $100, 100$ K= $100,100$	手劲太大	纠正过猛，机器人来回震荡、乘客晕车
K = $k 1 o p t , k 2 o p t$ K = $k_1\^{opt}, k_2\^{opt}$ K= $k1opt,k2opt$	最优	纠正又快又稳，同时控制量最小

所以"最优"不是"最快纠正"------而是在"纠偏效果"和"控制代价"之间找到最佳平衡 。这个平衡的标准就是 2-1 节的二次型代价函数 J J J------ J J J 越小，说明偏离小 + 用力少。最优反馈增益 K K K 就是那个让 J J J 达到最小的增益值。
最优控制的核心问题就归结为：怎么算出这个让 J J J 最小的 K？ 答案是求解下面这个方程。

2-3 代数黎卡提方程（ARE）

在最优控制中，求最优反馈增益 K K K 最终归结为解下面这个矩阵方程------CARE（Continuous-time Algebraic Riccati Equation）：

A T P + P A − P B R − 1 B T P + Q = 0 A^T P + P A - P B R^{-1} B^T P + Q = 0 ATP+PA−PBR−1BTP+Q=0

这是一个矩阵未知数方程 ------未知数 P P P 是一个对称矩阵（和 A A A 同维度），其他矩阵 A , B , Q , R A, B, Q, R A,B,Q,R 都是已知的。后面章节会具体解释它们各自是什么，这里先关注方程本身的特点：
拆开看结构：
- 一次项 （ A T P + P A A^T P + P A ATP+PA 和 + Q +Q +Q）：和常规线性方程一样
- 二次项 （ − P B R − 1 B T P -P B R^{-1} B^T P −PBR−1BTP）： P P P 以平方的形式出现------这是它被称为 Riccati 方程 的原因（18 世纪意大利数学家 Jacopo Riccati 首次系统研究了含未知数平方的非线性方程）
怎么解？对于 2 × 2 2\times2 2×2 的小矩阵，可以用待定系数法手算解析解 （本文 4-4 节展示完整计算过程）；对于大矩阵，用 Python 一行搞定：scipy.linalg.solve_continuous_are(A, B, Q, R)。
解出 P P P 之后，最优增益为 K = R − 1 B T P K = R^{-1} B^T P K=R−1BTP------这就是我们要的"看到多大偏差、出多大力纠正"的配方。

3 最优控制与 LQR

有了上面的数学工具，最优控制的逻辑就清晰了：
我们在【现代控制理论与状态估计】从状态空间到Python仿真实现的 1-5 节提到过最优控制的基本框架。最优控制的本质 ：在满足系统动态约束（系统状态不能随便变，必须遵守物理规律------比如 x ˙ = A x + B u \dot{x} = Ax + Bu x˙=Ax+Bu）的前提下，找一个控制输入 u u u，最小化某个代价函数 J J J。

J = ∫ 0 ∞ ( x T Q x + u T R u ) d t J = \int_0^\infty (x^T Q x + u^T R u) \, dt J=∫0∞(xTQx+uTRu)dt

其中 Q Q Q 惩罚"偏离目标状态的程度"， R R R 惩罚"用力太猛"。

LQR（Linear Quadratic Regulator） 是最优控制中最简单也最实用的特例------线性系统 + 二次型代价函数 。它的核心魅力在于：最优解是一个线性反馈律 u = − K x u = -Kx u=−Kx（见 2-2）， K K K 可以直接通过求解代数黎卡提方程（见 2-3）得到，不需要在线优化、不需要迭代。
说人话就是：

LQR 就像自动驾驶里一个训练有素的老司机------它看一眼当前位置离路径有多远、方向偏了多少，然后按一个固定的"配方"（增益矩阵 K）瞬间算出该打多少方向盘。不需要生成轨迹、不需要采样------纯粹是"看到偏差 → 按比例纠正"。

LQR 相对 DWB（上一期用的 Nav2 默认局部规划器）的差异：

	DWB	LQR
原理	采样多条轨迹，用代价函数评分选最优	解析计算最优反馈增益
计算量	高（每条轨迹都要碰撞检测）	极低（只做矩阵运算）
输出	发布局部轨迹到 `/local_plan`	直接发 `/cmd_vel`，不产生局部轨迹
调参	多个 critic 权重	三个数： Q c t e , Q h e a d i n g , R Q_{cte}, Q_{heading}, R Qcte,Qheading,R

4 LQR 路径跟踪

4-1 问题建模

我们要控制的是一辆 4WD 差速小车（linorobot2）。它的运动简化成独轮模型（unicycle）：

{ x ˙ = v cos ⁡ θ y ˙ = v sin ⁡ θ θ ˙ = ω \begin{cases} \dot{x} = v \cos\theta \\ \dot{y} = v \sin\theta \\ \dot{\theta} = \omega \end{cases} ⎩ ⎨ ⎧x˙=vcosθy˙=vsinθθ˙=ω

其中控制输入：
- v v v：线速度（前进/后退），由速度曲率衰减策略单独控制
- ω \omega ω：角速度（转向），由 LQR 反馈律计算
路径跟踪关心的不是一个绝对位姿误差，而是两个相对于路径的误差：
1. 横向误差（Cross-Track Error, CTE）：机器人到参考路径的垂直距离------"偏左了还是偏右了"
2. 航向误差（Heading Error）：机器人朝向与路径切线方向的角度差------"车头指偏了多少"

4-2 误差动力学

记误差状态 e = $e c t e e θ$ e = \begin{bmatrix} e_{cte} \\ e_{\theta} \end{bmatrix} e= $ecteeθ$ 。在参考路径附近线性化独轮模型，得到误差动力学：

{ e ˙ c t e = v ⋅ e θ e ˙ θ = ω \begin{cases} \dot{e}{cte} = v \cdot e{\theta} \\ \dot{e}_{\theta} = \omega \end{cases} {e˙cte=v⋅eθe˙θ=ω

这里的关键假设是：v v v 在小角度下可视为常数 （由外部速度策略设定），于是系统退化成：角速度 ω \omega ω 直接控制朝向误差 e θ e_\theta eθ，朝向误差又积分为横向误差 e c t e e_{cte} ecte 的变化率。
写成标准的状态空间形式 e ˙ = A e + B u \dot{e} = A e + B u e˙=Ae+Bu：

e ˙ = $0 v 0 0$ ⏟ A e + $0 1$ ⏟ B ω \dot{e} = \underbrace{\begin{bmatrix} 0 & v \\ 0 & 0 \end{bmatrix}}{A} \; e + \underbrace{\begin{bmatrix} 0 \\ 1 \end{bmatrix}}{B} \; \omega e˙=A $00v0$ e+B $01$ ω

这就是系统的状态方程 ------ A A A 描述系统自身如何演化（ e ˙ c t e = v ⋅ e θ \dot{e}{cte} = v \cdot e\theta e˙cte=v⋅eθ，即横向误差的变化率 = 航向误差 × 速度）， B B B 描述控制输入如何施加到系统上（ ω \omega ω 只直接影响 e ˙ θ \dot{e}\theta e˙θ，不直接影响 e ˙ c t e \dot{e}{cte} e˙cte）。它是一个线性时不变系统------LQR 可以直接上场。

4-3 LQR 代价函数

定义二次型代价：

J = ∫ 0 ∞ ( q c t e ⋅ e c t e 2 + q h e a d i n g ⋅ e θ 2 + r ⋅ ω 2 ) d t J = \int_0^\infty (q_{cte} \cdot e_{cte}^2 + q_{heading} \cdot e_\theta^2 + r \cdot \omega^2) \, dt J=∫0∞(qcte⋅ecte2+qheading⋅eθ2+r⋅ω2)dt

即：

Q = $q c t e 0 0 q h e a d i n g$ , R = $r$ Q = \begin{bmatrix} q_{cte} & 0 \\ 0 & q_{heading} \end{bmatrix}, \quad R = $r$ Q= $qcte00qheading$ ,R= $r$

三个参数的含义很直观：
- q c t e q_{cte} qcte：横向误差的代价。调大 → 控制器更激进地修正偏移，路径跟踪更紧
- q h e a d i n g q_{heading} qheading：航向误差的代价。调大 → 车头更"贴"路径方向，转弯更果断
- r r r：控制量的代价。调大 → 方向盘更"懒"，转弯更平缓但可能反应迟钝
说人话就是：

Q 的两个值决定"你有多在意偏了和歪了"，R 决定"你有多在意猛打方向盘"。Q 大 R 小 = 紧贴路径但转向激进；Q 小 R 大 = 转向温柔但路径松。

4-4 解析求解 LQR 增益

对于 2 状态 1 控制输入的系统，连续时间 LQR 的代数黎卡提方程可以手算解析解，不需要调用数值求解器：
设对称矩阵 P = $p 1 p 12 p 12 p 2$ P = \begin{bmatrix} p_1 & p_{12} \\ p_{12} & p_2 \end{bmatrix} P= $p1p12p12p2$ ，求解 CARE（Continuous-time Algebraic Riccati Equation）：

A T P + P A − P B R − 1 B T P + Q = 0 A^T P + P A - P B R^{-1} B^T P + Q = 0 ATP+PA−PBR−1BTP+Q=0

代入 4-2 节推导出的 A , B A, B A,B 矩阵，展开得到三个方程：

{ p 12 = q c t e ⋅ r p 2 = r ⋅ ( 2 v ⋅ p 12 + q h e a d i n g ) p 1 = p 12 ⋅ p 2 r \begin{cases} p_{12} = \sqrt{q_{cte} \cdot r} \\ $4pt$ p_2 = \sqrt{r \cdot (2v \cdot p_{12} + q_{heading})} \\ $4pt$ p_1 = \dfrac{p_{12} \cdot p_2}{r} \end{cases} ⎩ ⎨ ⎧p12=qcte⋅r p2=r⋅(2v⋅p12+qheading) p1=rp12⋅p2

最优反馈增益 K = R − 1 B T P K = R^{-1} B^T P K=R−1BTP 为：

K = $k 1 k 2$ = $p 12 r p 2 r$ K = \begin{bmatrix} k_1 & k_2 \end{bmatrix} = \begin{bmatrix} \frac{p_{12}}{r} & \frac{p_2}{r} \end{bmatrix} K= $k1k2$ = $rp12rp2$

最优控制律： ω = − k 1 ⋅ e c t e − k 2 ⋅ e θ \omega = -k_1 \cdot e_{cte} - k_2 \cdot e_\theta ω=−k1⋅ecte−k2⋅eθ
对照代码，solveLQR() 完整翻译了这套解析公式：

cpp 复制代码

void solveLQR(double v, double & k1, double & k2)
{
  double vc = std::max(v, 0.1);                          // 速度不能为 0，否则 A=0 矩阵奇异
  double p12 = std::sqrt(q_cte_ * r_ctrl_);              // p12 = √(q_cte·r)
  double p22 = std::sqrt(r_ctrl_ * (2.0 * vc * p12 + q_heading_));
                                                         // p2 = √(r·(2v·p12 + q_heading))
  k1 = p12 / r_ctrl_;                                    // k1 = p12 / r
  k2 = p22 / r_ctrl_;                                    // k2 = p2 / r
}

std::max(v, 0.1) 保证速度不低于 0.1 m/s------当机器人静止时， A A A 矩阵退化为零矩阵，CARE 的解无意义。这个 clamp 只是为了防止数值问题，不影响正常行驶。
最后反馈到角速度：

cpp 复制代码

double omega = -k1 * cte - k2 * heading_err;   // LQR 反馈律：u = -Kx

4-5 路径参考点与前视距离

LQR 需要知道"当前应该追踪路径上的哪个点"。最简单的做法是找路径上离自己最近的点（findClosestPoint）。但这会有一个问题------如果机器人已经在路径上，CTE=0，LQR 就不产生纠正了，机器人会沿着切线漂走。
解决方法是前视距离 （lookahead）：不在最近点做误差计算，而是沿路径向前多走 lookahead_dist_ 米（默认 0.8m），在那个更远的参考点处计算 CTE 和航向误差。

cpp 复制代码

void computeErrors(pose, closest_idx, cte, heading_err, curvature)
{
  // 从最近点出发，沿路径累积弧长，找到 lookahead_dist 米之后的参考点
  size_t ref_idx = closest_idx;
  double accum = 0.0;
  for (size_t i = closest_idx; i + 1 < n; ++i) {
    accum += hypot(path[i+1] - path[i]);
    ref_idx = i + 1;
    if (accum >= lookahead_dist_) break;
  }

  double ref_x = path[ref_idx].x, ref_y = path[ref_idx].y;
  double ref_yaw = getYaw(path[ref_idx].orientation);

  // 横向误差（带符号）：机器人坐标在参考点坐标系下的 y 分量
  double dx = cur_x - ref_x, dy = cur_y - ref_y;
  cte = dy * cos(ref_yaw) - dx * sin(ref_yaw);

  // 航向误差（归一化到 [-π, π]）
  heading_err = atan2(sin(cur_yaw - ref_yaw), cos(cur_yaw - ref_yaw));
}

横向误差公式 c t e = d y ⋅ cos ⁡ θ r e f − d x ⋅ sin ⁡ θ r e f cte = dy \cdot \cos\theta_{ref} - dx \cdot \sin\theta_{ref} cte=dy⋅cosθref−dx⋅sinθref 是 2D 旋转矩阵的逆变换------把世界坐标系下的位置差投影到参考路径的局部坐标系中。 c t e > 0 cte > 0 cte>0 表示机器人在路径左侧， c t e < 0 cte < 0 cte<0 在右侧。
前视距离的选择是一个权衡：太小 → 控制器反应过度、容易震荡；太大 → 控制器对局部偏差不敏感、转弯松。0.6~1.0m 是常见范围。

4-6 线速度策略

LQR 本身只输出角速度 ω \omega ω。线速度 v v v 由两个因素决定：

cpp 复制代码

// ① 曲率减速：弯道自动降速
double v_curv = max_linear_vel_ / (1.0 + 2.0 * curvature);
// ② 终点减速：快到终点时逐步收油
double v_goal = max_linear_vel_ * std::min(1.0, dist_to_goal / 0.5);
// ③ 组合：取最保守的一个
double v_cmd = std::min({v_curv, v_goal, max_linear_vel_});

cmd.twist.angular.z = clamp(omega, -max_angular_vel_, max_angular_vel_);

曲率 κ \kappa κ 用 Menger 曲率公式估算。思路是：取参考点及其前后邻居，三点确定一个外接圆------弯越急、圆越小、曲率越大。
设三边长为 a , b , c a, b, c a,b,c，面积的半周长 s = ( a + b + c ) / 2 s = (a+b+c)/2 s=(a+b+c)/2，用 Heron 公式求三角形面积 A = s ( s − a ) ( s − b ) ( s − c ) A = \sqrt{s(s-a)(s-b)(s-c)} A=s(s−a)(s−b)(s−c) 。外接圆半径和曲率为：

R = a b c 4 A , κ = 1 R = 4 A a b c R = \frac{abc}{4A}, \quad \kappa = \frac{1}{R} = \frac{4A}{abc} R=4Aabc,κ=R1=abc4A

代码在 computeErrors() 的末尾，取参考点 ref_idx 的前后邻居做计算：

cpp 复制代码

// 三点 Menger 曲率
double a = hypot(x1 - x2, y1 - y2);    // 边 AB
double b = hypot(x2 - x3, y2 - y3);    // 边 BC
double c = hypot(x1 - x3, y1 - y3);    // 边 AC
double s = (a + b + c) / 2.0;
double area = sqrt(s * (s-a) * (s-b) * (s-c));
curvature = 4.0 * area / (a * b * c);  // κ = 4A/(abc)

举个例子：直线上三个等距点，面积 A = 0 A=0 A=0 → κ = 0 \kappa=0 κ=0 → 直道全速；弯道上三点形成明显三角形， A A A 和 a b c abc abc 相当 → κ ≈ 0.5 \kappa \approx 0.5 κ≈0.5 → 弯道降速。曲率大 → 弯道 → 减速通过；曲率小 → 直道 → 全速前进。

4-7 完整控制流程

cpp 复制代码

TwistStamped computeVelocityCommands(pose, velocity, goal_checker)
{
  // ---- 1. 距离目标太近 → 停车 ----
  if (dist_to_goal < goal_tolerance_) return zero_cmd;

  // ---- 2. 找参考点 + 计算误差 ----
  size_t closest = findClosestPoint(pose);
  double cte, heading_err, curvature;
  computeErrors(pose, closest, cte, heading_err, curvature);

  // ---- 3. LQR 求解角速度 ----
  double k1, k2;
  solveLQR(max_linear_vel_, k1, k2);
  double omega = -k1 * cte - k2 * heading_err;

  // ---- 4. 线速度策略 ----
  double v_cmd = min(curvature_slowdown, goal_slowdown, max_vel);

  // ---- 5. 输出 cmd_vel ----
  cmd.twist.linear.x = v_cmd;
  cmd.twist.angular.z = clamp(omega, -max_angular_vel_, max_angular_vel_);
  return cmd;
}

LQR 路径跟踪的一个关键特征是：不发布局部轨迹到 /local_plan ，直接把最终 cmd_vel 发给底盘。对比 DWB 在每次控制周期内采样上百条轨迹、碰撞检测、评分后选最优------LQR 只做一次解析计算，耗时 < 0.1ms。

5 编译与使用

编译 controller 包：

bash 复制代码

cd ~/ros2_ws
colcon build --packages-select controller

在 navigation_sim.yaml 中替换控制器：

yaml 复制代码

controller_server:
  ros__parameters:
    controller_plugins: ["FollowPath"]
    FollowPath:
      plugin: "controller/LQRController"          # 替换掉 dwb_core::DWBLocalPlanner
      q_cte: 5.0            # 横向误差权重（越大路径跟踪越紧）
      q_heading: 3.0        # 航向误差权重（越大车头越贴路径方向）
      r_ctrl: 1.0           # 控制量权重（越大转向越温柔）
      lookahead_dist: 0.8   # 前视距离 (m)
      max_linear_vel: 0.4   # 最大线速度 (m/s)
      max_angular_vel: 1.0  # 最大角速度 (rad/s)
      goal_tolerance: 0.3   # 目标容差 (m)

确认生效：

bash 复制代码

ros2 param get /controller_server FollowPath.plugin
# controller/LQRController

调参口诀：
- 机器人贴不住路径 （转弯时切弯）→ 加大 q_cte
- 机器人来回摇摆 （震荡）→ 减小 q_cte 或加大 r_ctrl
- 机器人转弯迟钝 （转角不够）→ 加大 q_heading
- 机器人转弯过猛 （甩尾）→ 加大 r_ctrl 或减小 max_angular_vel

总结

本期我们从零手写了一个 LQR 路径跟踪控制器，以 Nav2 插件形式接入。

控制器的角色 ：与规划器不同------规划器输出路径，控制器输出 cmd_vel。setPlan() 收路径，computeVelocityCommands() 高频计算控制指令。
LQR 路径跟踪：以横向误差和航向误差为状态，自行车模型为系统，解析求解黎卡提方程得到最优反馈增益。前视距离让控制器"看得远"而不是追着最近点跑。
极简调参 ：三个数------ q c t e q_{cte} qcte 控制贴线紧度、 q h e a d i n g q_{heading} qheading 控制方向敏感度、 r r r 控制转向温柔度。配合线速度曲率/终点衰减策略，弯道自动降速。
与 DWB 的差异 ：LQR 是解析计算、不采样不碰撞检测、不发布 /local_plan，计算量极低。适合已知路径的高精度跟踪，复杂动态环境下 DWB 更鲁棒。

本期实现了 LQR 路径跟踪控制器，下一期我们将探索 MPC（模型预测控制）------在 LQR 的基础上加入预测时域和约束处理，让控制器既最优又安全~
感谢支持！！！！
如有错误，欢迎指出！！！！！！