文章地址：Trajectory generation and control of a quadrotor with a cable-suspended load - A differentially-flat hybrid system | IEEE Conference Publication | IEEE Xplore+Geometric control and differential flatness of a quadrotor UAV with a cable-suspended load | IEEE Conference Publication | IEEE Xplore
摘要
考虑了一种具有8自由度并且存在4自由度欠驱动的绳载负载的四旋翼飞行器，将其建立为微分平坦混合动力系统。利用平坦特性，提出了一种轨迹生成方法，该方法能够找到具有各种约束的标称轨迹，这些轨迹不仅能在需要时产生最小的载荷摆动，而且还能在动态敏捷运动中引起较大的负载摆动。本文提出了一种控制器设计，专门用于平面情况下的系统，使其跟踪四旋翼姿态，负载姿态或负载位置。
引言
希望无人机最小化负载摆动的需求是显然易见的，但是我们也希望能使负载也能产生较大的负载摆动。
即针对如下两种情况：
a）为了使无人机携带绳载负载进入/退出窄口(如窗户)，载荷需要动态摆动进入窄口，从而使在正确的时间点，缆绳的张力为零，使得无人机能够在缆绳的张力重新出现之前在负载经历自由落体时穿过窗户。
b）为了使无人机能够在严格的天花板高度限制下飞行时运输悬浮载荷，且避免雷达探测或在室内飞行时，需要载荷的动态运动，以避免地面上的大型障碍物。
需要实现绳载负载的无人机系统的如下功能：实现具有大振荡相位的负载轨迹，以及在有限的持续时间内，当缆绳中的张力趋于零时，并使用适当设计的反馈控制器跟踪轨迹。
动力学模型
非零张力的动力学模型
如图1（a）四旋翼位置 $x_Q$ 和负载位置 $x_L$ 由如下公式联系，其中L为绳长，p为四旋翼指向负载的单位向量
$$x_Q=x_L-Lp\tag{1}$$四旋翼在绳载负载作用下的动力学方程为:
$$\dot{x}_{L}=v_{L}$$ $$\left(m_{Q}+m_{L}\right)\left(\dot{v}_{L}+g e_{3}\right)=\left(p \cdot f R e_{3}-m_{Q} L(\dot{p} \cdot \dot{p})\right) p$$ $$\dot{p}=\omega \times p$$ $$m_{Q} L \dot{\omega}=-p \times f R e_{3}$$ $$\dot{R}=R \hat{\Omega}$$ $$J_{Q} \dot{\Omega}+\Omega \times J_{Q} \Omega=M .\tag{2}$$为建立混合模型，上述动力学可以表示为标准形式$\dot{X}_{1}=f_{1}\left(X_{1}\right)+g_{1}\left(X_{1}\right) u$ ，其中$X_{1}=\left\{x_{L}, q, R, v_{L}, \omega, \Omega\right\}$ 为状态，$u=\{f, M\}$ 为系统的输入
注1:四旋翼姿态动力学分别与负载姿态和位置动力学解耦，而负载姿态动力学与负载位置动力学解耦。重力不影响负载姿态动力学。
注2：载荷姿态动力学(8)也可以直接写成载荷姿态 $p∈S^2$ ，其导数被表示为:
$m_{Q} L \ddot{p}+m_{Q} L(\dot{p} \cdot \dot{p}) p=p \times\left(p \times f \operatorname{Re}_{3}\right)$零张力的动力学模型
四旋翼和负载作为独立的系统，负载呈自由落体状态。（两个系统都是微分平坦的）对应的动力学模型为
$$\dot{x}_{L}=v_{L}$$ $$m_{L}\left(\dot{v}_{L}+g e_{3}\right)=0$$ , $$\dot{x}_{Q}=v_{Q}$$ , $$m_{Q}\left(\dot{v}_{Q}+g e_{3}\right)=f R e_{3}$$ , $$\dot{R}=R \hat{\Omega}$$ , $$J_{Q} \dot{\Omega}+\Omega \times J_{Q} \Omega=M .\tag{3}$$为建立混合模型，上述动力学可以表示为标准形式$\dot{X}_{2}=f_{2}\left(X_{1}\right)+g_{2}\left(X_{2}\right) u$ ，其中$X_{2}=\left\{x_{L}, q, R, v_{L}, \omega, \Omega\right\}$ 为状态，$u=\{f, M\}$ 为系统的输入
专门针对平面的动力学模型
如图1（b）， $φ_L$ 为负载的滚转角， $φ_Q$ 为四旋翼的滚转角，p和旋转矩阵R定义如下
$$p=\left[\begin{array}{c}\sin \left(\phi_{L}\right) \\ -\cos \left(\phi_{L}\right)\end{array}\right], \quad R=\left[\begin{array}{cc}\cos \left(\phi_{Q}\right) & -\sin \left(\phi_{Q}\right) \\ \sin \left(\phi_{Q}\right) & \cos \left(\phi_{Q}\right)\end{array}\right] \tag{3}$$由公式（2）得到的平面动力学方程为：
$$\left(m_{Q}+m_{L}\right)\left(\dot{v}_{L}+g e_{3}\right)=\left(f \cos \left(\phi_{Q}-\phi_{L}\right)-m_{Q} l \dot{\phi}_{L}^{2}\right) p$$ $$m_{Q} l \ddot{\phi}_{L}=\sin \left(\phi_{Q}-\phi_{L}\right)$$ $$J_{Q} \ddot{\phi}_{Q}=M$$微分平坦性
如果存在一组平坦输出，系统的状态和输入可以表示为平坦输出及其高阶导数的平滑函数，则系统是微分平坦的。
本文采用微分平面度法对四旋翼系统进行了动态轨迹规划，本文证明了具有缆索悬挂负载的四旋翼是微分平坦的，而且它是一个定义如下的微分平坦混合系统。
$$\Sigma: \begin{cases}\dot{x}_{1}=f_{1}\left(x_{1}\right)+g_{1}\left(x_{1}\right) u_{1}, & x_{1} \notin \mathcal{S}_{1} \\ x_{2}^{+}=\Delta_{1}\left(x_{1}^{-}\right), & x_{1}^{-} \in \mathcal{S}_{1} \\ \dot{x}_{2}=f_{2}\left(x_{2}\right)+g_{2}\left(x_{2}\right) u_{2}, & x_{2} \notin \mathcal{S}_{2} \\ x_{1}^{+}=\Delta_{2}\left(x_{2}^{-}\right), & x_{2}^{-} \in \mathcal{S}_{2},\end{cases} \tag{4}$$假设 $x_1, x_2$ 的动力学是微分平坦的，分别有一个平坦的输出 $y_1,y_2$ 。输出 $y_2$ 的过渡后值是平坦输出 $y_1 $ 的过渡前值及其高阶导数的函数，则系统Σ是微分平坦混合系统。
为了表明由四旋翼和缆索悬挂载荷组成的系统是微分平坦的，我们将看由Newton-Euler推导出的动力学运动方程，从而系统的内部约束力（对应于缆索中的张力）被引入了。如图1a，运动方程可以写成公式（5）， T为缆绳的张力。
$$m_{Q} \ddot{x}_{Q}=f R e_{3}-m_{Q} g e_{3}+T p$$ $$J \dot{\Omega}+\Omega \times J \Omega=M$$ $$m_{L} \ddot{x}_{L}=-T p-m_{L} g e_{3}\tag{5}$$定理1： $y_1 = (x_L,ψ)$ 是上述系统的平坦输出，其中 $ψ∈R$ 是四旋翼飞行器的偏航角。
证明：由公式（5）可以得到单位向量p，从而由公式（1）可以得到四旋翼无人机的位置 $x_Q$ 。而 $(x_Q,ψ)$ 为四旋翼无人机的平坦输出，则 $y_1 = (x_L,ψ)$ 是上述系统的平坦输出。（仅当缆绳绷紧时成立，拉力为零时不成立）
注2:输入M（四旋翼在固定机体坐标系中的力矩矢量）由负载位置的6阶导数求得。
控制器设计（Geometric control and differential flatness of a quadrotor UAV with a cable-suspended load）
控制器中的误差项的定义如下
$$\begin{aligned} e_{R} &=\frac{1}{2}\left(R_{d}^{T} R-R^{T} R_{d}\right)^{\vee} \\ e_{\Omega} &=\Omega-R^{T} R_{d} \Omega_{d} \end{aligned}$$ $$\begin{aligned} e_{q} &=\hat{q}^{2} q_{d} \\ e_{\dot{q}} &=\dot{q}-\left(q_{d} \times \dot{q}_{d}\right) \times q \end{aligned}$$ $$e_{x}=x-x_{d}$$ $$e_{v}=v-v_{d}$$四旋翼力矩定义如下：
$$\begin{aligned} M=&-\frac{1}{\epsilon^{2}} k_{R} e_{R}-\frac{1}{\epsilon} k_{\Omega} e_{\Omega}+\Omega \times J_{Q} \Omega \\ &-J_{Q}\left(\hat{\Omega} R^{T} R_{d} \Omega_{d}-R^{T} R_{d} \dot{\Omega}_{d}\right) \end{aligned}$$四旋翼的姿态定义如下
$$R_{c}:=\left[b_{1_{c}} ; b_{3_{c}} \times b_{1_{c}} ; b_{3_{c}}\right], \quad \hat{\Omega}_{c}=R_{c}^{T} \dot{R}_{c}$$ $$b_{3_{c}}=\frac{F}{\|F\|},$$其中，
$$F=F_{n}-F_{p d}-F_{f f},$$ $$\begin{array}{r} F_{n}=-\left(q_{d} \cdot q\right) q \\ F_{p d}=-k_{q} e_{q}-k_{\omega} e_{\dot{q}} \\ F_{f f}=m_{Q} l\left\langle q, q_{d} \times \dot{q}_{d}\right\rangle(q \times \dot{q})+m_{Q} l\left(q_{d} \times \ddot{q}_{d}\right) \times q . \end{array}$$ $$b_{1_{c}}=-\frac{1}{\left\|b_{3_{c}} \times b_{1_{d}}\right\|}\left(b_{3_{c}} \times\left(b_{3_{c}} \times b_{1_{d}}\right)\right) .$$ $$f=F \cdot R e_{3},$$负载的姿态定义如下：
$$q_{c}=-\frac{A}{\|A\|}$$ $$A=-k_{x} e_{x}-k_{v} e_{v}+\left(m_{Q}+m_{L}\right)\left(\ddot{x}_{L}^{d}+g e_{3}\right)+m_{Q} l(\dot{q} \cdot \dot{q}) q,$$控制器设计（Trajectory Generation and Control of a Quadrotor with a Cable-Suspended Load– A Differentially-Flat Hybrid System）
上述控制器可以跟踪(a) 无人机姿态,(b)负载姿态,或(c)负载位置
四旋翼力矩定义如下：
$$M=J_{Q}\left(-k_{p}^{Q} e_{Q}-k_{d}^{Q} \dot{e}_{Q}+\ddot{\phi}_{Q}^{d}\right)$$其中， $e_Q =φ_Q−φ^d_Q$ 为跟踪期望的四旋翼姿态的误差。
四旋翼姿态定义如下：
$$\phi_{Q}^{c}:=\phi_{L}+\sin ^{-1}\left(-k_{p}^{L} e_{L}-k_{d}^{L} \dot{e}_{L}+\frac{\ddot{\phi}_{L}^{d} m_{Q} l}{f}\right)$$其中， $e_L =φ_L−φ^d_L$ 为跟踪期望负载姿态的误差。
四旋翼推力和期望负载姿态定义如下：
$$f=(A+B) \cdot R e_{3}$$ $$p_{d}=-\frac{A}{\|A\|} \Longrightarrow \phi_{L}^{d}=\tan ^{-1}\left(\frac{-A_{1}}{A_{2}}\right)$$其中，A，B定义如下， $A_1,A_2$ 为A的分量。
$$\begin{aligned} A &=-k_{p}^{x} e_{x}-k_{d}^{x} \dot{e}_{x}+m_{L} \ddot{x}_{L}^{d}+m_{L} g e_{3} \\ B &=m_{Q} \ddot{x}_{Q}^{d}+m_{Q} g e_{3} \\ &=m_{L} \ddot{x}_{L}^{d}-m_{Q} l \ddot{p}^{d}+m_{Q} g e_{3} \end{aligned}$$其中 $e_{x}=x_{L}-x_{L}^{d}, e_{v}=\dot{e}_{x}$ 为跟踪期望的负载位置的误差。
可以看出Trajectory …的控制器只是把Geometric…中的用旋转矩阵以及角速度表示的四旋翼姿态用更简单更直接的四旋翼姿态角表示，更方面在实际的控制器设计中实现。
实验
我们首先讨论了轨迹生成，然后介绍了反馈控制跟踪的结果（见原文）。
我们选择一个合适的基函数将每个平坦输出参数化为时间的函数。在初始状态和最终状态的约束下，求解基多项式的系数以最小化负载位置的六阶导数。将负载位置的六阶导数最小化，确保四旋翼的快速运动最小。即我们需要对如下公式求解：
$$\min \int_{t_{0}}^{t_{1}}\left\|\frac{d^{k} x_{i}}{d t^{k}}\right\|^{2} d t$$其中$x_{i}(t)=\Sigma_{j=0}^{n} c_{i, j} \beta_{j}(t)$是参数化的平面输出。而$\beta_{j}(t)$
