无人机吊运系统（二级摆）——几何建模

无人机吊运系统（二级摆）——几何建模

---

---

前言

无人机（UAV）由于其高机动性、体积小等优势被广泛应用于吊运任务上，简化的吊运系统一般由无人机本体、轻质绳、重物组成，但实际上无人机吊运系统常常需要单独采用一挂钩另外悬挂重物，如图所示：

（图片来自网络，侵权请联系删除）
即由无人机本体悬挂吊钩（Hook），由吊钩悬挂重物（Payload），过去大部分关于无人机吊运系统的分析工作中，都将模型简化为单摆，但两级摆动实际会对无人机控制带来不小的影响（这在吊车的领域可以看到），为了后续进一步对无人机控制做分析，本文提出从几何角度对无人机吊运系统进行建模，其好处是模型结构相对简洁。

---

一、建模原理

拉格朗日法

定义系统拉格朗日量为：
$$L(q,\dot{q})=T(q,\dot{q})-V(q)$$
其中$q$为系统广义坐标，$T$为系统总动能，$V$为系统总势能，则动力学方程可用拉格朗日函数表示如下：
$$f=\frac{d}{dt}\frac{\partial L}{\partial \dot{q}}-\frac{\partial L}{\partial q}$$

达朗贝尔原则（D’Alembert’s Principle）

对于一个质点系统，每一个质点的动力效应，即实际作用在质点上的力与质点的惯性力（虚拟的力，等于质点质量与加速度乘积的相反数）的矢量和，可以视为在任意虚位移上的总静力效应为零。
数学上表述为：
$$\sum ({\mathbf{F}}_{\mathbf{i}}-m_i{\mathbf{a}}_{\mathbf{i}})\cdot \partial {\mathbf{r}}_{\mathbf{i}}=0$$
其中，${\mathbf{F}}_{\mathbf{i}}$为作用在第$i$个质点的实际力，$m_i$为第$i$个质点的质量，${\mathbf{a}}_{\mathbf{i}}$为第$i$个质点的加速度，$\partial {\mathbf{r}}_{\mathbf{i}}$是一个虚位移。
没有学过理论力学的同学可能不太好第一时间理解，其实达朗贝尔原则的优势在于他化解了面对处于运动状态的系统的受力分析，可以理解为将牛二定律升级为非惯性系下的表述。在拉格朗日力学中，达朗贝尔原则常用于导出拉格朗日方程，这是描述系统动态的基本方程。

二、使用步骤

变量定义：

• ${\mathbf{x}}_{\mathbf{Q}}、{\mathbf{x}}_{\mathbf{h}}、{\mathbf{x}}_{\mathbf{L}}$分别为无人机位置、挂钩位置（hook）、负载位置（Load）；
• $l_1、l_2$：吊绳1、2长度；
• ${\mathbf{p}}_1、{\mathbf{p}}_2$：方向向量，分别代表由无人机质心指向挂钩、由挂钩指向负载；
• $m_Q、m_h、m_L$：分别为无人机质量、挂钩质量（hook）、负载质量（Load）；
• $g$：重力加速度；
• $L$：拉格朗日量；
• $T$：系统总动能；
• $V$：系统总势能；
• $R$：无人机旋转矩阵；

1.系统能量

根据系统示意图，有：
$$\begin{array}{rl}{\mathbf{x}}_{\mathbf{h}}& ={\mathbf{x}}_{\mathbf{L}}-l_2{\mathbf{p}}_2\\ {\mathbf{x}}_{\mathbf{Q}}& ={\mathbf{x}}_{\mathbf{h}}-l_1{\mathbf{p}}_1={\mathbf{x}}_{\mathbf{L}}-l_2{\mathbf{p}}_2-l_1{\mathbf{p}}_1\\ T& =\frac{1}{2}{m}_Q{\mathbf{v}}_{\mathbf{Q}}\cdot {\mathbf{v}}_{\mathbf{Q}}+\frac{1}{2}{m}_h{\mathbf{v}}_{\mathbf{h}}\cdot {\mathbf{v}}_{\mathbf{h}}+\frac{1}{2}{m}_L{\mathbf{v}}_{\mathbf{L}}\cdot {\mathbf{v}}_{\mathbf{L}}+\frac{1}{2}{\Omega }^{\top }J\Omega \\ V& =m_{Q}g{\mathbf{e}}_3\cdot {\mathbf{x}}_{\mathbf{Q}}+m_{h}g{\mathbf{e}}_3\cdot {\mathbf{x}}_{\mathbf{h}}+m_{L}g{\mathbf{e}}_3\cdot {\mathbf{x}}_{\mathbf{L}}\\ \therefore L& =T-V\\ & =\frac{1}{2}{m}_Q\left({\dot{\mathbf{x}}}_L-L_1{\dot{\mathbf{p}}}_1-l_2{\dot{\mathbf{p}}}_2\right)\cdot \left({\dot{\mathbf{x}}}_L-l_1{p}_1-L_2{\dot{\mathbf{p}}}_2\right)+\frac{1}{2}{m}_h\left({\dot{\mathbf{x}}}_L-L_2{\dot{\mathbf{p}}}_2\right)\cdot \left({\dot{\mathbf{x}}}_L-L_1{\dot{\mathbf{p}}}_2\right)\\ & +\frac{1}{2}{m}_L{\dot{\mathbf{x}}}_L\cdot {\dot{\mathbf{x}}}_L+\frac{1}{2}{\Omega }^{\top }J\Omega \\ & -m_{Q}g{\mathbf{e}}_3\cdot \left({\mathbf{x}}_{\mathbf{L}}-L_1{\mathbf{p}}_1-l_2{\mathbf{p}}_2\right)-m_{h}g{\mathbf{e}}_3\cdot \left({\mathbf{x}}_{\mathbf{L}}-L_2{\mathbf{p}}_2\right)-m_{L}g{\mathbf{e}}_3\cdot {\mathbf{x}}_{\mathbf{L}}\end{array}$$
由此构建好二级摆系统的拉格朗日量。

注意：由于无人机姿态在这里与负载动力学解耦，为了主要展现二级摆部分几何建模，后将无人机本体旋转部分能量忽略。

2.建模

准备好拉格朗日方程需要的偏导量：
$$\begin{array}{rl}\frac{\partial L}{\partial {\mathbf{x}}_{\mathbf{L}}}=& -\left(m_Q+m_h+m_L\right)g{\mathbf{e}}_3\\ \frac{d}{dt}\frac{\partial L}{\partial {\dot{\mathbf{x}}}_2}=& m_Q\left({\ddot{\mathbf{x}}}_L-l_1{\ddot{\mathbf{p}}}_1-l_1{\ddot{\mathbf{p}}}_2\right)+m_h\left({\ddot{\mathbf{x}}}_L-l_2{\ddot{\mathbf{p}}}_2\right)+m_L{\ddot{\mathbf{x}}}_L\\ \frac{\partial L}{\partial {\mathbf{p}}_1}=& -m_{Q}g{\mathbf{e}}_3{l}_1\\ \frac{d}{dt}\frac{\partial L}{\partial {\dot{\mathbf{p}}}_1}=& -m_Q{l}_1\left({\ddot{\mathbf{x}}}_L-l_1{\ddot{\mathbf{p}}}_1-l_2{\ddot{\mathbf{p}}}_2\right)\\ \frac{\partial L}{\partial {\mathbf{p}}_2}=& -m_{Q}g{\mathbf{e}}_3{l}_2-m_{h}g{\mathbf{e}}_3{l}_2\\ \frac{d}{dt}\frac{\partial L}{\partial {\mathbf{p}}_2}=& -m_Q{l}_2\left({\ddot{\mathbf{x}}}_L-l_1{\ddot{\mathbf{p}}}_1-l_2{\ddot{\mathbf{p}}}_2\right)-m_h{l}_2\left({\ddot{\mathbf{x}}}_L-l_2{\ddot{\mathbf{p}}}_2\right)\end{array}$$
另外关于 $\delta {\mathbf{p}}_1$和 $\delta {\mathbf{p}}_2$有：
$$\delta {\mathbf{p}}_1={\mathbf{\xi }}_1\times {\mathbf{p}}_1\delta {\mathbf{p}}_2={\mathbf{\xi }}_2\times {\mathbf{p}}_2$$
现定义动作积分函数 $G={\int }_{t_0}^{t_f}Ldt$
$$\begin{array}{rl}& \delta G={\int }_{t_0}^{t_f}[\left(\frac{\partial L}{\partial {\mathbf{x}}_{\mathbf{L}}}-\frac{d}{dt}\frac{\partial L}{\partial {\dot{\mathbf{x}}}_L}\right)\partial {\mathbf{x}}_{\mathbf{L}}+\left(\frac{\partial L}{\partial {\mathbf{p}}_1}-\frac{d}{dt}\frac{\partial L}{\partial {\dot{\mathbf{p}}}_1}\right)\delta {\mathbf{p}}_1\\ & +\left(\frac{\partial L}{\partial {\mathbf{p}}_2}-\frac{d}{dt}\frac{\partial L}{\partial {\mathbf{p}}_2}\right)\delta {\mathbf{p}}_2]dt\end{array}$$
考虑到系统唯一外力为无人机升力$fR{\mathbf{e}}_3$，由达朗贝尔原则知:
$$\begin{array}{r}\delta G=-{\int }_{t_0}^{t_1}fR{\mathbf{e}}_3\delta {\mathbf{x}}_{\mathbf{Q}}dt=-{\int }_{t_0}^{t_1}\left(\delta {\mathbf{x}}_{\mathbf{L}}-L_1\delta {\mathbf{p}}_1-l_2\partial {\mathbf{p}}_2\right)fR{\mathbf{e}}_{3}dt\end{array}$$
现将对应项相等列写：
$$\begin{array}{rl}\text{①}& \left(\frac{\partial L}{\partial {\mathbf{x}}_{\mathbf{L}}}-\frac{d}{dt}\frac{\partial L}{\partial {\dot{\mathbf{x}}}_L}\right)\partial {\mathbf{x}}_{\mathbf{L}}=\left[-\left(m_Q+m_h+m_L\right)g{\mathbf{e}}_3-m_Q\left({\ddot{\mathbf{x}}}_L-l_1{\ddot{\mathbf{p}}}_1-l_1{\ddot{\mathbf{p}}}_2\right)-m_h\left({\ddot{\mathbf{x}}}_L-l_2{\ddot{\mathbf{p}}}_2\right)-m_L{\ddot{\mathbf{x}}}_L\right]\partial {\mathbf{x}}_{\mathbf{L}}=-fR{\mathbf{e}}_3\delta {\mathbf{x}}_L\\ \text{②}& \left(\frac{\partial L}{\partial {\mathbf{p}}_1}-\frac{d}{dt}\frac{\partial L}{\partial {\dot{\mathbf{p}}}_1}\right)\delta {\mathbf{p}}_1=\left[-m_{Q}g{\mathbf{e}}_3{l}_1+m_Q{l}_1\left({\ddot{\mathbf{x}}}_L-l_1{\ddot{\mathbf{p}}}_1-l_2{\ddot{\mathbf{p}}}_2\right)\right]\left({\mathbf{\xi }}_1\times {\mathbf{p}}_1\right)=fR{\mathbf{e}}_3{l}_1\left({\mathbf{\xi }}_1\times {\mathbf{p}}_1\right)\\ \text{③}& \left(\frac{\partial L}{\partial {\mathbf{p}}_2}-\frac{d}{dt}\frac{\partial L}{\partial {\dot{\mathbf{p}}}_2}\right)\delta {\mathbf{p}}_2=\left[-m_{Q}g{\mathbf{e}}_3{l}_2-m_{h}g{\mathbf{e}}_3{l}_2+m_Q{l}_2\left({\ddot{\mathbf{x}}}_L-l_1{\ddot{\mathbf{p}}}_1-l_2{\ddot{\mathbf{p}}}_2\right)+m_h{l}_2\left({\ddot{\mathbf{x}}}_L-l_2{\ddot{\mathbf{p}}}_2\right)\right]\left({\mathbf{\xi }}_2\times {\mathbf{p}}_2\right)=fR{\mathbf{e}}_3{l}_2\left({\mathbf{\xi }}_2\times {\mathbf{p}}_2\right)\end{array}$$
注意，由于上式均涉及到向量乘法/叉乘运算，在进行约去时需要格外注意，必要时需结合物理受力情况进行分析：

关于①式：

$$\left[-\left(m_Q+m_h+m_L\right)g{\mathbf{e}}_3-m_Q\left({\ddot{\mathbf{x}}}_L-l_1{\ddot{\mathbf{p}}}_1-l_1{\ddot{\mathbf{p}}}_2\right)+m_h\left({\ddot{\mathbf{x}}}_L-l_2{\ddot{\mathbf{p}}}_2\right)+m_L{\ddot{\mathbf{x}}}_L\right]$$
显然由系统重力、惯性力合成，与系统所受外力同向，即与$-fR{\mathbf{e}}_3$同向，故$\delta {\mathbf{x}}_L$可直接约去，得到：
$$fR{e}_3=\left(m_Q+m_h+m_L\right)\left(g{Q}_3+{\dot{\nu }}_L\right)-\left(m_Q+m_h\right)l_2{\ddot{\mathbf{p}}}_2-m_Q{l}_1{\ddot{\mathbf{p}}}_1$$

关于②式：

根据向量叉乘运算性质，可化为：
$$\left({\mathbf{p}}_1\times \left[-m_{Q}g{\mathbf{e}}_3{l}_1+m_Q{l}_1\left({\ddot{\mathbf{x}}}_L-l_1{\ddot{\mathbf{p}}}_1-l_2{\ddot{\mathbf{p}}}_2\right)\right]\right)\cdot {\mathbf{\xi }}_1=\left({\mathbf{p}}_1\times fR{\mathbf{e}}_3{l}_1\right)\cdot {\mathbf{\xi }}_1$$

已知${\mathbf{\xi }}_1\cdot {\mathbf{p}}_1=0$，即向量${\mathbf{\xi }}_1$与${\mathbf{p}}_1$垂直，现若能说明$\left({\mathbf{p}}_1\times \left[m_{Q}g{\mathbf{e}}_3{l}_1+m_Q{l}_1\left({\ddot{\mathbf{x}}}_L-l_1{\ddot{\mathbf{p}}}_1-l_2{\ddot{\mathbf{p}}}_2\right)\right]\right)$与$\left({\mathbf{p}}_1\times fR{\mathbf{e}}_3\right)$同向，则可约去${\mathbf{\xi }}_1$，而同向问题可转化为${\mathbf{p}}_1、\left[m_{Q}g{\mathbf{e}}_3{l}_1+m_Q{l}_1\left({\ddot{\mathbf{x}}}_L-l_1{\ddot{\mathbf{p}}}_1-l_2{\ddot{\mathbf{p}}}_2\right)\right]、fR{\mathbf{e}}_3$是否共平面，对无人机本体做受力分析，可发现以上三项正分别代表了无人机所受重力、升力、第一根挂绳拉力三个方向，故三者必共平面，由此可约去${\mathbf{\xi }}_1$，方程变为(将$l_1$约去)：
$${\mathbf{p}}_1\times \left[-m_{Q}g{\mathbf{e}}_3+m_Q\left({\ddot{\mathbf{x}}}_L-l_1{\ddot{\mathbf{p}}}_1-l_2{\ddot{\mathbf{p}}}_2\right)\right]={\mathbf{p}}_1\times fR{\mathbf{e}}_3$$

关于③式：

依照②式的推导方法，同理得到：
$${\mathbf{p}}_2\times \left[-m_{Q}g{\mathbf{e}}_3{l}_2-m_{h}g{\mathbf{e}}_3{l}_2+m_Q{l}_2\left({\ddot{\mathbf{x}}}_L-l_1{\ddot{\mathbf{p}}}_1-l_2{\ddot{\mathbf{p}}}_2\right)+m_h{l}_2\left({\ddot{\mathbf{x}}}_L-l_2{\ddot{\mathbf{p}}}_2\right)\right]={\mathbf{p}}_2\times fR{\mathbf{e}}_3{l}_2$$
但是对于③式中的$\left(m_Q+m_h\right)\left(-g{\mathbf{e}}_3+{\ddot{\mathbf{x}}}_L\right)$，由于该向量方向由后括号决定，其方向经受力分析可得与${\mathbf{p}}_2$同向，则其与${\mathbf{p}}_2$叉乘结果为0。故可化简为(将$l_2$约去)：
$${\mathbf{p}}_2\times \left[m_Q\left(-l_1{\ddot{\mathbf{p}}}_1-l_2{\ddot{\mathbf{p}}}_2\right)+m_h\left(-l_2{\ddot{\mathbf{p}}}_2\right)\right]={\mathbf{p}}_2\times fR{\mathbf{e}}_3$$

3.进一步化简

在上一节得到了三个方程：
$$\begin{array}{rl}& ④fR{e}_3=\left(m_Q+m_h+m_L\right)\left(g{Q}_3+{\dot{\nu }}_L\right)-\left(m_Q+m_h\right)l_2{\ddot{\mathbf{p}}}_2-m_Q{l}_1{\ddot{\mathbf{p}}}_1\\ & ⑤{\mathbf{p}}_1\times \left[-m_{Q}g{\mathbf{e}}_3+m_Q\left({\ddot{\mathbf{x}}}_L-l_1{\ddot{\mathbf{p}}}_1-l_2{\ddot{\mathbf{p}}}_2\right)\right]={\mathbf{p}}_1\times fR{\mathbf{e}}_3\\ & ⑥{\mathbf{p}}_2\times \left[m_Q\left(-l_1{\ddot{\mathbf{p}}}_1-l_2{\ddot{\mathbf{p}}}_2\right)+m_h\left(-l_2{\ddot{\mathbf{p}}}_2\right)\right]={\mathbf{p}}_2\times fR{\mathbf{e}}_3\end{array}$$
现做进一步化简。
准备：
$$\begin{array}{rl}\because & {\dot{\mathbf{p}}}_1={\mathbf{\omega }}_1\times {\mathbf{p}}_1\\ \therefore & {\ddot{\mathbf{p}}}_1={\dot{\mathbf{\omega }}}_1\times {\mathbf{p}}_1+{\mathbf{\omega }}_1\times \left({\mathbf{\omega }}_1\times {\mathbf{p}}_1\right)={\dot{\mathbf{\omega }}}_1\times {\mathbf{p}}_1-{\mathbf{p}}_1\\ & {\ddot{\mathbf{p}}}_2={\dot{\mathbf{\omega }}}_2\times {\mathbf{p}}_2-{\mathbf{p}}_2\end{array}$$

关于⑤式

现对⑤式做如下处理：
$$\begin{array}{r}{\mathbf{p}}_1\times \left[-m_{Q}g{\mathbf{e}}_3+m_Q\left({\ddot{\mathbf{x}}}_L-l_1{\ddot{\mathbf{p}}}_1-l_2{\ddot{\mathbf{p}}}_2\right)\right]={\mathbf{p}}_1\times fR{\mathbf{e}}_3\\ {\mathbf{p}}_1\times \left[-m_Q{l}_1{\ddot{\mathbf{p}}}_1+m_Q\left(-g{\mathbf{e}}_3+{\ddot{\mathbf{x}}}_L\right)-\frac{m_Q{m}_h}{m_h+m_L}{l}_2{\ddot{\mathbf{p}}}_2-\frac{m_Q{m}_L}{m_h+m_L}{l}_2{\ddot{\mathbf{p}}}_2\right]={\mathbf{p}}_1\times fR{\mathbf{e}}_3\end{array}$$
其中
$$m_Q\left(-g{\mathbf{e}}_3+{\ddot{\mathbf{x}}}_L\right)-\frac{m_Q{m}_h}{m_h+m_L}{l}_2{\ddot{\mathbf{p}}}_2=\frac{m_Q}{m_h+m_L}\left[\left(m_h+m_L\right)\left(-g{\mathbf{e}}_3+{\ddot{\mathbf{x}}}_L\right)-m_h{l}_2{\ddot{\mathbf{p}}}_2\right]$$
式中中括号内容为挂钩及负载所受合外力，方向必与${\mathbf{p}}_1$同向，故在叉乘运算中化为0，⑤式变为：
$${\mathbf{p}}_1\times \left[-m_Q{l}_1{\ddot{\mathbf{p}}}_1-\frac{m_Q{m}_L}{m_h+m_L}{l}_2{\ddot{\mathbf{p}}}_2\right]={\mathbf{p}}_1\times fR{\mathbf{e}}_3$$
另外：
$$\begin{array}{rl}& {\mathbf{p}}_1\times {\ddot{\mathbf{p}}}_1={\mathbf{p}}_1\times [{\dot{\mathbf{\omega }}}_1\times {\mathbf{p}}_1-{\mathbf{p}}_1]={\dot{\mathbf{\omega }}}_1\\ & {\mathbf{p}}_1\times {\ddot{\mathbf{p}}}_2={\mathbf{p}}_1\times [{\dot{\mathbf{\omega }}}_2\times {\mathbf{p}}_2-{\mathbf{p}}_2]\end{array}$$
于是⑤式化为：
$$-m_Q{l}_1{\dot{\mathbf{\omega }}}_1-\frac{m_Q{m}_L}{m_h+m_L}{l}_2{\mathbf{p}}_1\times [{\dot{\mathbf{\omega }}}_2\times {\mathbf{p}}_2-{\mathbf{p}}_2]={\mathbf{p}}_1\times fR{\mathbf{e}}_3$$

关于⑥式

因为：
$$\begin{array}{rl}& {\mathbf{p}}_2\times {\ddot{\mathbf{p}}}_1={\mathbf{p}}_2\times [{\dot{\mathbf{\omega }}}_1\times {\mathbf{p}}_1-{\mathbf{p}}_1]\\ & {\mathbf{p}}_2\times {\ddot{\mathbf{p}}}_2={\mathbf{p}}_2\times [{\dot{\mathbf{\omega }}}_2\times {\mathbf{p}}_2-{\mathbf{p}}_2]={\dot{\mathbf{\omega }}}_2\end{array}$$
所以⑥式化为：

$$-m_Q{l}_1{\mathbf{p}}_2\times [{\dot{\mathbf{\omega }}}_1\times {\mathbf{p}}_1-{\mathbf{p}}_1]-\left(m_Q+m_h\right)l_2{\dot{\mathbf{\omega }}}_2={\mathbf{p}}_2\times fR{\mathbf{e}}_3$$

三、整理

将上述得到的式子整理，并加入无人机旋转动力学方程等：
$$\begin{array}{rl}\left(m_Q+m_h+m_L\right)\left(g{\mathbf{e}}_3+{\dot{\mathbf{\nu }}}_L\right)-\left(m_Q+m_h\right)l_2{\ddot{\mathbf{p}}}_2-m_Q{l}_1{\ddot{\mathbf{p}}}_1& =fR{e}_3\\ -m_Q{l}_1{\dot{\mathbf{\omega }}}_1-\frac{m_Q{m}_L}{m_h+m_L}{l}_2{\mathbf{p}}_1\times [{\dot{\mathbf{\omega }}}_2\times {\mathbf{p}}_2-{\mathbf{p}}_2]& ={\mathbf{p}}_1\times fR{\mathbf{e}}_3\\ -\left(m_Q+m_h\right)l_2{\dot{\mathbf{\omega }}}_2-m_Q{l}_1{\mathbf{p}}_2\times [{\dot{\mathbf{\omega }}}_1\times {\mathbf{p}}_1-{\mathbf{p}}_1]& ={\mathbf{p}}_2\times fR{\mathbf{e}}_3\\ J_Q\dot{\mathbf{\Omega }}+\mathbf{\Omega }\times J_Q\mathbf{\Omega }& =M\\ {\dot{\mathbf{x}}}_L& ={\dot{\mathbf{\nu }}}_L\\ \dot{R}& =R\hat{\Omega }\\ {\dot{\mathbf{p}}}_1& ={\mathbf{\omega }}_1\times {\mathbf{p}}_1\\ {\dot{\mathbf{p}}}_2& ={\mathbf{\omega }}_2\times {\mathbf{p}}_2\end{array}$$

总结

至此完成无人机二级摆系统几何建模，其优势在于形式简介，相较于传统引入四个摆角进行建模公式数量少且直观，随后可基于建模方程进行控制器设计。