六自由度四旋翼悬停恢复

背景

小型四旋翼偏离悬停状态后，需要在转速和转速变化能力均受限时，同时 消除平移和转动。由于推力方向随姿态变化，即使不考虑空气动力，这仍是 一个平移与转动耦合的非线性最优控制问题。

本例在固定的 5 秒时域内，把一架 Crazyflie 量级的飞行器恢复到原点 悬停。姿态采用三个修正罗德里格斯参数（MRP），而不是四维四元数， 因此不需要冗余状态和单位范数等式约束。

问题定义

令 \(\boldsymbol r,\boldsymbol v\in\mathbb R^3\) 为惯性系位置和速度， \(\boldsymbol p\in\mathbb R^3\) 为 MRP 向量， \(\boldsymbol\omega\in\mathbb R^3\) 为机体系角速度。第 \(i\) 个转子的 执行器状态定义为无量纲转速平方比

\[ c_i=\left(\frac{\Omega_i}{\Omega_h}\right)^2, \qquad \Omega_h=\sqrt{\frac{mg}{4k_f}}, \]

其中 \(\Omega_i\) 是转速，\(\Omega_h\) 是水平悬停转速；控制量为其变化率 \(\nu_i=\dot c_i\)。对应的机体系推力为

\[ \boldsymbol f_i=\frac{mg}{4}c_i\boldsymbol e_3. \]

模型采用以下四个力臂和机体反作用偏航力矩符号：

\[ \begin{aligned} \boldsymbol\rho_1&=(l,l,0)^{\mathsf T},& \boldsymbol\rho_2&=(l,-l,0)^{\mathsf T},\\ \boldsymbol\rho_3&=(-l,-l,0)^{\mathsf T},& \boldsymbol\rho_4&=(-l, l,0)^{\mathsf T},\\ l&=0.028\ \mathrm m,& (d_1,d_2,d_3,d_4)&=(+1,-1,+1,-1). \end{aligned} \]

这里 \(d_i\) 表示转子作用在机体上、绕 \(+\boldsymbol e_3\) 轴的反作用 偏航力矩符号，并不是桨叶自转方向本身的符号。机体系合力与合力矩为

\[ \boldsymbol F_b=\sum_{i=1}^4\boldsymbol f_i, \qquad \boldsymbol\tau_b=\sum_{i=1}^4 \left( \boldsymbol\rho_i\times\boldsymbol f_i +d_i\frac{k_m}{k_f}\boldsymbol f_i \right). \]

机体系到惯性系的旋转矩阵 \(R(\boldsymbol p)\) 由非冗余 MRP 映射得到：

\[ q_0=\frac{1-\boldsymbol p^{\mathsf T}\boldsymbol p} {1+\boldsymbol p^{\mathsf T}\boldsymbol p}, \qquad \boldsymbol q=\frac{2\boldsymbol p} {1+\boldsymbol p^{\mathsf T}\boldsymbol p}. \]

系统动力学为

\[ \begin{aligned} \dot{\boldsymbol r}&=\boldsymbol v,\\ \dot{\boldsymbol v}&=\frac{1}{m}R(\boldsymbol p)\boldsymbol F_b -g\boldsymbol e_3,\\ \dot{\boldsymbol p}&=\frac14 \left[(1-\boldsymbol p^{\mathsf T}\boldsymbol p)I +2[\boldsymbol p]_\times +2\boldsymbol p\boldsymbol p^{\mathsf T}\right]\boldsymbol\omega,\\ \dot{\boldsymbol\omega}&=I_b^{-1} \left(\boldsymbol\tau_b- \boldsymbol\omega\times I_b\boldsymbol\omega\right),\\ \dot c_i&=\nu_i,\qquad i=1,\ldots,4. \end{aligned} \]

这里 \([\boldsymbol p]_\times\) 是叉乘矩阵。固定终端时间为 \(T=5\ \mathrm s\)，初始状态为

\[ \begin{aligned} \boldsymbol r(0)&=(1.5,-1.0,1.8)^{\mathsf T}\ \mathrm m,\\ \boldsymbol v(0)&=(0.6,-0.4,0.2)^{\mathsf T}\ \mathrm{m/s},\\ \boldsymbol p(0)&=(0.15,-0.10,0.08)^{\mathsf T},\\ \boldsymbol\omega(0)&=(0.45,-0.35,0.25)^{\mathsf T}\ \mathrm{rad/s}. \end{aligned} \]

四个转子在首尾均取平衡悬停命令：

\[ c_i(0)=c_i(T)=1,\qquad i=1,\ldots,4. \]

因此终端执行器状态产生的总推力为 \(mg\)，机体系合力矩为零。终端刚体 状态自由，但受到较强惩罚。

目标函数中的每一项都先作无量纲化。定义

\[ \bar{\boldsymbol r}=\frac{\boldsymbol r}{r_s},\quad \bar{\boldsymbol v}=\frac{\boldsymbol v}{v_s},\quad \boldsymbol e_p=\frac{4\boldsymbol p}{\theta_s},\quad \bar{\boldsymbol\omega}=\frac{\boldsymbol\omega}{\omega_s},\quad \bar\nu_i=\frac{\nu_i}{\nu_s}, \]

其中 \(r_s=1\ \mathrm m\)、\(v_s=1\ \mathrm{m/s}\)、 \(\theta_s=1\ \mathrm{rad}\)、\(\omega_s=1\ \mathrm{rad/s}\)， \(\nu_s=4\ \mathrm{s^{-1}}\)。光滑的局部姿态误差代理 \(4\boldsymbol p\) 在悬停邻域与主旋转向量一阶一致。无量纲目标函数为

\[ \begin{aligned} \min J={}&\frac{1}{T}\int_0^T \left[ 5\lVert\bar{\boldsymbol r}\rVert_2^2 +1.5\lVert\bar{\boldsymbol v}\rVert_2^2 +8\lVert\boldsymbol e_p\rVert_2^2 +0.5\lVert\bar{\boldsymbol\omega}\rVert_2^2 +0.06\sum_{i=1}^4(c_i-1)^2 +0.02\sum_{i=1}^4\bar\nu_i^2 \right]\,\mathrm dt\\ &+800\lVert\bar{\boldsymbol r}(T)\rVert_2^2 +250\lVert\bar{\boldsymbol v}(T)\rVert_2^2 +500\lVert\boldsymbol e_p(T)\rVert_2^2 +120\lVert\bar{\boldsymbol\omega}(T)\rVert_2^2. \end{aligned} \]

转子和姿态局部参数域的路径约束为

\[ 0\le c_i(t)\le \left(\frac{2500}{\Omega_h}\right)^2, \qquad |\nu_i(t)|\le4\ \mathrm{s^{-1}}, \qquad \lVert\boldsymbol p(t)\rVert_2 \le\tan\left(\frac{150^\circ}{4}\right). \]

最后一个不等式把主旋转角限制在 \(150^\circ\) 内。

变量、参数与单位

符号	含义	数值或单位
\(\boldsymbol r,\boldsymbol v\)	惯性系位置和速度	m、m/s
\(\boldsymbol p\)	修正罗德里格斯姿态向量	无量纲
\(\boldsymbol\omega\)	机体系角速度	rad/s
\(c_i\)	转速平方比；执行器状态	无量纲
\(\nu_i=\dot c_i\)	转速平方比变化率；控制量	1/s
\(\Omega_i\)	转子物理转速	rad/s
\(m\)	飞行器质量	\(0.033\ \mathrm{kg}\)
\(I_b\)	机体惯量对角线	\((1.395,1.395,2.173)\times10^{-5}\ \mathrm{kg\,m^2}\)
\(k_f\)	推力系数	\(2.3\times10^{-8}\ \mathrm{N/(rad/s)^2}\)
\(k_m\)	阻力矩系数	\(7.8\times10^{-10}\ \mathrm{N\,m/(rad/s)^2}\)
\(l\)	转子力臂半跨距	\(0.028\ \mathrm m\)
\(d_i\)	机体反作用偏航力矩符号	\(+1,-1,+1,-1\)
\(r_s,v_s,\theta_s,\omega_s,\nu_s\)	目标函数参考尺度	\(1\ \mathrm m,\ 1\ \mathrm{m/s},\ 1\ \mathrm{rad},\ 1\ \mathrm{rad/s},\ 4\ \mathrm{s^{-1}}\)

建模选择与适用边界

MRP 是最小维、光滑的局部姿态坐标，能够避免额外的四元数单位范数 约束；但它不是全局姿态表示，在整周 \(360^\circ\) 旋转处存在奇异性。 本例也没有实现影子集切换，因此显式加入 \(150^\circ\) 路径边界，只适合 局部恢复机动，不适合任意翻滚轨迹。

模型包含刚体平移、陀螺转动、力臂力矩、转子阻力矩和连续且受变化率 约束的执行器命令。状态 \(c_i\) 是命令整形模型，而不是电气或机电电机 模型：变化率上限约束的是转速平方比，并不直接等同于转轴角加速度。 模型还省略了气动阻力、地面效应、传感器噪声和外部扰动。默认离散为 \(44\times2\) Lobatto 网格；--quick 只用于粗网格冒烟测试。

运行示例

在仓库根目录运行：

python -m examples.drone_stabilization

使用统一绘图风格保存图片且不打开窗口：

python -m examples.drone_stabilization --save drone-stabilization.png --no-show

验证结果

求解后，脚本在 4,001 个物理时刻重构全部状态与控制，把无量纲转子状态 换算为物理转速，检查密集网格上的转速、变化率和 MRP 参数域边界，并 核对优化终端状态与阶段端点。脚本还分别要求终端位置、速度、姿态和角 速度误差足够小，四个命令均等于 1，而且终端线加速度与角加速度足够小。 最后两项检查能够区分真正的平衡悬停与“状态恰好经过零附近、但终端力矩 仍不平衡”的轨迹。

指标	默认求解值
目标函数 \(J\)	\(8.54936175\)
终端位置误差	\(0.000001\ \mathrm m\)
终端速度误差	\(0.000002\ \mathrm{m/s}\)
终端主姿态角误差	\(0.000089^\circ\)
终端角速度误差	\(0.000000\ \mathrm{rad/s}\)
最大终端转子命令误差	按表中精度为 \(0\)
终端线加速度范数	\(1.532\times10^{-5}\ \mathrm{m/s^2}\)
终端角加速度范数	\(3.843\times10^{-19}\ \mathrm{rad/s^2}\)
密集转速范围	\(1400.89\) 至 \(2164.74\ \mathrm{rad/s}\)
转速平方比变化率峰值	\(4.0000\ \mathrm{s^{-1}}\)
总推力峰值	\(0.4258\ \mathrm N\)
MRP 参数域最小径向余量	\(0.569544\)

结果不仅把飞行器恢复到接近静止，还使终端转子状态产生平衡悬停力和 力矩，而不只是减小运动学状态误差。密集重构后的转速和变化率均满足 边界。这些数值对应离散问题的一个局部最优解，并不构成全局最优性证明。

源代码

完整可运行示例见： examples/drone_stabilization.py。