轮式底盘的微分平坦

文章目录

一、微分平坦定义
二、轮式底盘的微分平坦
- [1. 轮式底盘的微分平坦推导准备](#1. 轮式底盘的微分平坦推导准备)
- [2. 轮式底盘的微分平坦推导](#2. 轮式底盘的微分平坦推导)
三、轨迹生成
- [1. 轨迹生成步骤如下](#1. 轨迹生成步骤如下)

第一步，先摆上霍普金斯的关于微分平坦的讲义。

一、微分平坦定义

控制非线性系统到达目标状态通常分为两步：第一步是轨迹生成 ，第二步是利用反馈进行轨迹跟踪 。根据可用算力，轨迹生成可低频重规划，而跟踪控制则以接近传感器频率运行 。两步法的关键优势在于：轨迹生成时可直接考虑系统动力学、输入约束等限制，并优化性能指标。因此即使系统因不确定性偏离路径，整体性能仍接近最优。

1.1 基本定义

有一类系统的轨迹生成特别简单，被称为微分平坦系统 。

若一个系统能找到一组输出（维度与输入相等） ，使得无需对系统积分 就能由这组输出完全确定整个状态与控制输入 ，则该系统是微分平坦的。

更形式化地：

状态 x ∈ R n x∈ℝ^n x∈Rn、输入 u ∈ R m u∈ℝ^m u∈Rm 的系统，若存在输出 y ∈ R m y∈ℝ^m y∈Rm
y = h ( x , u , u ˙ , ... , u ( a ) ) y=h(x,u,\dot u,...,u^{(a)}) y=h(x,u,u˙,...,u(a))

使得

x = φ ( y , y ˙ , ... , y ( b ) ) u = α ( y , y ˙ , ... , y ( c ) ) \begin{aligned} x=φ(y,\dot y,...,y^{(b)}) \\ u=α(y,\dot y,...,y^{(c)}) \end{aligned} x=φ(y,y˙,...,y(b))u=α(y,y˙,...,y(c))

则称系统微分平坦。坐标 y y y 称为平坦输出。

二、轮式底盘的微分平坦

微分平坦的关键判定(满足就是平坦)；

平坦输出维度 = 控制输入维度(都是 2)；
全部状态 可由 y 及其导数 唯一确定；
全部控制量 可由 y 及其导数 唯一计算；
不需要积分动力学方程；

轮式移动机器人是微分平坦系统，平坦输出为： y = [ x 1 , x 2 ] y=[x_1,x_2] y=[x1,x2] 完美满足，所以是最经典、最常用的微分平坦系统。

1. 轮式底盘的微分平坦推导准备

1.1 机器人状态（3 个）
x = [ x 1 x 2 x 3 ] = [ x y θ ] \bold x= \left[ \begin{matrix} x_1\\ x_2 \\ x_3 \end{matrix} \right] = \left[ \begin{matrix} x\\ y \\ \theta \end{matrix} \right] x= x1x2x3 = xyθ

1.2 控制输入（2 个）
u = [ v ω ] \bold u= \left[ \begin{matrix} v\\ \omega \end{matrix} \right] u=[vω]

1.3 运动学模型

{ x ˙ 1 = v ⋅ c o s θ x ˙ 2 = v ⋅ s i n θ x ˙ 3 = θ ˙ = ω \left\{ \begin{array}{l} \dot x_1 &= v \cdot cos\theta \\ \dot x_2 &= v \cdot sin\theta\\ \dot x_3 &= \dot \theta = \omega \ \end{array} \right . ⎩ ⎨ ⎧x˙1x˙2x˙3=v⋅cosθ=v⋅sinθ=θ˙=ω

2. 轮式底盘的微分平坦推导

如何用平坦输出 y 算出所有状态？
2.1 航向角 θ 的计算

θ = a r c t a n 2 ( x ˙ 2 , x ˙ 1 ) + k π , k = 0 , 1 θ=arctan2(\dot x_2, \dot x_1)+kπ, \quad k=0,1 θ=arctan2(x˙2,x˙1)+kπ,k=0,1

其中：

k=0：机器人前进
k=1：机器人后退

由此，可知：只要知道位置轨迹 x ( t ) , y ( t ) x (t), y (t) x(t),y(t)，就能直接算出朝向！

进一步，想算出机器人全部状态 ( x , y , θ ) (x,y,θ) (x,y,θ)，只需要平坦输出 y 到 1 阶导数(位置 + 速度)，即，状态依赖平坦输出的1 阶导数：b=1

2.2 线速度 v v v
v = ± x ˙ 1 2 + x ˙ 2 2 v=±\sqrt{\dot x_1^2+\dot x_2^2} v=±x˙12+x˙22

其中：

正：前进
负：后退

2.3 角速度 ω
ω = θ ˙ = x ¨ 1 x ˙ 2 − x ˙ 1 x ¨ 2 x ˙ 1 2 + x ˙ 2 2 ω=\dotθ=\frac{\ddot x_1 \dot {x}_2−\dot x_1 \ddot {x}_2}{\dot x_1^2+\dot x_2^2} ω=θ˙=x˙12+x˙22x¨1x˙2−x˙1x¨2

可知，想算出控制输入 ( v , ω ) (v,ω) (v,ω)，需要平坦输出 y y y 到 2 阶导数(位置 + 速度 + 加速度)，即，控制依赖平坦输出的2 阶导数：c=2

总结：轮式机器人平坦系统的阶数：

状态依赖平坦输出的1 阶导数：b=1
控制依赖平坦输出的2 阶导数：c=2

故，给定任意光滑的位置轨迹 y ( t ) = [ x ( t ) , y ( t ) ] T y (t) = [x (t), y (t)]^T y(t)=[x(t),y(t)]T，可以直接算出：

航向角 θ ( t ) θ(t) θ(t)
线速度 v ( t ) v (t) v(t)
角速度 ω ( t ) ω(t) ω(t)
整个状态轨迹 [ x , y , θ ] T [x,y,θ]^T [x,y,θ]T

完全不需要解微分方程、不需要积分、不需要优化 ！这就是微分平坦的威力。

三、轨迹生成

现在使用微分平坦输出做轨迹。

1. 轨迹生成步骤如下

步骤 1：选择平坦输出的参数化形式

以选三次多项式基 为例， y ( t ) y (t) y(t)平坦输出轨迹， x ( t ) 、 y ( t ) x (t)、y (t) x(t)、y(t) 各自都是 3 次多项式：
x ( t ) = a 1 t 3 + a 2 t 2 + a 3 t + a 4 y ( t ) = b 1 t 3 + b 2 t 2 + b 3 t + b 4 \begin{aligned} x(t)&=a_1t^3+a_2t^2+a_3t+a_4 \\ y(t)&=b_1t^3+b_2t^2+b_3t+b_4 \end{aligned} x(t)y(t)=a1t3+a2t2+a3t+a4=b1t3+b2t2+b3t+b4

合起来写成矩阵：
y ( t ) = A ⋅ λ ( t ) = [ a 1 a 2 a 3 a 4 b 1 b 2 b 3 b 4 ] [ t 3 t 2 t 1 ] y(t)=A⋅λ(t)=\left[ \begin{matrix} a_1 & a_2 & a_3&a_4\\ b_1 & b_2 & b_3&b_4 \end{matrix} \right] \left[ \begin{matrix} t^3\\ t^2\\ t\\ 1 \end{matrix} \right] y(t)=A⋅λ(t)=[a1b1a2b2a3b3a4b4] t3t2t1

其中，

y ( t ) = [ x ( t ) , y ( t ) ] T y(t)=[x(t),\ y(t)]^T y(t)=[x(t), y(t)]T
λ ( t ) λ(t) λ(t)：基函数(多项式)，3次多项式， λ ( t ) = [ t 3 , t 2 , t , 1 ] T λ(t)=[t^3 ,\ t^2 ,\ t ,\ 1]^T λ(t)=[t3, t2, t, 1]T
A A A：系数矩阵(2×4)

注意：基函数维度必须满足 N ≥ 2 ( b + 1 ) N≥2(b+1) N≥2(b+1)

步骤2：边界条件（起点 + 终点）

约束条件：

起点： y 0 y_0 y0, y ˙ 0 \dot y_0 y˙0
终点： y f y_f yf, y ˙ f \dot y_f y˙f

对应：
Y = [ x 0 x ˙ 0 x f x ˙ f y 0 y ˙ 0 y f y ˙ f ] Y=\left[ \begin{matrix} x_0 \ \dot x_0 \ x_f \ \dot x_f\\ y_0 \ \dot y_0 \ y_f \ \dot y_f \end{matrix} \right] Y=[x0 x˙0 xf x˙fy0 y˙0 yf y˙f]

每一列含义：

第 1 列：起点位置 y(0)

第 2 列：起点速度 ẏ(0)

第 3 列：终点位置 y(T)

第 4 列：终点速度 ẏ(T)

步骤3：求解系数矩阵 A

上述边界条件按列拼接为矩阵方程 Y = A Λ Y=AΛ Y=AΛ，则
A = Y ⋅ Λ − 1 A=Y⋅Λ^{−1} A=Y⋅Λ−1

其中， Λ Λ Λ 是 4×4 满秩矩阵(把 t = 0 t=0 t=0 和 t = T t=T t=T 代入 λ ( t ) λ(t) λ(t) 和 λ ˙ ( t ) λ̇(t) λ˙(t) 拼起来的矩阵)，
Λ = [ λ ( 0 ) λ ˙ ( 0 ) λ ( T ) λ ˙ ( T ) ] = [ 0 0 T 3 3 T 2 0 0 T 2 2 T 0 1 T 1 1 0 1 0 ] Λ= [λ(0) \quad \dotλ(0) \quad λ(T) \quad \dotλ(T)]= \left[ \begin{matrix} 0 & 0 & T^3&3 T^2\\ 0 & 0 & T^2& 2T\\ 0 & 1 & T&1\\ 1 & 0 & 1&0\\ \end{matrix} \right] Λ=[λ(0)λ˙(0)λ(T)λ˙(T)]= 00010010T3T2T13T22T10

注：每一行对应：

行 1：λ₁(t)=t³ 在 0、T 位置与导数

行 2：λ₂(t)=t² 在 0、T 位置与导数

行 3：λ₃(t)=t 在 0、T 位置与导数

行 4：λ₄(t)=1 在 0、T 位置与导数

步骤4：得到整条轨迹

由 A A A 得到完整轨迹与控制
x ( t ) , y ( t ) → θ ( t ) → v ( t ) → ω ( t ) x(t),y(t)→θ(t)→v(t)→ω(t) x(t),y(t)→θ(t)→v(t)→ω(t)

简单基于微分平坦生成的轨迹，执行如下命令

bash 复制代码

# 无参数
python3 differential_flatness.py

# 自定义 终止点位姿
python3 differential_flatness.py  5.0 6.0 1.75

# 自定义 起始点位姿 + 终止点位姿
 python3 differential_flatness.py  5.0 6.0 1.75 0 0 3.14

differential_flatness.py 脚本内容如下：

python 复制代码

import sys
import numpy as np
import matplotlib.pyplot as plt

# ==============================================
# 🔧 全局参数：角度单位设置
# 0 = 输入是 角度 (deg)
# 1 = 输入是 弧度 (rad)
# ==============================================
ANGLE_UNIT = 1

# ==================== 字体（Ubuntu 稳定）====================
plt.rcParams['font.sans-serif'] = ['DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False

# ==================== 命令行参数 ====================
argc = len(sys.argv)

# 默认值
x0, y0, yaw0 = 0.0, 0.0, 0.0
xf, yf, yawf = 5.0, 3.0, 1.5708
T = 5.0

if argc == 1:
    pass

elif argc == 4:
    xf = float(sys.argv[1])
    yf = float(sys.argv[2])
    yawf = float(sys.argv[3])

elif argc == 7:
    x0 = float(sys.argv[1])
    y0 = float(sys.argv[2])
    yaw0 = float(sys.argv[3])
    xf = float(sys.argv[4])
    yf = float(sys.argv[5])
    yawf = float(sys.argv[6])

else:
    print("Usage:")
    print("  No args    : python3 diff_plat.py")
    print("  3 args     : python3 diff_plat.py xf yf yaw")
    print("  6 args     : python3 diff_plat.py x0 y0 yaw0 xf yf yawf")
    print(f"ANGLE_UNIT   : {ANGLE_UNIT} (0=deg, 1=rad)")
    sys.exit(1)

# ==================== 统一转为弧度 ====================
if ANGLE_UNIT == 0:
    theta0 = np.deg2rad(yaw0)
    thetaf = np.deg2rad(yawf)
else:
    theta0 = yaw0
    thetaf = yawf

# ==================== 速度方向（朝向）====================
v0 = 1.0
dx0 = v0 * np.cos(theta0)
dy0 = v0 * np.sin(theta0)

vf = 1.0
dxf = vf * np.cos(thetaf)
dyf = vf * np.sin(thetaf)

# ==================== 微分平坦轨迹生成 ====================
Y = np.array([
    [x0, dx0, xf, dxf],
    [y0, dy0, yf, dyf]
])

Lambda = np.array([
    [0,   0,    T**3,  3*T**2],
    [0,   0,    T**2,  2*T   ],
    [0,   1,    T,     1     ],
    [1,   0,    1,     0     ]
])

Lambda_inv = np.linalg.inv(Lambda)
A = Y @ Lambda_inv

print("✅ Coefficient Matrix A:")
print(A)
print(f"✅ ANGLE_UNIT = {ANGLE_UNIT} (0=deg, 1=rad)")
print(f"✅ Start yaw: {theta0:.3f} rad | Goal yaw: {thetaf:.3f} rad")

# ==================== 生成轨迹 ====================
dt = 0.1
t_list = np.arange(0, T+dt, dt)

def get_traj(t):
    return A @ np.array([t**3, t**2, t, 1.0])

x_traj, y_traj = [], []
dx_traj, dy_traj = [], []
ddx_traj, ddy_traj = [], []

for t in t_list:
    x, y = get_traj(t)
    dx, dy = A @ np.array([3*t**2, 2*t, 1, 0])
    ddx, ddy = A @ np.array([6*t, 2, 0, 0])

    x_traj.append(x)
    y_traj.append(y)
    dx_traj.append(dx)
    dy_traj.append(dy)
    ddx_traj.append(ddx)
    ddy_traj.append(ddy)

x_traj = np.array(x_traj)
y_traj = np.array(y_traj)
dx_traj = np.array(dx_traj)
dy_traj = np.array(dy_traj)
ddx_traj = np.array(ddx_traj)
ddy_traj = np.array(ddy_traj)

# ==================== 状态 & 控制 ====================
theta_traj = np.arctan2(dy_traj, dx_traj)
v_traj = np.hypot(dx_traj, dy_traj)
omega_traj = (ddx_traj * dy_traj - dx_traj * ddy_traj) / (dx_traj**2 + dy_traj**2 + 1e-6)

# ==================== 绘图 ====================
plt.figure(figsize=(12,8))

plt.subplot(2,2,1)
plt.plot(x_traj, y_traj, 'b-', linewidth=2, label='Trajectory')
plt.scatter([x0, xf], [y0, yf], c='red', s=100, label='Start/Goal')
plt.xlabel('x')
plt.ylabel('y')
plt.title(f'Differential Flatness 3rd-order (Fixed Start/Goal)')
plt.grid(True)
plt.axis('equal')
plt.legend()

plt.subplot(2,2,2)
plt.plot(t_list, theta_traj, 'r-', linewidth=2, label='Actual yaw')
plt.axhline(thetaf, color='g', linestyle='--', linewidth=2, label='Target yaw')
plt.xlabel('t (s)')
plt.ylabel('yaw (rad)')
plt.title('Yaw Angle')
plt.grid(True)
plt.legend()

plt.subplot(2,2,3)
plt.plot(t_list, v_traj, 'm-', label='v')
plt.plot(t_list, omega_traj, 'c-', label='omega')
plt.xlabel('t (s)')
plt.ylabel('Control')
plt.title('Control Input v, ω')
plt.grid(True)
plt.legend()

plt.tight_layout()
plt.show()

微分平坦轨迹生成如下：