CN102360217B - 多旋翼无人机全局输入解耦装置及包含该装置的控制系统 - Google Patents

Abstract

本发明涉及一种多旋翼无人机全局输入解耦装置，该装置包括系数矩阵M_co存储模块；结构矩阵M_rel存储模块；将系数矩阵M_co与结构矩阵M_rel相乘并求逆得到逆矩阵(M_co·M_rel)^-1的模块；主导旋翼转速运算模块；本发明通过非线性解耦映射实现了模型未知情况下的多旋翼无人机输入解耦，使多旋翼无人机这个多输入多输出系统解耦为若干个单输入单输出子系统，且每一个自由度只受到一个子系统回路的影响。从而实现了自由度(位置及姿态)的独立控制，降低了系统控制的复杂度，系统控制实现简单、不基于对象的数学模型、适用范围广、适合于任意结构的多旋翼无人机。

Description

多旋翼无人机全局输入解耦装置及包含该装置的控制系统

技术领域

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

背景技术

六旋翼无人机区别于其他旋翼无人机如直升机、四旋翼无人机等，通过改变六个旋翼的组织形式，可使系统呈现欠驱动或全驱动的不同工作模式。相比于一般的多旋翼无人机需通过控制姿态角变化进而控制水平运动，全驱动的六旋翼无人机采用独特的结构配置，可以直接控制水平运动，该无人机机动性强，可以实现快速前飞、悬停以及以任意姿态前飞等特殊功能。而欠驱动六旋翼无人机由于其控制相对简单，系统存在的扰动和不确定性较小，因此也具有研究价值。全驱动六旋翼无人机(结构任意)机体一般设有六个连接杆，均匀分布在机体周围，连接杆的外端连接执行机构，执行机构由旋翼及驱动器组成。驱动器可采用电机或油机等。欠驱动六旋翼无人机(结构任意)机体一般设有三个连接杆，均匀分布在机体周围，每个连接杆的外端连接上下两个旋翼。目前的多旋翼无人机控制系统在解耦问题上没有通用的方法，每一个状态量的变化都受到多个旋翼转速的影响，而任一旋翼转速改变后也会使多个状态量发生变化，控制复杂，不容易实现各个自由度的动态解耦，从而实现各个自由度(位置及姿态)的独立控制。

发明内容

本发明要解决的一个技术问题是提供一种通过非线性解耦映射，使多旋翼无人机这个多输入多输出系统解耦为若干个单输入单输出系统，从而实现无人机六个自由度(位置及姿态)独立控制的多旋翼无人机全局输入解耦装置。

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

系数矩阵M_co存储模块101；

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

结构矩阵M_rel存储模块102；

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

式(2)中i＝1、2、3、……m，j＝1、2、3、4、5、6，m为旋翼的数量；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；

将系数矩阵M_co与结构矩阵M_rel相乘并求逆得到逆矩阵(M_co·M_rel)^-1的模块103；

主导旋翼转速运算模块104：利用式(5)得到各主导旋翼转速，并根据其他旋翼与主导旋翼的对应关系得到其他旋翼转速，然后将得到的各旋翼转速分别传输给各旋翼对应的驱动装置的模块：

$\left[\begin{array}{c}{\mathrm{\Ω}}_1\\ {\mathrm{\Ω}}_2\\ ...\\ {\mathrm{\Ω}}_q\end{array}\right]=\sqrt{{\left(M_{\mathrm{co}}{M}_{\mathrm{rel}}\right)}^{-1}\left[\begin{array}{c}{U}_1\\ U_2\\ ...\\ U_p\end{array}\right]}---\left(5\right)$

式(5)中U₁、U₂、……、U_p为有效自由度虚拟控制量，Ω₁、Ω₂、……、Ω_q为各主导旋翼的转速。

本发明通过非线性解耦映射实现了模型未知情况下的多旋翼无人机输入解耦，使多旋翼无人机这个多输入多输出系统解耦为若干个单输入单输出子系统，且每一个自由度只受到一个子系统回路的影响。从而实现了自由度(位置及姿态)的独立控制，降低了系统控制的复杂度。

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

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

附图说明

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

图1本发明的多旋翼无人机全局输入解耦装置结构框图。

图2本发明的多旋翼无人机控制系统结构框图。

图3全驱动六旋翼无人机结构示意图。

图4全驱动六旋翼无人机俯视图。

图5欠驱动六旋翼无人机结构示意图。

具体实施方式

如图1所示，本发明的多旋翼无人机全局输入解耦装置包括：

系数矩阵M_co存储模块101；

结构矩阵M_rel存储模块102；

将系数矩阵M_co与结构矩阵M_rel相乘并求逆得到逆矩阵(M_co·M_rel)^-1的模块103；

利用式(5)得到各主导旋翼转速，并根据其他旋翼与主导旋翼的对应关系得到其他旋翼转速，然后将得到的各旋翼转速分别传输给各旋翼对应的驱动装置的模块104；

$\left[\begin{array}{c}{\mathrm{\Ω}}_1\\ {\mathrm{\Ω}}_2\\ ...\\ {\mathrm{\Ω}}_q\end{array}\right]=\sqrt{{\left(M_{\mathrm{co}}{M}_{\mathrm{rel}}\right)}^{-1}\left[\begin{array}{c}{U}_1\\ U_2\\ ...\\ U_p\end{array}\right]}---\left(5\right)$

式(5)中U₁、U₂、……、U_p(p≤6)为有效自由度虚拟控制量，Ω₁、Ω₂、……、Ω_q(q≤6)为各主导旋翼的转速。

如图2所示，本发明的多旋翼无人机控制系统的x控制器、y控制器、z控制器、θ控制器、φ控制器、ψ控制器分别根据给定的x向位移、y向位移、z向位移、俯仰角θ、滚转角φ、偏航角ψ与对应的反馈信号的差值e₁、e₂、e₃、e₄、e₅、e₆计算并输出各自由度虚拟控制量U₁、U₂、……；全局输入解耦装置根据接收的各自由度虚拟控制量U₁、U₂、……，通过非线性解耦映射得到各旋翼转速Ω₁、Ω₂、……，然后将各旋翼转速传输到各旋翼对应的驱动装置。

所述的全局输入解耦装置通过数字信号处理器编程实现。

本发明通过非线性解耦映射改变各旋翼转速，实现了每一个自由度只存在于一个子系统回路，即只受一个自由度虚拟控制量的影响。实现了在不系统模型未知的情况下自由度(位置及姿态)的独立控制，降低了系统控制的复杂度。本发明不基于对象的数学模型、适用范围广、适合于任意结构的多旋翼无人机。

实施例1

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

六旋翼无人机输入解耦原理如图2所示。通过非线性解耦映射实现飞行器输入解耦，使得每一个自由度只受到一个自由度虚拟控制量的影响。自由度虚拟控制量与旋翼转速间存在如下关系：

$\left[\begin{array}{c}{U}_{11}\\ U_{12}\\ U_{13}\\ U_{14}\\ U_{15}\\ U_{16}\end{array}\right]=M_{\mathrm{co}\_1}\·M_{\mathrm{rel}\_1}\left[\begin{array}{c}{\mathrm{\Ω}}_{11}^2\\ {\mathrm{\Ω}}_{12}^2\\ {\mathrm{\Ω}}_{13}^2\\ {\mathrm{\Ω}}_{14}^2\\ {\mathrm{\Ω}}_{15}^2\\ {\mathrm{\Ω}}_{16}^2\end{array}\right]---\left(11\right)$

M_{co_1}是全驱动六旋翼无人机控制系统的系数矩阵，M_{rel_1}是全驱动六旋翼无人机控制系统的结构矩阵。

1.系数矩阵M_co-1的确定

M_{co_1}＝diag(k_11-1，k_22-1，k_33-1，k_44-1，k_55-1，k_66-1)是全驱动六旋翼无人机控制系统的系数矩阵，k_11-1、k_22-1、k_33-1、k_44-1、k_55-1和k_66-1根据系统参数和电机-旋翼系统的动力学特性确定。一般地，当k_11-1、k_22-1、k_33-1、k_44-1、k_55-1和k_66-1的取值范围在0.000001到0.1之间时，无人机可实现稳定飞行。

2.结构矩阵M_rel-1的确定：

M_{rel_1}＝{-1^kAIV_ij-1}_{6×6，k＝1，2}    (12)

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

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

结构矩阵M_{rel_1}中元素的具体确定方法：

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

以六旋翼无人机的质心作为坐标原点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个旋翼动作时对俯仰角θ、滚转角φ、偏航角ψ转动自由度的绝对影响值：

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

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

通过计算可以得到：

$M_{\mathrm{rel}}=M_{\mathrm{rel}-1}=\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(13\right)$

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

对于全驱动六旋翼无人机，结构矩阵是满秩的，因此容易根据式(15)由自由度虚拟控制量解算出六个转速。

$\left[\begin{array}{c}{\mathrm{\Ω}}_{11}\\ {\mathrm{\Ω}}_{12}\\ ...\\ {\mathrm{\Ω}}_{16}\end{array}\right]=\sqrt{{\left(M_{\mathrm{co}\_1}{M}_{\mathrm{rel}\_1}\right)}^{-1}\left[\begin{array}{c}{U}_{11}\\ U_{12}\\ ...\\ U_{16}\end{array}\right]}---\left(15\right)$

式(15)中U₁₁、U₁₂、……、U₁₆为各自由度虚拟控制量，Ω₁₁、Ω₁₂、……、Ω₁₆为各旋翼的转速。

对于全驱动六旋翼无人机来说，U₁₁、U₁₂、……、U₁₆全部为有效自由度虚拟控制量，各旋翼全部为主导旋翼。

实施例2

如图5所示，欠驱动六旋翼无人机机体设有三个连接杆，均匀分布在机体周围，每个连接杆的外端连接上下两个旋翼，一号、二号旋翼21、22连接在同一个连接杆的外端，三号、四号旋翼23、24连接在同一个连接杆的外端，五号、六号旋翼25、26连接在同一个连接杆的外端。各旋翼可采用电机或油机驱动。

欠驱动六旋翼无人机自由度虚拟控制量与旋翼转速间的具有如下关系：

$\left[\begin{array}{c}{U}_{21}\\ U_{22}\\ U_{23}\\ U_{24}\\ U_{25}\\ U_{26}\end{array}\right]=M_{\mathrm{co}\_2}\·M_{\mathrm{rel}\_2}\left[\begin{array}{c}{\mathrm{\Ω}}_{21}^2\\ {\mathrm{\Ω}}_{22}^2\\ {\mathrm{\Ω}}_{23}^2\\ {\mathrm{\Ω}}_{24}^2\\ {\mathrm{\Ω}}_{25}^2\\ {\mathrm{\Ω}}_{26}^2\end{array}\right]---\left(21\right)$

M_{co_2}是欠驱动六旋翼无人机控制系统的系数矩阵，M_{rel_2}是欠驱动六旋翼无人机控制系统的结构矩阵。

2.系数矩阵M_{co_2}的确定

M_co-2＝diag(k_11-2，k_22-2，k_33-2，k_44-2，k_55-2，k_66-2)是系统的系数矩阵，k_11-2、k_22-2、k_33-2、k_44-2、k_55-2和k_66-2根据系统参数和电机-旋翼系统的动力学特性确定。一般地，当k_11-2、k_22-2、k_33-2、k_44-2、k_55-2和k_66-2的取值范围在0.000001到0.1之间时，无人机可实现稳定飞行。

2.结构矩阵M_{rel_2}的确定

M_{rel_2}＝{-1^kAIV_ij-2}_{6×6，k＝1，2}    (22)

式(2)中i＝1、2、3、……6，j＝1、2、3、4、5、6；AIV_ij-2为旋翼绝对影响值，即第i个旋翼动作时对第j个自由度的绝对影响值；当自由度向负方向变化时，k＝1，当自由度向正方向变化时k＝2.

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

结构矩阵M_{rel_2}中元素的具体确定方法：

第一、建立六旋翼无人机体坐标：

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

第二，根据无人机旋翼在机体坐标系下的分布形式，找出针对某自由度的最大影响旋翼组；对某一自由度的最大影响旋翼可根据此旋翼单独作用下对本自由度影响强弱来确定。对于欠驱动六旋翼无人机，针对x轴、y轴平动自由度没有最大影响旋翼，即AIV全部为0；针对z轴平动自由度最大影响旋翼组包括全部旋翼，即AIV全部为1；针对俯仰角θ转动自由度的最大影响旋翼组包括一号、二号旋翼21、22；针对滚转角φ转动自由度的最大影响旋翼组包括二号、四号、五号、六号旋翼23、24、25、26，针对偏航角ψ转动自由度的最大影响旋翼组包括全部旋翼。

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

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

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

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

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

下面以俯仰角θ自由度的旋翼绝对影响值为例进行说明：

从图5所示结构可以看出，六个旋翼对俯仰都有影响。因此全部入选俯仰自由度的最大影响旋翼组。

由于l₂₁＞l₂₂因此容易得出六个旋翼对俯仰自由度的影响顺序如下

ω₁＝ω₂＞ω₃＝ω₄＝ω₅＝ω₆    (25)

其中l₂₁为一号旋翼21的转轴到y轴的垂直距离，l₂₂为三号旋翼23的转轴到y轴的垂直距离。一号旋翼21和二号旋翼22均为最大影响旋翼，赋值1。现在计算三号旋翼23、四号旋翼24、五号旋翼25和六号旋翼26号的旋翼绝对影响值：

${\mathrm{AIV}}_{34}={\mathrm{AIV}}_{44}={\mathrm{AIV}}_{54}={\mathrm{AIV}}_{64}=\frac{l_2}{l_1}---\left(26\right)$

其中l₁为一号旋翼21转轴到无人机质心的垂直距离，l₂为三号旋翼23转轴到y轴的垂直距离；

确定作用符号：

根据右手定则，可以得到一号旋翼21、二号旋翼22的旋翼绝对影响值为正，k＝2；三号、四号、五号、六号旋翼23、24、25、26的旋翼绝对影响值为负，k＝1。

按照同俯仰角θ自由度一样的方法，确定其余五个自由度AIV的值，得到结构矩阵M_rel-2为：

$M_{\mathrm{rel}-2}=\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 1& 1& 1& 1& 1& 1\\ 1& 1& -\frac{l_2}{l_1}& -\frac{l_2}{l_1}& -\frac{l_2}{l_1}& -\frac{l_2}{l_1}\\ 0& 0& -1& -1& 1& 1\\ 1& -1& 1& -1& 1& -1\end{array}\right]---\left(27\right)$

欠驱动六旋翼无人机的水平位移x，y是没有自由度虚拟控制量与之对应的，结构矩阵的秩为4，这里将三号、四号旋翼-电机系统及五号、六号旋翼-电机系统分为两组，得到重构结构矩阵如下

$M_{\mathrm{rel}-2}^{\′}=\left[\begin{array}{cccc}\frac{1}{2}& \frac{1}{2}& 1& 1\\ \frac{l_1}{\max \{l_1,2l_2\}}& \frac{l_1}{\max \{l_1,2l_2\}}& -\frac{2l_2}{\max \{l_1,2l_2\}}& -\frac{2l_2}{\max \{l_1,2l_2\}}\\ 0& 0& -1& 1\\ 1& -1& 0& 0\end{array}\right]---\left(28\right)$

合并的旋翼是线性相关的，对其参数矩阵相应元素线性叠加，得到降维的参数矩阵：

M′_co-2＝diag(k_11-2，k_22-2，k_33-2+k_44-2，k_55-2+k_66-2)    (29)

通过非线性解算因子，将自由度虚拟控制量映射成主导旋翼转速。

$\left[\begin{array}{c}{\mathrm{\Ω}}_{21}\\ {\mathrm{\Ω}}_{22}\\ {\mathrm{\Ω}}_{23}\\ {\mathrm{\Ω}}_{25}\end{array}\right]=\sqrt{{\left(M_{\mathrm{co}-2}^{\′}{M}_{\mathrm{rel}-2}^{\′}\right)}^{-1}\left[\begin{array}{c}{U}_{23}\\ U_{24}\\ U_{25}\\ U_{26}\end{array}\right]}---\left(30\right)$

式(30)中U₂₃、U₂₄、U₂₅、U₂₆为有效自由度虚拟控制量，Ω₂₁、Ω₂₂、Ω₂₃、Ω₂₅为主导旋翼的转速。

Ω₂₄＝Ω₂₃，Ω₂₆＝Ω₂₅。

从上例可看出，欠驱动六旋翼无人机在计算结构矩阵时需要进行输入线性相关项的合并和对结构矩阵的降阶，而这些问题并不存在于全驱动六旋翼无人机，全驱动六旋翼无人机的结构矩阵是满秩的，可以直接求逆。

旋翼无人机的执行机构由旋翼-电机系统组成，每个旋翼-电机系统产生的升力以及反扭力矩均与旋翼转速的平方成正比，升力和反扭力矩分别等于和

F_i是第i个旋翼产生的升力，M_Di是第i个旋翼产生的反扭力矩，k₁和k₂是旋翼的升力系数与反扭力矩系数，Ω_i表示第i个旋翼的旋转速度。六旋翼无人机由六个旋翼-电机系统组成，具有输出六路独立控制量的能力，根据旋翼-电机系统的不同组织形式，系统将体现出不同特性，即可能出现欠驱动或全驱动两种形式，图3中无人机属于全驱动形式无人机，图5中的六旋翼无人机则属于欠驱动形式无人机。本发明提供一种全局输入解耦装置，解决六旋翼无人机在欠驱动和全驱动两种模式下的输入解耦，把六输入六输出的系统解耦为若干子系统，且子系统的个数与驱动的个数相等。

本发明可以适用于任意结构的多旋翼无人机，上述实施例仅仅是为了对本发明作出详细说明给出的具体实施方式，不能理解为对本发明保护范围的限制，因而根据本发明权利要求1技术方案作出的任何简单变形，都在本发明意图保护范围之内。

Claims (4)

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

系数矩阵M_co存储模块（101）；

所述的系数矩阵M_co=diag(k₁₁,k₂₂，....，k_mm)；（1）

结构矩阵M_rel存储模块（102）；

所述的结构矩阵M_rel＝{-1^kAIV_ij}_m×6,k＝1,2（2）

式（2）中i=1、2、3、……m，j=1、2、3、4、5、6，m为旋翼的数量；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；

将系数矩阵M_co与结构矩阵M_rel相乘并求逆得到逆矩阵（M_co·M_rel）^-1的模块103；

主导旋翼转速运算模块（104）：利用式（5）得到各主导旋翼转速，并根据其他旋翼与主导旋翼的对应关系得到其他旋翼转速，然后将得到的各旋翼转速分别传输给各旋翼对应的驱动装置：

$\left[\begin{array}{c}{\mathrm{\Ω}}_1\\ {\mathrm{\Ω}}_2\\ ...\\ {\mathrm{\Ω}}_q\end{array}\right]=\sqrt{{\left(M_{\mathrm{co}}{M}_{\mathrm{rel}}\right)}^{-1}\left[\begin{array}{c}{U}_1\\ U_2\\ ...\\ U_p\end{array}\right]}---\left(5\right)$

式（5）中U₁、U₂、……、U_p为有效自由度虚拟控制量，Ω₁、Ω₂、……、Ω_q为各主导旋翼的转速。

2.根据权利要求1所述的多旋翼无人机全局输入解耦装置，其特征在于所述的多旋翼无人机为全驱动六旋翼无人机，其机体坐标以六旋翼无人机的质心作为坐标原点o,一号旋翼（11）所在的方向为y轴正向，以y轴正向在六个连接杆所在的平面内逆时针旋转90°所指的方向为x轴正向，以垂直于六个连接杆所在的平面并且向上的方向为z轴正向；沿x、y、z轴正向移动自由度为正，沿x、y、z轴反向移动自由度为负；俯仰角θ为无人机绕y轴转动的角度，规定沿y轴负方向看逆时针转动为俯仰角正方向；滚转角φ为无人机绕x轴转动的角度，规定沿x轴负方向看逆时针转动为滚转角正方向；偏航角ψ为无人机绕z轴转动的角度，规定沿z轴负方向逆时针转动为偏航角正方向；

所述系数矩阵M_co=M_co-1

M_co-1=diag(k_11-1，k_22-1，k_33-1，k_44-1，k_55-1，k_66-1)

所述结构矩阵M_rel=M_rel-1

$M_{\mathrm{rel}-1}=\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(13\right)$

其中l₁₁为一号旋翼（11）转轴到x轴的垂直距离，l₁₂为二号、六号旋翼（12、16）转轴到x轴的垂直距离，l为无人机杆长；

所述各旋翼转速利用式（15）计算得到：

$\left[\begin{array}{c}{\mathrm{\Ω}}_{11}\\ {\mathrm{\Ω}}_{12}\\ ...\\ {\mathrm{\Ω}}_{16}\end{array}\right]=\sqrt{{\left(M_{\mathrm{co}\_1}{M}_{\mathrm{rel}\_1}\right)}^{-1}\left[\begin{array}{c}{U}_{11}\\ U_{12}\\ ...\\ U_{16}\end{array}\right]}---\left(15\right)$

式（15）中U₁₁、U₁₂、……、U₁₆为各自由度虚拟控制量，Ω₁₁、Ω₁₂、……、Ω₁₆为各旋翼的转速。

3.根据权利要求1所述的多旋翼无人机全局输入解耦装置，其特征在于所述的多旋翼无人机为全欠驱动六旋翼无人机，其机体坐标以无人机的质心作为坐标原点o,一号旋翼(21)所在的方向为x轴正向，以x轴正向在三个连接杆所在的平面内顺时针旋转90°所指的方向为y轴正向，以垂直于三个连接杆所在的平面并且向上的方向为z轴正向；沿x、y、z轴正向移动自由度为正，沿x、y、z轴反向移动为负；俯仰角θ为无人机绕y轴转动的角度，规定沿y轴逆时针转动为俯仰角正方向；滚转角φ为无人机绕x轴转动的角度，规定沿x轴逆时针转动为滚转角正方向；偏航角ψ为无人机绕z轴转动的角度，规定沿z轴逆时针转动为正方向;

所述的系数矩阵

$M_{\mathrm{co}-2}^{\′}=\mathrm{diag}(k_{11-2},k_{22-2},k_{33-2}+k_{44-2},k_{55-2}+k_{66-2})$

所述的结构矩阵

$M_{\mathrm{rel}-2}^{\′}=\left[\begin{array}{cccc}\frac{1}{2}& \frac{1}{2}& 1& 1\\ \frac{l_1}{\max \{l_1,2l_2\}}& \frac{l_1}{\max \{l_1,2l_2\}}& -\frac{{2l}_2}{\max \{l_1,2l_2\}}& -\frac{{2l}_2}{\max \{l_1,2l_2\}}\\ 0& 0& -1& 1\\ 1& -1& 0& 0\end{array}\right]$

其中l₁为一号旋翼21转轴到无人机质心的垂直距离，l₂为三号旋翼23转轴到y轴的垂直距离；

各旋翼转速利用式（30）计算得到：

$\left[\begin{array}{c}{\mathrm{\Ω}}_{21}\\ {\mathrm{\Ω}}_{22}\\ {\mathrm{\Ω}}_{\mathrm{23,24}}\\ {\mathrm{\Ω}}_{\mathrm{25,26}}\end{array}\right]=\sqrt{{\left(M_{\mathrm{co}-2}^{\′}{M}_{\mathrm{rel}-2}^{\′}\right)}^{-1}\left[\begin{array}{c}{U}_{23}\\ U_{24}\\ U_{25}\\ U_{26}\end{array}\right]}---\left(30\right)$

式（30）中U₂₃、U₂₄、U₂₅、U₂₆为有效自由度虚拟控制量，Ω₂₁、Ω₂₂、Ω₂₃、Ω₂₅为主导旋翼的转速；

Ω₂₄=Ω₂₃，Ω₂₆=Ω₂₅。

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

Images