差速机器人微分平坦可行性代价梯度推导

大名鼎鼎的ego planner规划期在无人机领域大放异彩，实机表现也令人眼前一亮，好几年了一直都是主流的无人机规划器，于是想到将其改成二维可用与地面机器人，但是不能简单只改一个维度，地面机器人多为非完整欠驱动系统，不像无人机一样可以任意维度横移倾转，需要进一步限制轨迹的角速度、曲率等，于是下面以差速无人车系统为例引入差速机器人微分平坦可行性代价梯度推导。

基础定义与公式

微分平坦模型 (Differential Flatness Model)

先列出差速车辆的运动学方程：

$$\begin{cases} \dot x = v\cos\theta,\\\\ \dot y = v\sin\theta,\\\\ \dot\theta = \omega, \end{cases}$$

差速车辆运动学模型图

 

我们选择车辆后轴中心的位置 $\bm{p}(t) = [x(t), y(t)]^T$ 作为平坦输出。车辆的其他状态可以由 $\bm{p}(t)$ 及其各阶导数代数地表示出来：

$$\begin{aligned} \text{线速度 (Velocity):} & \quad v(t) = \sqrt{\dot{x}(t)^2 + \dot{y}(t)^2} \\\\ \text{航向角 (Heading):} & \quad \theta(t) = \operatorname{atan2}(\dot{y}(t), \dot{x}(t)) \\\\ \text{角速度 (Angular V.):} & \quad \omega(t) = \dot{\theta}(t) = \frac{\dot{x}\ddot{y} - \dot{y}\ddot{x}}{\dot{x}^2 + \dot{y}^2} \\\\ \text{曲率 (Curvature):} & \quad \kappa(t) = \frac{\omega(t)}{v(t)} = \frac{\dot{x}\ddot{y} - \dot{y}\ddot{x}}{(\dot{x}^2 + \dot{y}^2)^{3/2}} \end{aligned}$$

B样条离散近似 (B-Spline Discretization)

在优化问题中，我们使用控制点 $\bm{q}_k = [q_{kx}, q_{ky}]^T \in \mathbb{R}^2$ 来表示轨迹。速度和加速度则通过控制点的差分进行近似计算：

$$\begin{aligned} \text{速度控制点:} & \quad \bm{v}_i = \frac{\bm{q}_{i+1} - \bm{q}_i}{\Delta t} = \begin{bmatrix} v_{ix} \\ v_{iy} \end{bmatrix} \\\\ \text{加速度控制点:} & \quad \bm{a}_i = \frac{\bm{q}_{i+2} - 2\bm{q}_{i+1} + \bm{q}_i}{(\Delta t)^2} = \begin{bmatrix} a_{ix} \\ a_{iy} \end{bmatrix} \end{aligned}$$

将这些离散的近似值代入连续模型，我们得到在第 $i$ 段轨迹上的近似物理量：

$$\begin{aligned} \text{角速度近似:} & \quad \omega_i \approx \frac{v_{ix}a_{iy} - v_{iy}a_{ix}}{v_{ix}^2 + v_{iy}^2} = \frac{v_{ix}a_{iy} - v_{iy}a_{ix}}{\|\bm{v}_i\|^2} \\\\ \text{曲率近似:} & \quad \kappa_i \approx \frac{v_{ix}a_{iy} - v_{iy}a_{ix}}{(v_{ix}^2 + v_{iy}^2)^{3/2}} = \frac{v_{ix}a_{iy} - v_{iy}a_{ix}}{\|\bm{v}_i\|^3} \end{aligned}$$

---

可行性代价函数 (Feasibility Penalty)

总的可行性代价 $J_{feasibility}$ 由多个惩罚项加权构成。本文档详细推导其中与非完整性约束最相关的角速度和曲率部分。

$$J_{feasibility} = \lambda_{\omega} \underbrace{(\omega_i - \omega_{max})^2}_{J_\omega} + \lambda_{\kappa} \underbrace{(\kappa_i - \kappa_{max})^2}_{J_\kappa} + \dots$$

根据链式法则，总梯度为：

$$\frac{\partial J}{\partial \bm{q}_k} = \lambda_{\omega} \frac{\partial J_{\omega}}{\partial \bm{q}_k} + \lambda_{\kappa} \frac{\partial J_{\kappa}}{\partial \bm{q}_k} + \dots$$

每一项的梯度可进一步分解，以角速度项为例：

$$\frac{\partial J_{\omega}}{\partial \bm{q}_k} = \frac{\partial J_{\omega}}{\partial \omega_i}\left(\frac{\partial \omega_i}{\partial \bm{v}_i}\frac{\partial \bm{v}_i}{\partial \bm{q}_k} + \frac{\partial \omega_i}{\partial \bm{a}_i}\frac{\partial \bm{a}_i}{\partial \bm{q}_k}\right)$$

---

角速度代价 ($J_\omega$) 的梯度推导

Step 1: 代价对角速度的梯度

$$g_\omega = \frac{\partial J_\omega}{\partial \omega_i} = 2(\omega_i - \omega_{max})$$

Step 2: 角速度对 $\bm{v}_i$ 和 $\bm{a}_i$ 的梯度

为简化表达，定义旋转矩阵 $\mathbf{B} = \begin{pmatrix} 0 & -1 \\ 1 & 0 \end{pmatrix}$。

对 $\bm{a}_i$ 的梯度:

$$\frac{\partial \omega_i}{\partial \bm{a}_i} = \frac{1}{\|\bm{v}_i\|^2} \begin{bmatrix} -v_{iy} \\ v_{ix} \end{bmatrix} = \frac{\mathbf{B}\bm{v}_i}{\|\bm{v}_i\|^2}$$

对 $\bm{v}_i$ 的梯度:
令 $N = v_{ix}a_{iy} - v_{iy}a_{ix}$， $D = \|\bm{v}_i\|^2$。

$$\frac{\partial \omega_i}{\partial \bm{v}_i} = \frac{(\frac{\partial N}{\partial \bm{v}_i})D - N(\frac{\partial D}{\partial \bm{v}_i})}{D^2}$$

其中 $\frac{\partial N}{\partial \bm{v}_i} = \begin{bmatrix} a_{iy} \\ -a_{ix} \end{bmatrix} = -\mathbf{B}\bm{a}_i$，且 $\frac{\partial D}{\partial \bm{v}_i} = 2\bm{v}_i$。

$$\begin{aligned} \frac{\partial \omega_i}{\partial \bm{v}_i} &= \frac{(-\mathbf{B}\bm{a}_i)\|\bm{v}_i\|^2 - (v_{ix}a_{iy} - v_{iy}a_{ix})(2\bm{v}_i)}{(\|\bm{v}_i\|^2)^2} \\\\ &= \frac{-\mathbf{B}\bm{a}_i}{\|\bm{v}_i\|^2} - \frac{2 \omega_i \bm{v}_i}{\|\bm{v}_i\|^2} \end{aligned}$$

Step 3: 组合并传播至控制点

定义中间梯度向量：

• $\bm{g}_{\omega v} = g_\omega \cdot \frac{\partial \omega_i}{\partial \bm{v}_i} = g_\omega \left( \frac{-\mathbf{B}\bm{a}_i - 2\omega_i \bm{v}_i}{\|\bm{v}_i\|^2} \right)$

• $\bm{g}_{\omega a} = g_\omega \cdot \frac{\partial \omega_i}{\partial \bm{a}_i} = g_\omega \frac{\mathbf{B}\bm{v}_i}{\|\bm{v}_i\|^2}$

最终梯度为：

$$\begin{cases} \nabla_{\bm{q}_i} J_\omega = -\frac{1}{\Delta t} \bm{g}_{\omega v} + \frac{1}{(\Delta t)^2} \bm{g}_{\omega a} \\\\ \nabla_{\bm{q}_{i+1}} J_\omega = \frac{1}{\Delta t} \bm{g}_{\omega v} - \frac{2}{(\Delta t)^2} \bm{g}_{\omega a} \\\\ \nabla_{\bm{q}_{i+2}} J_\omega = \frac{1}{(\Delta t)^2} \bm{g}_{\omega a} \end{cases}$$

---

曲率代价 ($J_\kappa$) 的梯度推导

Step 1: 代价对曲率的梯度

$$g_\kappa = \frac{\partial J_\kappa}{\partial \kappa_i} = 2(\kappa_i - \kappa_{max})$$

Step 2: 曲率对 $\bm{v}_i$ 和 $\bm{a}_i$ 的梯度

对 $\bm{a}_i$ 的梯度:

$$\frac{\partial \kappa_i}{\partial \bm{a}_i} = \frac{1}{\|\bm{v}_i\|^3} \begin{bmatrix} -v_{iy} \\ v_{ix} \end{bmatrix} = \frac{\mathbf{B}\bm{v}_i}{\|\bm{v}_i\|^3}$$

对 $\bm{v}_i$ 的梯度:
令 $N = v_{ix}a_{iy} - v_{iy}a_{ix}$， $D = \|\bm{v}_i\|^3$。

$$\frac{\partial \kappa_i}{\partial \bm{v}_i} = \frac{(\frac{\partial N}{\partial \bm{v}_i})D - N(\frac{\partial D}{\partial \bm{v}_i})}{D^2}$$

其中 $\frac{\partial N}{\partial \bm{v}_i} = -\mathbf{B}\bm{a}_i$，且 $\frac{\partial D}{\partial \bm{v}_i} = 3\|\bm{v}_i\|\bm{v}_i$。

$$\begin{aligned} \frac{\partial \kappa_i}{\partial \bm{v}_i} &= \frac{(-\mathbf{B}\bm{a}_i)\|\bm{v}_i\|^3 - (v_{ix}a_{iy} - v_{iy}a_{ix})(3\|\bm{v}_i\|\bm{v}_i)}{(\|\bm{v}_i\|^3)^2} \\\\ &= \frac{-\mathbf{B}\bm{a}_i}{\|\bm{v}_i\|^3} - \frac{3 (v_{ix}a_{iy} - v_{iy}a_{ix}) \bm{v}_i}{\|\bm{v}_i\|^5} \\\\ &= \frac{-\mathbf{B}\bm{a}_i}{\|\bm{v}_i\|^3} - \frac{3 \kappa_i \bm{v}_i}{\|\bm{v}_i\|^2} \end{aligned}$$

Step 3: 组合并传播至控制点

定义中间梯度向量：

• $\bm{g}_{\kappa v} = g_\kappa \cdot \frac{\partial \kappa_i}{\partial \bm{v}_i} = g_\kappa \left( \frac{-\mathbf{B}\bm{a}_i}{\|\bm{v}_i\|^3} - \frac{3\kappa_i \bm{v}_i}{\|\bm{v}_i\|^2} \right)$

• $\bm{g}_{\kappa a} = g_\kappa \cdot \frac{\partial \kappa_i}{\partial \bm{a}_i} = g_\kappa \frac{\mathbf{B}\bm{v}_i}{\|\bm{v}_i\|^3}$

最终梯度为：

$$\begin{cases} \nabla_{\bm{q}_i} J_\kappa = -\frac{1}{\Delta t} \bm{g}_{\kappa v} + \frac{1}{(\Delta t)^2} \bm{g}_{\kappa a} \\\\ \nabla_{\bm{q}_{i+1}} J_\kappa = \frac{1}{\Delta t} \bm{g}_{\kappa v} - \frac{2}{(\Delta t)^2} \bm{g}_{\kappa a} \\\\ \nabla_{\bm{q}_{i+2}} J_\kappa = \frac{1}{(\Delta t)^2} \bm{g}_{\kappa a} \end{cases}$$

---

代码实现

在calcFeasibilityCost函数下可增加如下代码实现上述算法

Eigen::Matrix2d B ;
B << 0 , -1,
    1 , 0;
/* angularVelocity feasibility */
for(int i = 0 ; i < q.cols() - 2 ; i++){
    // .head<2>() 从三维向量中取出前两个元素 (x, y) 形成一个二维向量
    Eigen::Vector2d vi = (q.col(i + 1).head<2>() - q.col(i).head<2>()) / ts;
    Eigen::Vector2d ai = (q.col(i + 2).head<2>() - 2 * q.col(i + 1).head<2>() + q.col(i).head<2>()) * ts_inv2;
    double vi_x = vi(0) , vi_y = vi(1);
    double ai_x = ai(0) , ai_y = ai(1);
    double s2 = vi.squaredNorm();
    if (s2 < 1e-6) continue; // 防除零

    double wi = (vi_x*ai_y - vi_y*ai_x) / vi.squaredNorm() ;
    if(wi > max_w_){
        cost += pow(wi - max_w_, 2);
        double gw = 2*(wi - max_w_);
        Eigen::Vector2d gwv = gw * ((-B*ai - 2*wi*vi) / s2);
        Eigen::Vector2d gwa = gw * (B * vi / s2);
        gradient.col(i + 0).head<2>() +=  -gwv/ts + gwa*ts_inv2;
        gradient.col(i + 1).head<2>() +=  gwv/ts - 2.0 * gwa*ts_inv2;
        gradient.col(i + 2).head<2>()+= gwa*ts_inv2;
    }
    else if(wi < -max_w_){
        cost += pow(wi + max_w_, 2);
        double gw = 2*(wi + max_w_);
        Eigen::Vector2d gwv = gw * ((-B*ai - 2*wi*vi) / s2);
        Eigen::Vector2d gwa = gw * (B * vi / s2);
        gradient.col(i + 0).head<2>()  +=  -gwv/ts + gwa*ts_inv2;
        gradient.col(i + 1).head<2>()  +=  gwv/ts - 2.0 * gwa*ts_inv2;
        gradient.col(i + 2).head<2>() += gwa*ts_inv2;
    }
}

/* curvature feasibility */
for(int i = 0 ; i < q.cols() - 2 ; i++){
    Eigen::Vector2d vi = (q.col(i + 1).head<2>() - q.col(i).head<2>()) / ts;
    Eigen::Vector2d ai = (q.col(i + 2).head<2>() - 2 * q.col(i + 1).head<2>() + q.col(i).head<2>()) * ts_inv2;
    double vi_x = vi(0) , vi_y = vi(1);
    double ai_x = ai(0) , ai_y = ai(1);
    double s2 = vi.squaredNorm();
    if (s2 < 1e-6) continue; // 防除零
    double ki = (vi_x*ai_y - vi_y*ai_x) / (s2 * vi.norm()) ;
    if(ki > max_k_){
        cost += pow(ki - max_k_ , 2);
        double gk = 2*(ki - max_k_);
        Eigen::Vector2d gkv = gk * ((-B*ai / (s2 * vi.norm())) - 3*ki*vi / s2);
        Eigen::Vector2d gka = gk * ( B * vi / ( s2 * vi.norm()) );
        gradient.col(i + 0).head<2>() +=  -gkv/ts + gka*ts_inv2;
        gradient.col(i + 1).head<2>() +=  gkv/ts - 2.0 * gka*ts_inv2;
        gradient.col(i + 2).head<2>() += gka*ts_inv2;
    }
    else if(ki < -max_k_){
        cost += pow(ki + max_k_ , 2);
        double gk = 2*(ki + max_k_);
        Eigen::Vector2d gkv = gk * ((-B*ai / (s2 * vi.norm())) - 3*ki*vi / s2);
        Eigen::Vector2d gka = gk * ( B * vi / ( s2 * vi.norm()) );
        gradient.col(i + 0).head<2>() +=  -gkv/ts + gka*ts_inv2;
        gradient.col(i + 1).head<2>() +=  gkv/ts - 2.0 * gka*ts_inv2;
        gradient.col(i + 2).head<2>() += gka*ts_inv2;
    }
}

效果对比

下面视频时加入运动学约束前后的对比视频，给定一条初始轨迹后，左边的轨迹没有优化大曲率尖角部分，可以看到这里车辆出现了无法跟踪轨迹的情况，而右边加上约束后可以正常跟踪轨迹。

后续工作

后续计划使用MInco时空联合规划样条轨迹来取代B样条，然后学习一下Fast-lab最新论文是怎么解决上面微分平坦分式部分奇异点问题的