CN102323758A - 六旋翼无人机分立输入解耦装置及包含该装置的控制系统 - Google Patents

Abstract

一种六旋翼无人机分立输入解耦装置及包含该装置的控制系统。本发明涉及一种六旋翼无人机分立输入解耦装置，该装置包括系数矩阵M_co存储模块；结构矩阵M_rel存储模块；将系数矩阵与结构矩阵相乘并存储为映射矩阵Map的模块；映射矩阵降维模块；降维齐次线性方程求解模块；基础解向量归一化模块；差分模块；放大增益模块；求和模块；非线性解算模块；本发明使全驱动六旋翼无人机这个多输入多输出系统解耦为六个单输入单输出系统，从而实现了无人机六个自由度独立控制，其最大优点是解耦之后的系统形式简单易于设计控制算法，由虚拟附加控制量可以快速地求取各个电机实际转速，算法对电机转速发生波动具有一定的鲁棒性。

Description

六旋翼无人机分立输入解耦装置及包含该装置的控制系统

技术领域

本发明属于多旋翼无人飞行器控制领域，涉及一种六旋翼无人机分立输入解耦装置及包含该装置的控制系统。

背景技术

六旋翼无人机区别于其他旋翼无人机如直升机、四旋翼无人机等，通过改变六个旋翼的组织形式，可使系统呈现欠驱动或全驱动的不同工作模式。相比于一般的多旋翼无人机需通过控制姿态角变化进而控制水平运动，全驱动六旋翼无人机机体设有六个连接杆，均匀分布在机体周围，每个连接杆的外端连接一个旋翼。六个旋翼的旋转平面与六个连接杆所在的平面形成六个倾角，每个旋翼的旋转平面与相隔一个旋翼的第三个旋翼的旋转平面倾角相等，每个旋翼与相邻旋翼的旋转平面与六个连接杆所在的平面倾角互为补角。该无人机利用改变各个旋翼转速的方法控制飞行器的六个自由度，具有高度的可控性、机动性、稳定性。可以实现快速前飞、悬停以及以任意姿态前飞等动作。但由无人机自身结构以及空气动力学因素决定，六旋翼无人机存在较强的输入耦合问题，即每一个自由度状态的变化都受到多个旋翼转速的影响，而任意一个旋翼转速的变化也会引起多个自由度状态的改变，采用多输入多输出的非线性控制器

控制复杂，难以实现。

发明内容

本发明要解决的一个技术问题是提供一种通过分立式解耦映射，使全驱动六旋翼无人机这个多输入多输出系统解耦为六个单输入单输出系统，从而实现无人机六个自由度独立控制的六旋翼无人机分立输入解耦装置。

为了解决上述技术问题，本发明的六旋翼无人机分立输入解耦装置包括：

系数矩阵M_co存储模块；

所述的系数矩阵M_co＝diag(k₁₁，k₂₂，....，k₆₆)；(1)

结构矩阵M_rel存储模块；

所述的结构矩阵M_rel＝{-1^kAIV_ij}_{6×6，k＝1，2}   (2)

式(2)中，i代表旋翼编号，j代表自由度编号；i＝1、2、3、……6，j＝1、2、3、4、5、6；AIV_ij为第i个旋翼动作时对第j个自由度的绝对影响值；

设六旋翼无人机机体坐标系的x、y轴位于各连接杆所在的平面内并相互垂直，z轴通过六旋翼无人机的质心且垂直于各连接杆所在的平面；沿x、y、z轴正向移动自由度为正，沿x、y、z轴反向移动自由度为负；俯仰角θ为无人机机体绕y轴转动的角度，逆时针转动自由度为正，顺指针转动自由度为负；滚转角φ为无人机机体绕x轴转动的角度，逆时针转动自由度为正，顺时针转动自由度为负；偏航角ψ为无人机机体绕z轴转动的角度，逆时针转动自由度为正，顺时针转动自由度为负；则

第i个旋翼动作时对x、y、z轴方向平动自由度的绝对影响值：

第i个旋翼动作时对俯仰角θ、滚转角φ、偏航角ψ转动自由度的绝对影响值：

当自由度向负方向变化时，k＝1，当自由度向正方向变化时k＝2；

将系数矩阵与结构矩阵相乘并存储为映射矩阵Map的模块；

映射矩阵降维模块：依次将映射矩阵Map中的某一行去掉，得到六个五行六列的行满秩矩阵Map′_j(j＝，1，2，3，4，5，6)；

降维齐次线性方程求解模块：求出六个降维齐次线性方程的基础解向量

所述降维齐次线性方程为：

${\mathrm{Map}}_j^{\′}{\left[\begin{array}{cccccc}{\mathrm{\Δ\Ω}}_{1j}^2& {\mathrm{\Δ\Ω}}_{2j}& {\mathrm{\Δ\Ω}}_{3j}^2& {\mathrm{\Δ\Ω}}_{4j}^2& {\mathrm{\Δ\Ω}}_{5j}^2& {\mathrm{\Δ\Ω}}_{6j}^2\end{array}\right]}^T=0---\left(11\right)$

其中 ${\mathrm{\Δ\Ω}}_{1j}^2,{\mathrm{\Δ\Ω}}_{2j}^2,{\mathrm{\Δ\Ω}}_{3j}^2,{\mathrm{\Δ\Ω}}_{4j}^2,{\mathrm{\Δ\Ω}}_{5j}^2,{\mathrm{\Δ\Ω}}_{6j}^2$ 分别为自由度虚拟控制量增量ΔU_j引起的各旋翼转速平方增量；

基础解向量归一化模块：

将基础解向量

进行归一化运算得到

然后将

与自由度虚拟控制量增量ΔU_j对应的映射矩阵的行a_j相乘得到

最后将

送给求和模块，将

送给放大增益模块；

差分模块：根据两个采样周期分别计算得到的自由度虚拟控制量

和计算自由度虚拟控制量增量ΔU_j，其中，t₂＝t₁+Δt；

放大增益模块：利用式(12)计算得到放大增益K_j，然后将K_j与相乘，并将相乘的结果送到求和模块；

${\mathrm{\ΔU}}_j=K_j{\left|\right|{\stackrel{r}{a}}_j\frac{{\stackrel{r}{\mathrm{\ξ}}}_j}{\left|{\stackrel{r}{\mathrm{\ξ}}}_j\right|}\left|\right|}_1---\left(12\right)$

求和模块：利用式(13)计算出各旋翼转速总平方增量；

非线性解算模块：利用式(14)得到各旋翼转速

${\mathrm{\Ω}}_i(t+\mathrm{\Δt})=\sqrt{{\mathrm{\Ω}}_i^2\left(t\right)+{{\mathrm{\Δ\Ω}}_i}^2}---\left(14\right)$

其中

为当前时刻第i个旋翼转速的平方，Ω_i(t+Δt)为下一时刻第i个旋翼的转速。

本发明利用赋予各个电机一个基础转速并构造虚拟附加控制量的方法实现输入解耦，使得每个等效虚拟附加控制量单独控制一个状态变量通道，而每个通道也只受到单个虚拟附加控制量的作用，将各个状态变量通道转化为单输入单输出的二阶系统。在此解耦条件下，对各个通道单独设计控制算法求取所需的虚拟附加控制量。最后通过虚拟附加控制量与电机转速平方的增量值之间的关系可以得到一组映射算法求取由各个虚拟附加控制量引起的电机转速平方的增量值，并将其逐个与基础电机转速平方值叠加最后通过开方运算计算实际所需的各个电机的转速。本发明使全驱动六旋翼无人机这个多输入多输出系统解耦为六个单输入单输出系统，从而实现了无人机六个自由度独立控制，其最大优点是解耦之后的系统形式简单易于设计控制算法，由虚拟附加控制量可以快速地求取各个电机实际转速，算法对电机转速发生波动具有一定的鲁棒性。

本发明要解决的另一个技术问题是提供一种包含上述六旋翼无人机分立输入解耦装置的六旋翼无人机控制系统。

为了解决上述技术问题，本发明的多旋翼无人机控制系统的x控制器、y控制器、z控制器、θ控制器、φ控制器、ψ控制器分别根据给定的x向位移、y向位移、z向位移、俯仰角θ、滚转角φ、偏航角ψ与对应的反馈信号的差值计算并输出各自由度虚拟控制量；分立输入解耦装置根据接收的各自由度虚拟控制量，通过非线性解耦映射得到各旋翼转速，然后将各旋翼转速传输到各旋翼对应的驱动装置。

附图说明

下面结合附图和具体实施方式对本发明作进一步详细说明。

图1为六旋翼无人机结构示意图。

图2为六旋翼无人机结构俯视图。

图3为本发明的六旋翼无人机分立输入解耦装置结构框图。

图4为本发明的六旋翼无人机控制系统原理图。

具体实施方式

如图1所示，全驱动六旋翼无人机机体设有六个连接杆，均匀分布在机体周围，六个连接杆的外端分别连接一号、二号、……、六号旋翼11、12、……、16，各旋翼可采用电机或油机驱动。六个旋翼的旋转平面与机体坐标系的xoy平面(六个连接杆所在的平面)形成六个倾角，每个旋翼的旋转平面与相隔两个旋翼的第三个旋翼的旋转平面倾角相等。每个旋翼与相邻旋翼的旋转平面与机体坐标系的xoy平面倾角互为补角(例如，一号旋翼11的旋转平面与机体坐标系的xoy平面之间的倾角为α，二号旋翼12的旋转平面与机体坐标系的xoy平面之间的倾角为β，则α与β互为补角)。

六个旋翼按编号逆时针排放，旋翼转轴与机体坐标系的xoy平面呈一定夹角0～90°。一号旋翼11、三号旋翼13、五号旋翼15逆时针旋转，二号旋翼2、四号旋翼4、六号旋翼6顺指针旋转。由旋翼空气动力学可知，各个旋翼产生的升力以及反扭力矩均与其转速的平方成正比，升力和反扭力矩分别等于 $F_i=k_1{\mathrm{\Ω}}_i^2(i=\mathrm{1,2},\·\·\·\·\·\·,6)$ 和 $M_{\mathrm{Di}}=k_2{\mathrm{\Ω}}_i^2(i=\mathrm{1,2},\·\·\·\·\·\·6),$ F_i是第i个旋翼产生的升力，M_Di是第i个旋翼产生的反扭力矩，k₁和k₂是旋翼的升力系数与反扭力矩系数，Ω_i表示第i个旋翼的旋转速度。各个旋翼对于无人机质心产生的升力矩为

d是旋翼转轴距无人机质心的距离。无人机的状态变量分为三个姿态角(φ，θ，ψ)和三个三维空间位置(x，y，z)，由Newton-Euler动力学方程可知无人机具有六个自由度共十二个状态。每一个自由度都受到多个旋翼产生的力和力矩的影响。

如图3所示，本发明的六旋翼无人机分立输入解耦装置包括：系数矩阵M_co存储模块101；结构矩阵M_rel存储模块102；将系数矩阵与结构矩阵相乘并存储为映射矩阵Map模块103；映射矩阵降维模块104；降维齐次线性方程求解模块105；基础解向量归一化模块106；差分模块107；放大增益模块108；求和模块109；非线性解算模块110。

六旋翼无人机输入解耦原理如图4所示。通过分立输入解耦，使得无人机每一个自由度只受到一个自由度虚拟控制量的影响。自由度虚拟控制量U_j(j＝1，2，3，4，5，6)与各旋翼力矩将满足关系式(6)。

$\left[\begin{array}{c}{U}_1\\ U_2\\ U_3\\ U_4\\ U_5\\ U_6\end{array}\right]=M_{\mathrm{rel}}\left[\begin{array}{c}{F}_1\\ F_2\\ F_3\\ M_1\\ M_2\\ M_3\end{array}\right]---\left(6\right)$

其中M_rel为结构矩阵，反映无人机旋翼在机体坐标系下的分布形式。当无人机结构确定时，结构矩阵为常矩阵。由旋翼动力学特性可知，旋翼在旋转过程中产生的力或力矩与旋翼转速成正比，等式两边取增量(6)式又可改写成(7)式。

$\left[\begin{array}{c}{\mathrm{\ΔU}}_1\\ {\mathrm{\ΔU}}_2\\ {\mathrm{\ΔU}}_3\\ {\mathrm{\ΔU}}_4\\ {\mathrm{\ΔU}}_5\\ {\mathrm{\ΔU}}_6\end{array}\right]=M_{\mathrm{co}}\·M_{\mathrm{rel}}\left[\begin{array}{c}{\mathrm{\Δ\Ω}}_1^2\\ {\mathrm{\Δ\Ω}}_2^2\\ {\mathrm{\Δ\Ω}}_3^2\\ {\mathrm{\Δ\Ω}}_4^2\\ {\mathrm{\Δ\Ω}}_5^2\\ {\mathrm{\Δ\Ω}}_6^5\end{array}\right]---\left(7\right)$

其中M_co为系数矩阵，与旋翼特性有关，也为常矩阵。Map＝M_co·M_rel为映射矩阵。

系数矩阵M_co的确定：

M_co＝diag(k₁₁，k₂₂，....，k₆₆)；(1)

其中k₁₁，k₂₂，....，k₆₆根据系统参数和电机-旋翼系统的动力学特性确定。一般地，当k₁₁，k₂₂，....，k₆₆的取值范围在0.000001到0.1之间时，无人机可实现稳定飞行。

结构矩阵M_rel的确定：

M_rel＝{-1^kAIV_ij}_{6×6，k＝1，2}                    (2)

式(2)中，i代表旋翼编号，j代表自由度编号；i＝1、2、3、……6，j＝1、2、3、4、5、6；AIV_ij为第i个旋翼动作时对第j个自由度的绝对影响值；

M_rel是系统的六阶结构矩阵，结构矩阵根据无人机不同的机体坐标系的建立方法以及旋翼在所建立的机体坐标系下的分布有不同的形式，根据六旋翼无人机旋翼的组织形式，矩阵中的元素的取值范围为[-1，1]。

结构矩阵M_rel中元素的具体确定方法：

第一、建立六旋翼无人机机体坐标，如图1、2所示：

以六旋翼无人机的质心作为坐标原点o，一号旋翼11所在的方向为y轴正向，以y轴正向在六个连接杆所在的平面内逆时针旋转90°所指的方向为x轴正向，以垂直于六个连接杆所在的平面并且向上的方向为z轴正向。沿x、y、z轴正向移动自由度为正，沿x、y、z轴反向移动自由度为负；俯仰角θ为无人机绕y轴转动的角度，规定沿y轴负方向看逆时针转动为俯仰角正方向；滚转角φ为无人机绕x轴转动的角度，规定沿x轴负方向看逆时针转动为滚转角正方向；偏航角ψ为无人机绕z轴转动的角度，规定沿z轴负方向逆时针转动为偏航角正方向。

第二，根据无人机旋翼在机体坐标系下的分布形式，找出针对某自由度的最大影响旋翼；对某一自由度的最大影响旋翼可根据此旋翼单独作用下对本自由度影响强弱来确定。对于全驱动六旋翼无人机，针对x、z轴平动自由度的最大影响旋翼组包括所有旋翼；针对y轴平动自由度的最大影响旋翼组包括二号、三号、五号、六号旋翼12、13、15、16；针对俯仰角θ转动自由度的最大影响旋翼组包括二号、三号、五号、六号旋翼12、13、15、16；针对滚转角φ其最大影响旋翼为一号和四号旋翼11、14；偏航角ψ转动自由度的最大影响旋翼组包括所有旋翼。

第三，计算旋翼绝对影响值AIV(Absolute Influence Value)，标定各个旋翼动作时对本自由度的影响强弱。在最大影响旋翼组中找出最大影响旋翼，对最大影响旋翼赋最大绝对值1，对于没有选入最大影响旋翼组的旋翼赋零值，对于其他旋翼的赋值采用如下两个公式计算

第i个旋翼动作时对x、y、z轴方向平动自由度的绝对影响值：

第i个旋翼动作时对俯仰角θ、滚转角φ、偏航角ψ转动自由度的绝对影响值：

这里单位转速通常取1000rpm。

根据不同旋翼对自由度作用的不同确定旋翼绝对影响值AIV的正负号。对某一自由度的最大影响旋翼可根据此旋翼单独作用下对本自由度影响强弱来确定。而符号通过此旋翼动作时会产生相应自由度的变化方向来确定。当自由度向负方向变化时，k＝1，当自由度向正方向变化时k＝2。

以六个连接杆均匀分布在机体周围为例，通过计算可以得到：

$M_{\mathrm{rel}}=\left[\begin{array}{cccccc}-\frac{l_{12}}{l}& -\frac{l_{12}}{l}& 1& -\frac{l_{12}}{l}& -\frac{l_{12}}{l}& 1\\ -1& 1& 0& -1& 1& 0\\ 1& 1& 1& 1& 1& 1\\ \frac{l_{12}}{l_{11}}& -\frac{l_{12}}{l_{11}}& -1& -\frac{l_{12}}{l_{11}}& \frac{l_{12}}{l_{11}}& 1\\ 1& 1& 0& -1& -1& 0\\ 1& -1& 1& -1& 1& -1\end{array}\right]=\left[\begin{array}{cccccc}-0.5& -0.5& 1& -0.5& -0.5& 1\\ -1& 1& 0& -1& 1& 0\\ 1& 1& 1& 1& 1& 1\\ 0.5& -0.5& -1& -0.5& 0.5& 1\\ 1& 1& 0& -1& -1& 0\\ 1& -1& 1& -1& 1& -1\end{array}\right]---\left(5\right)$

其中l₁₁为一号旋翼11转轴到x轴的垂直距离，l₁₂为二号、六号旋翼12、16转轴到x轴的垂直距离，1为无人机杆长，如图2所示。

系数矩阵M_co与结构矩阵M_rel相乘得到映射矩阵Map＝M_co·M_rel；

降维映射矩阵Map′的确定：

第一、设某时刻由自由度虚拟控制量增量所得的某旋翼转速总平方增量为

(i＝1，2，3，4，5，6)，其中，i代表旋翼编号。某一路自由度虚拟控制量增量ΔU_j对应的转速平方增量为

(i，j＝1，2，3，4，5，6)。其中，i代表旋翼编号，j代表自由度虚拟控制量编号。则某旋翼转速总平方增量满足关系式(8)

${\mathrm{\Δ\Ω}}_i^2=\underset{j=\mathrm{1,2,3,4,5,6}}{\mathrm{\Σ}}{\mathrm{\Δ\Ω}}_{\mathrm{ij}}^2(i=\mathrm{1,2,3,4,5,6})---\left(6\right)$

第二、要保证输入解耦需满足：某自由度虚拟控制量增量ΔU_j对应的旋翼转速增量

并不引起其他虚拟控制量的变化。此关系可以表示为：

当各旋翼转速增量为

(i＝1，2，3，4，5，6)时，

ΔU_j≠0                      (7)

ΔU_v＝0(v≠j)                (8)

当六旋翼无人机的结构确定时，无人机的结构矩阵是常矩阵，由此可以得到自由度虚拟控制量与六个旋翼的转速是线性关系，满足叠加原理。本发明的核心思想是利用叠加原理，找到当虚拟控制量变化时相应各旋翼转速的增量，满足关系式(9)、(10)。即每个自由度虚拟控制量对应且仅对应一路自由度输出。这里的有效自由度虚拟控制量是指该自由度虚拟控制量对应结构矩阵的行为非零行。

将Map矩阵中与不为零的自由度虚拟控制量增量ΔU_j对应行去除，即得到六个五行六列的行满秩降维映射矩阵Map′_j(j＝，1，2，3，4，5，6)。当自由度虚拟控制量增量ΔU₁不为零时，ΔU₂、ΔU₃……ΔU₆为零，Map矩阵第一行去除，得到五行六列的行满秩降维映射矩阵Map′₁；当自由度虚拟控制量增量ΔU₂不为零时，ΔU₁、ΔU₃……ΔU₆为零，Map矩阵第二行去除，得到五行六列的行满秩降维映射矩阵Map′₂，……，当自由度虚拟控制量增量ΔU₆不为零时，ΔU₁、ΔU₂……ΔU₅为零，Map矩阵第六行去除，得到五行六列的行满秩降维映射矩阵Map′₆。

降维齐次线性方程的求解：

根据降维矩阵Map′_j(j＝，1，2，3，4，5，6)得到六个齐次线性方程

${\mathrm{Map}}_j^{\′}{\left[\begin{array}{cccccc}{\mathrm{\Δ\Ω}}_{1j}^2& {\mathrm{\Δ\Ω}}_{2j}^2& {\mathrm{\Δ\Ω}}_{3j}^2& {\mathrm{\Δ\Ω}}_{4j}^2& {\mathrm{\Δ\Ω}}_{5j}^2& {\mathrm{\Δ\Ω}}_{6j}^2\end{array}\right]}^T=0---\left(11\right)$

由于(11)式中矩阵Map′_j(j＝，1，2，3，4，5，6)的秩R(A)＝5，解向量维数为6，则(11)式的基础解系只有一个向量，设基础解向量为

基础解向量归一化模块106：将(11)式的基础解向量

归一化得到然后将

与自由度虚拟控制量增量ΔU_j对应的映射矩阵的行

相乘，最后将相乘的结果

送到放大增益模块108，将

送给求和模块109。

自由度虚拟控制量增量ΔU_j的计算：

如图4所示，六旋翼无人机控制系统的x控制器、y控制器、z控制器、θ控制器、φ控制器、ψ控制器分别根据给定的x向位移、y向位移、z向位移、俯仰角θ、滚转角φ、偏航角ψ与对应的反馈信号的差值e₁、e₂、e₃、e₄、e₅、e₆计算并输出各自由度虚拟控制量U_j，差分模块107根据两个采样周期分别采集的自由度虚拟控制量

和

计算自由度虚拟控制量增量ΔU_j。

放大增益为K_j的确定：

设放大增益为K_j(K_j用来调节自由度虚拟控制量增量ΔU_j的调节灵敏度)，自由度虚拟控制量增量ΔU_j对应映射矩阵的行为

则

放大增益模块108可利用式(12)计算得到K_j

${\mathrm{\ΔU}}_j=K_j{\left|\right|{\stackrel{r}{a}}_j\frac{{\stackrel{r}{\mathrm{\ξ}}}_j}{\left|{\stackrel{r}{\mathrm{\ξ}}}_j\right|}\left|\right|}_1---\left(12\right)$

根据叠加原理，求和模块109可以利用式(13)计算出各旋翼转速总平方增量：

非线性解算因子δ为对向量元素分别进行同一种非线性运算，输出仍为一向量。这里δ采用了对向量各元素取算术平方根运算。

非线性解算模块110利用式(14)得到各旋翼转速

${\mathrm{\Ω}}_i=(t+\mathrm{\Δt})=\mathrm{\δ}({\mathrm{\Ω}}_i^2\left(t\right)+{{\mathrm{\Δ\Ω}}_i}^2)=\sqrt{{\mathrm{\Ω}}_i^2\left(t\right)+{{\mathrm{\Δ\Ω}}_i}^2}---\left(14\right)$

其中为当前时刻第i个旋翼转速的平方，Ω_i(t+Δt)为下一时刻第i个旋翼的转速。

本发明采用下述方法实现分立输入解耦：

首先，将六个旋翼转速平方增量ΔΩ_i(i＝1，2，3，4，5，6)分为六组，每一组都仅对应于一个虚拟控制量的变化。

第二，分别计算旋翼在各虚拟控制量下的转速平方增量

对于图1所示的六旋翼无人机，其系数矩阵和结构矩阵都是方阵且满秩。为了保证某自由度虚拟控制量增量ΔU_j对应的旋翼转速增量ΔΩ_ij并不引起其他虚拟控制量的变化。这里采用了矩阵降维处理方法。

先将不为零虚拟控制量增量ΔU_j对应的映射矩阵的行去掉，得到六个五行六列的行满秩降维矩阵Map′_j(j＝，1，2，3，4，5，6)，解齐次线性方程组。

${\mathrm{Map}}_j^{\′}{\left[\begin{array}{cccccc}{\mathrm{\Δ\Ω}}_{1j}^2& {\mathrm{\Δ\Ω}}_{2j}^2& {\mathrm{\Δ\Ω}}_{3j}^2& {\mathrm{\Δ\Ω}}_{4j}^2& {\mathrm{\Δ\Ω}}_{5j}^2& {\mathrm{\Δ\Ω}}_{6j}^2\end{array}\right]}^T=0---\left(11\right)$

由于(11)式中降维矩阵Map′_j(j＝，1，2，3，4，5，6)的秩R(A)＝5，解向量维数为6，则(11)式的基础解系只有一个向量，设基础解向量为

将其归一化，设放大增益为K_j(K_j用来调节虚拟控制量调节灵敏度)，设自由度虚拟控制量增量为ΔU_j，自由度虚拟控制量增量ΔU_j对应映射矩阵的行为a_j，则控制量增量ΔU_j可表示为

${\mathrm{\ΔU}}_j=K_j{\left|\right|{\stackrel{r}{a}}_j\frac{{\stackrel{r}{\mathrm{\ξ}}}_j}{\left|{\stackrel{r}{\mathrm{\ξ}}}_j\right|}\left|\right|}_1---\left(12\right)$

第三、根据叠加原理，得到各旋翼转速总平方增量

按照第二步根据六个自由度虚拟控制量增量分别解出放大增益K_j，进而得到各旋翼转速增量，根据叠加原理，将其线性相加如下式所示。

下面以六个连接杆均匀分布在机体周围的全驱动六旋翼无人机为例详细说明。

图1所示全驱动六旋翼无人机，设系数矩阵为I，结构矩阵为

$M_{\mathrm{rel}}=\left[\begin{array}{cccccc}-0.5& -0.5& 1& -0.5& -0.5& 1\\ -1& 1& 0& -1& 1& 0\\ 1& 1& 1& 1& 1& 1\\ 0.5& -0.5& -1& -0.5& 0.5& 1\\ 1& 1& 0& -1& -1& 0\\ 1& -1& 1& -1& 1& -1\end{array}\right]---\left(15\right)$

这里只说明水平位移x和俯仰角θ两个自由度，其他自由度通道计算方法类似。

1.水平位移x控制

这里考虑的是单独x坐标变化其他输出不变的情况。由于水平位移x只与U1有关，所以这里需要使U₁变化，而使其他五个控制量不变，即对于(6)式应有

${\mathrm{Map}}_1^{\′}\left[\begin{array}{c}{\mathrm{\Δ\Ω}}_{11}^2\\ {\mathrm{\Δ\Ω}}_{21}^2\\ {\mathrm{\Δ\Ω}}_{31}^2\\ {\mathrm{\Δ\Ω}}_{41}^2\\ {\mathrm{\Δ\Ω}}_{51}^2\\ {\mathrm{\Δ\Ω}}_{61}^2\end{array}\right]=\left[\begin{array}{cccccc}-0.5& -0.5& 1& -0.5& -0.5& 1\\ -1& 1& 0& -1& 1& 0\\ 1& 1& 1& 1& 1& 1\\ 0.5& -0.5& -1& -0.5& 0.5& 1\\ 1& 1& 0& -1& -1& 0\\ 1& -1& 1& -1& 1& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\Δ\Ω}}_{11}^2\\ {\mathrm{\Δ\Ω}}_{21}^2\\ {\mathrm{\Δ\Ω}}_{31}^2\\ {\mathrm{\Δ\Ω}}_{41}^2\\ {\mathrm{\Δ\Ω}}_{51}^2\\ {\mathrm{\Δ\Ω}}_{61}^2\end{array}\right]=0---\left(16\right)$

由式(9)、(10)可得如下关系

${\mathrm{\Δ\Ω}}_{11}^2={\mathrm{\Δ\Ω}}_{21}^2={\mathrm{\Δ\Ω}}_{41}^2={\mathrm{\Δ\Ω}}_{51}^2=-\frac{1}{2}{\mathrm{\Δ\Ω}}_{31}^2=-\frac{1}{2}{\mathrm{\Δ\Ω}}_{61}^2---\left(17\right)$

可得基础解向量为

则自由度虚拟控制量增量

${\mathrm{\ΔU}}_1=K_1{\left|\right|{\stackrel{r}{a}}_1\frac{{\stackrel{r}{\mathrm{\ξ}}}_1}{\left|{\stackrel{r}{\mathrm{\ξ}}}_1\right|}\left|\right|}_1---\left(18\right)$

容易验证ΔU₂＝ΔU₃＝ΔU₄＝ΔU₅＝ΔU₆＝0。

2.俯仰角θ控制

俯仰角θ变化只与U₄有关系所以这里需要使U₄变化，而使其他五个自由度虚拟控制量不变，即对于(6)式应有

${\mathrm{Map}}_4^{\′}\left[\begin{array}{c}{\mathrm{\Δ\Ω}}_{14}^2\\ {\mathrm{\Δ\Ω}}_{24}^2\\ {\mathrm{\Δ\Ω}}_{34}^2\\ {\mathrm{\Δ\Ω}}_{44}^2\\ {\mathrm{\Δ\Ω}}_{54}^2\\ {\mathrm{\Δ\Ω}}_{64}^2\end{array}\right]=\left[\begin{array}{cccccc}-0.5& -0.5& 1& -0.5& -0.5& 1\\ -1& 1& 0& -1& 1& 0\\ 1& 1& 1& 1& 1& 1\\ 0.5& -0.5& -1& -0.5& 0.5& 1\\ 1& 1& 0& -1& -1& 0\\ 1& -1& 1& -1& 1& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\Δ\Ω}}_{14}^2\\ {\mathrm{\Δ\Ω}}_{24}^2\\ {\mathrm{\Δ\Ω}}_{34}^2\\ {\mathrm{\Δ\Ω}}_{44}^2\\ {\mathrm{\Δ\Ω}}_{54}^2\\ {\mathrm{\Δ\Ω}}_{64}^2\end{array}\right]=0---\left(19\right)$

由(15)式可得如下关系

${\mathrm{\Δ\Ω}}_{14}^2={-\mathrm{\Δ\Ω}}_{24}^2=-\frac{1}{2}{\mathrm{\Δ\Ω}}_{34}^2={-\mathrm{\Δ\Ω}}_{44}^2={\mathrm{\Δ\Ω}}_{54}^2=\frac{1}{2}{\mathrm{\Δ\Ω}}_{64}^2---\left(20\right)$

基础解向量为

则自由度虚拟控制量增量控制量

${\mathrm{\ΔU}}_4=K_4{\left|\right|{\stackrel{r}{a}}_4\frac{{\stackrel{r}{\mathrm{\ξ}}}_4}{\left|{\stackrel{r}{\mathrm{\ξ}}}_4\right|}\left|\right|}_1---\left(14\right)$

容易验证ΔU₁＝ΔU₂＝ΔU₃＝ΔU₅＝ΔU₆＝0。

本发明不仅适用于六个连接杆均匀分布于机体周围的六旋翼无人机，也适用于六个连接杆不均匀分布的情况，因而上述实施例并不能构成对本发明保护范围的限制。

Claims (2)

1.一种六旋翼无人机分立输入解耦装置，其特征在于包括：

系数矩阵M_co存储模块(101)；

所述的系数矩阵M_co＝diag(k₁₁，k₂₂，....，k₆₆)；(1)

结构矩阵M_rel存储模块(102)；

所述的结构矩阵M_rel＝{-1^kAIV_ij}_{6×6，k＝1，2}   (2)

式(2)中，i代表旋翼编号，j代表自由度编号；i＝1、2、3、……6，j＝1、2、3、4、5、6；AIV_ij为第i个旋翼动作时对第j个自由度的绝对影响值；

设六旋翼无人机机体坐标系的x、y轴位于各连接杆所在的平面内并相互垂直，z轴通过六旋翼无人机的质心且垂直于各连接杆所在的平面；沿x、y、z轴正向移动自由度为正，沿x、y、z轴反向移动自由度为负；俯仰角θ为无人机机体绕y轴转动的角度，逆时针转动自由度为正，顺指针转动自由度为负；滚转角φ为无人机机体绕x轴转动的角度，逆时针转动自由度为正，顺时针转动自由度为负；偏航角ψ为无人机机体绕z轴转动的角度，逆时针转动自由度为正，顺时针转动自由度为负；则

第i个旋翼动作时对x、y、z轴方向平动自由度的绝对影响值：

第i个旋翼动作时对俯仰角θ、滚转角φ、偏航角ψ转动自由度的绝对影响值：

当自由度向负方向变化时，k＝1，当自由度向正方向变化时k＝2；

将系数矩阵与结构矩阵相乘并存储为映射矩阵Map的模块(103)；

映射矩阵降维模块(104)：依次将映射矩阵Map中的某一行去掉，得到六个五行六列的行满秩矩阵Map′_j(j＝，1，2，3，4，5，6)；

降维齐次线性方程求解模块(105)：求出六个降维齐次线性方程的基础解向量

所述降维齐次线性方程为：

${\mathrm{Map}}_j^{\′}{\left[\begin{array}{cccccc}{\mathrm{\Δ\Ω}}_{1j}^2& {\mathrm{\Δ\Ω}}_{2j}& {\mathrm{\Δ\Ω}}_{3j}^2& {\mathrm{\Δ\Ω}}_{4j}^2& {\mathrm{\Δ\Ω}}_{5j}^2& {\mathrm{\Δ\Ω}}_{6j}^2\end{array}\right]}^T=0---\left(11\right)$

其中 ${\mathrm{\Δ\Ω}}_{1j}^2,{\mathrm{\Δ\Ω}}_{2j}^2,{\mathrm{\Δ\Ω}}_{3j}^2,{\mathrm{\Δ\Ω}}_{4j}^2,{\mathrm{\Δ\Ω}}_{5j}^2,{\mathrm{\Δ\Ω}}_{6j}^2$ 分别为自由度虚拟控制量增量ΔU_j引起的各旋翼转速平方增量；

基础解向量归一化模块(106)：

将基础解向量

进行归一化运算得到

然后将

与自由度虚拟控制量增量ΔU_j对应的映射矩阵的行a_j相乘得到最后将

送给求和模块(109)，将

送给放大增益模块(108)；

差分模块(107)：根据两个采样周期分别计算得到的自由度虚拟控制量

和

计算自由度虚拟控制量增量ΔU_j，其中，t₂＝t₁+Δt；

放大增益模块(108)：利用式(12)计算得到放大增益K_j，然后将K_j与

相乘，并将相乘的结果送到求和模块(109)；

${\mathrm{\ΔU}}_j=K_j{\left|\right|{\stackrel{r}{a}}_j\frac{{\stackrel{r}{\mathrm{\ξ}}}_j}{\left|{\stackrel{r}{\mathrm{\ξ}}}_j\right|}\left|\right|}_1---\left(12\right)$

求和模块(109)：利用式(13)计算出各旋翼转速总平方增量；

非线性解算模块(110)：利用式(14)得到各旋翼转速

${\mathrm{\Ω}}_i(t+\mathrm{\Δt})=\sqrt{{\mathrm{\Ω}}_i^2\left(t\right)+{{\mathrm{\Δ\Ω}}_i}^2}---\left(14\right)$

其中

为当前时刻第i个旋翼转速的平方，Ω_i(t+Δt)为下一时刻第i个旋翼的转速。

2.一种包含如权利要求1所述的六旋翼无人机分立输入解耦装置的控制系统，其特征在于x控制器、y控制器、z控制器、θ控制器、φ控制器、ψ控制器分别根据给定的x向位移、y向位移、z向位移、俯仰角θ、滚转角φ、偏航角ψ与对应的反馈信号的差值计算并输出各自由度虚拟控制量；分立输入解耦装置根据接收的各自由度虚拟控制量，通过非线性解耦映射得到各旋翼转速，然后将各旋翼转速传输到各旋翼对应的驱动装置。

Images