具体实施方式
下面将结合本发明实施例中附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此,以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围,而是仅仅表示本发明的选定实施例。基于本发明的实施例,本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例,都属于本发明保护的范围。
应注意到:相似的标号和字母在下面的附图中表示类似项,因此,一旦某一项在一个附图中被定义,则在随后的附图中不需要对其进行进一步定义和解释。同时,在本发明的描述中,术语“第一”、“第二”等仅用于区分描述,而不能理解为指示或暗示相对重要性。
请参照图1,图1示出了本发明实施例提供的多旋翼飞行器100的方框示意图。多旋翼飞行器100可以是,但不限于四旋翼飞行器、六旋翼飞行器、八旋翼飞行器等等。所述多旋翼飞行器100包括控制分配装置200、存储器110、存储控制器120及处理器130。
所述存储器110、存储控制器120及处理器130各元件相互之间直接或间接地电性连接,以实现数据的传输或交互。例如,这些元件相互之间可通过一条或多条通讯总线或信号线实现电性连接。所述控制分配装置200包括至少一个可以软件或固件(firmware)的形式存储于所述存储器110中或固化在所多旋翼飞行器100的操作***(operating system,OS)中的软件功能模块。所述处理器130用于执行存储器110中存储的可执行模块,例如所述控制分配装置200包括的软件功能模块或计算机程序。
其中,存储器110可以是,但不限于,随机存取存储器(Random Access Memory,RAM),只读存储器(Read Only Memory,ROM),可编程只读存储器(Programmable Read-OnlyMemory,PROM),可擦除只读存储器(Erasable Programmable Read-Only Memory,EPROM),电可擦除只读存储器(Electric Erasable Programmable Read-Only Memory,EEPROM)等。其中,存储器110用于存储程序,所述处理器130在接收到执行指令后,执行所述程序。
处理器130可以是一种集成电路芯片,具有信号处理能力。上述的处理器130可以是通用处理器,包括中央处理器(Central Processing Unit,CPU)、网络处理器(NetworkProcessor,NP)、语音处理器以及视频处理器等;还可以是数字信号处理器、专用集成电路、现场可编程门阵列或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器130也可以是任何常规的处理器等。
所述多旋翼飞行器100还包括多个动力***,动力***的数量与该多旋翼飞行器100的旋翼数量对应,例如,四旋翼飞行器包括四个动力***,六旋翼飞行器包括六个动力***等。
第一实施例
请参照图2,图2示出了本发明实施例提供的控制分配方法流程图。控制分配方法包括以下步骤:
步骤S101,获取多个状态控制误差,其中,每个状态控制误差分别与一个方向对应,多个方向包括滚转方向、俯仰方向、偏航方向及高度方向。
在本发明实施例中,获取滚转方向、俯仰方向、偏航方向及高度方向各自对应的状态控制误差的方法可以是:首先,处理器130根据相应的姿态角和高度指令,计算得到期望的滚转欧拉角、俯仰欧拉角、偏航欧拉角和高度值;然后,将期望的滚转欧拉角、俯仰欧拉角、偏航欧拉角和高度值与多旋翼飞行器100实际的滚转欧拉角、俯仰欧拉角、偏航欧拉角和高度值进行相减,得到滚转方向、俯仰方向、偏航方向及高度方向中每个方向对应的状态控制误差,实际的滚转欧拉角、俯仰欧拉角、偏航欧拉角是由姿态传感器获取的,实际的高度值是由高度传感器获取的。
步骤S102,利用反馈控制器对每个方向的状态控制误差进行计算,得到每个方向的归一化控制量。
在本发明实施例中,得到滚转方向、俯仰方向、偏航方向及高度方向四个方向的状态控制误差之后,将该四个方向的状态控制误差输入反馈控制器,利用反馈控制器分别对每个方向的状态控制误差进行计算,得到每个方向的归一化控制输出,其过程可以包括:
首先,将每个方向的状态控制误差输入反馈控制器,得到每个方向的初始控制量,可以用以下公式实现:
roll_err=[rollControler](roll_cmd-roll_measure)
pitch_err=[pitchControler](pitch_cmd-pitch_measure)
yaw_err=[yawControler](yaw_cmd-yaw_measure)
height_err=[heightControler](height_cmd-height_measure)
其中,roll为滚转方向,pitch为俯仰方向,yaw为偏航方向,height为高度方向,_cmd为各个方向的期望值,_measure为各个方向的实际测量值,[rollControler]、[pitchControler]、[yawControler]和[heightControler]均为反馈控制器,_err为反馈控制器输出的各个方向的初始控制量。
其次,根据反馈控制器的预设输出阈值,对初始控制量进行限幅,得到每个方向的限幅控制量,可以用以下公式实现:
其中,_min,_max为反馈控制器在各个方向上的预设输出阈值,滚转、俯仰和偏航方向为对称限幅,范围为±_max,而高度方向为正向,范围为_min~_max,_err_sigmoid为各个方向限幅之后反馈控制器输出的限幅控制量。
最后,根据预设输出阈值,对限幅控制量进行归一化,得到每个方向的归一化控制量,可以用以下公式实现:
roll_output=roll_err_sigmoid/roll_max
pitch_output=pitch_err_sigmoid/pitch_max
yaw_output=yaw_err_sigmoid/yaw_max
height_output=(height_err_sigmoid-height_min)/(height_max-height_min)
其中,_output为各个方向归一化后反馈控制器输出的归一化控制量,滚转、俯仰和偏航方向的范围为±1.0,高度方向的范围为0~1.0。
在本发明实施例中,反馈控制器可以是,但不限于PID控制器、BackStep控制器、或者动态逆控制器,反馈控制器有一个输入和多个输出,也就是说,滚转、俯仰、偏航和高度方向的状态控制误差输入值反馈控制器之后,反馈控制器经过上述的一系列运算之后,可以输出滚转、俯仰、偏航和高度四个方向的归一化控制量。
请参照图3,步骤S102可以包括以下子步骤:
子步骤S1021,将每个方向的状态控制误差输入反馈控制器,得到每个方向的初始控制量。
在本发明实施例中,将滚转、俯仰、偏航和高度四个方向的状态控制误差输入反馈控制器之后,同时输出四个方向的初始控制量。
子步骤S1022,根据反馈控制器的预设输出阈值,对初始控制量进行限幅,得到每个方向的限幅控制量。
在本发明实施例中,根据反馈控制器对各个方向的预设输出阈值,对滚转、俯仰、偏航和高度四个方向的初始控制量进行限幅,滚转、俯仰和偏航方向为对称限幅,范围为±_max,而高度方向为正向,范围为_min~_max,同时输出四个方向的限幅控制量。
子步骤S1023,根据预设输出阈值,对限幅控制量进行归一化,得到每个方向的归一化控制量。
在本发明实施例中,滚转、俯仰和偏航方向的归一化控制量的范围为±1.0,高度方向的归一化控制量的范围为0~1.0。
步骤S103,根据动力***的数量和位置关系,基于多个归一化控制量,建立分配矩阵。
在本发明实施例中,得到各个方向的归一化控制量之后,根据动力***的数量和位置关系,基于各个方向的归一化控制量建立分配矩阵,可以用以下公式实现:
其中,D1~D8均为动力***,如果多旋翼飞行器100的动力***的数量向小于8个,则缺失的动力***在计算分配矩阵时设置为0,该动力***在分配矩阵中的所有项也均为0。
步骤S104,按照多个方向的预设顺序,利用分配矩阵将对应方向的归一化控制量进行分配,得到每个动力***的最终控制量。
在本发明实施例中,建立出分配矩阵之后,按照滚转、俯仰、偏航和高度方向的顺序,利用分配矩阵将对应方向的归一化控制量进行分配,得到每个动力***的最终控制量,其过程可以包括:
首先,根据分配矩阵,将滚转方向和俯仰方向的归一化控制量进行分配,得到每个动力***的第一控制量,可以用以下公式实现:
其中,Di_rp为每个动力***的第一控制量。
其次,根据分配矩阵,将高度方向的归一化控制量进行分配,得到每个动力***的高度控制量,可以用以下公式实现:
其中,Di_h为每个动力***的高度控制量,接着根据第一控制量和高度控制量,得到每个动力***的第二控制量,具体过程可以包括:首先,将第一控制量和高度控制量作差,得到每个动力***的控制量差值;然后,对所有控制量差值求平均,得到控制量平均值;最后,将所述控制量平均值叠加至每个动力***的第一控制量,得到每个控制***的第二控制量。
第三,根据分配矩阵,将偏航方向的归一化控制量进行分配,得到每个动力***的偏航控制量,可以用以下公式实现:
其中,Di_y为每个动力***的偏航控制量,接着将第二控制量和偏航控制量进行叠加,得到每个动力***的第三控制量,可以用以下公式实现:
其中,Di_rpyh为每个动力***的第三控制量。
最后,将第三控制量与预设控制量D_max进行对比,得到缩减系数D_reduce,预设控制量也就是预设的动力***的最大输出,如果所有动力***的第三控制量均小于或等于预设控制量,则D_reduce为1,将每个动力***的第三控制量均设置为最终控制量;如果任意一个或多个动力***的第三控制量大于预设控制量,则设置D_reduce为预设控制量与所有第三控制量中的最大值的商,并设置所有第三控制量中的最大值为预设控制量,同时利用该D_reduce对其他的第三控制量进行缩减,得到最终控制量。
请参照图3,步骤S104可以包括以下子步骤:
子步骤S1041,根据分配矩阵,将滚转方向和俯仰方向的归一化控制量进行分配,得到每个动力***的第一控制量。
子步骤S1042,根据分配矩阵,将高度方向的归一化控制量进行分配,得到每个动力***的高度控制量。
子步骤S1043,根据第一控制量和高度控制量,得到每个动力***的第二控制量。
在本发明实施例中,得到每个***的第一控制量Di_rp和高度控制量Di_h之后,首先,将第一控制量Di_rp和高度控制量Di_h作差,得到每个动力***的控制量差值,可以用以下公式实现:
其中,Di_delta为每个动力***的控制量差值;然后对所有控制量差值求平均,得到控制量平均值,即最后,将所述控制量平均值叠加至每个动力***的第一控制量,得到每个控制***的第二控制量,可以用下式实现:
其中,Di_rph为每个动力***的第二控制量。
子步骤S1044,根据分配矩阵,将偏航方向的归一化控制量进行分配,得到每个动力***的偏航控制量。
子步骤S1045,将第二控制量和偏航控制量进行叠加,得到每个动力***的第三控制量。
子步骤S1046,将第三控制量与预设控制量进行对比,得到缩减系数。
在本发明实施例中,预设控制量是用户预先设置的动力***最大输出,计算缩减系数的过程可以包括:判断所有动力***的第三控制量是否均小于或等于预设控制量,若是,则设置缩减系数为1;若否,则设置缩减系数为预设控制量与所有第三控制量中的最大值的商。
子步骤S1047,利用缩减系数对第三控制量进行缩减,得到每个动力***的最终控制量。
在本发明实施例中,如果缩减系数为1,则每个动力***的第三控制量就为最终控制量;如果缩减系数不为1,则设置所有第三控制量中的最大值为预设控制量,并利用该缩减系数对其他的第三控制量进行缩减,得到最终控制量,同时,可以通过部分减小高度方向的控制量输出,来保持另外三个方向的控制量输出精度。
在本发明实施例中,假设有一架六旋翼飞行器,该六旋翼飞行器悬停所需动力***输出为0.93左右。由于六旋翼飞行器由六个动力***组成,因此分配矩阵中与动力***D7~D8对应的所有项均为零,相对于六旋翼飞行器重心,该六个动力***从俯视视角来看分别位于右前、最前、左、左后、右后、右六个方向,相对于重心的距离归一化为1。考虑到各个动力***的滚转方向和俯仰方向的力臂、产生力矩的符号,且D1动力***转动方向为顺时针,相邻动力***转动方向相反,则该六旋翼飞行器的分配矩阵为:
利用该分配矩阵将滚转、俯仰、偏航和高度方向的归一化控制量进行分配,得到每个动力***的最终控制量,请结合参照图5~图10,对比图5和图6可知,使用缩减系数能够有效的避免动力***输出超过预设控制量,根据图7至图10可知,由于缩减系数的采用,动力***输出饱和情况下不影响姿态控制精度,只产生轻微的高度波动,能够有效保持该六旋翼飞行器的飞行姿态稳定。
本发明实施例提供的控制分配方法,与现有技术相比具有以下优势:
第一、该控制分配方法输出精度高、计算量小,同时充分利用了多旋翼飞行器100动力***的能力;
第二、动力***饱和情况下,可以使用输出缩减方法放弃部分高度控制回路的控制权限,对保持多旋翼飞行器100姿态稳定、提高多旋翼飞行器100的安全性和扩展其飞行包线具有积极意义。
第二实施例
请参照图11,图11示出了本发明实施例提供的控制分配装置200的方框示意图。控制分配装置200包括状态控制误差获取模块210、归一化控制量获得模块220、分配矩阵建立模块230及最终控制量获得模块240。
状态控制误差获取模块210,用于获取多个状态控制误差,其中,每个状态控制误差分别与一个方向对应,多个方向包括滚转方向、俯仰方向、偏航方向及高度方向。
在本发明实施例中,状态控制误差获取模块210可以用于执行步骤S101。
归一化控制量获得模块220,用于利用反馈控制器对每个方向的状态控制误差进行计算,得到每个方向的归一化控制量。
在本发明实施例中,归一化控制量获得模块220可以用于执行步骤S102及子步骤S1021~子步骤S1023。
分配矩阵建立模块230,用于根据动力***的数量和位置关系,基于多个归一化控制量,建立分配矩阵。
在本发明实施例中,分配矩阵建立模块230可以用于执行步骤S103。
最终控制量获得模块240,用于按照多个方向的预设顺序,利用分配矩阵将对应方向的归一化控制量进行分配,得到每个动力***的最终控制量。
在本发明实施例中,最终控制量获得模块240可以用于执行步骤S104及子步骤S1041~子步骤S1047。
综上所述,本发明提供的一种控制分配方法、装置及多旋翼飞行器,所述方法包括获取多个状态控制误差,其中,每个状态控制误差分别与一个方向对应,多个方向包括滚转方向、俯仰方向、偏航方向及高度方向;利用反馈控制器对每个方向的状态控制误差进行计算,得到每个方向的归一化控制量;根据动力***的数量和位置关系,基于多个归一化控制量,建立分配矩阵;按照多个方向的预设顺序,利用分配矩阵将对应方向的归一化控制量进行分配,得到每个动力***的最终控制量。本发明实施例提供的控制分配方法,与现有技术相比具有以下优势:第一、该控制分配方法输出精度高、计算量小,同时充分利用了多旋翼飞行器100动力***的能力;第二、动力***饱和情况下,可以使用输出缩减方法放弃部分高度控制回路的控制权限,对保持多旋翼飞行器100姿态稳定、提高多旋翼飞行器100的安全性和扩展其飞行包线具有积极意义。
在本申请所提供的几个实施例中,应该理解到,所揭露的装置和方法,也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的,例如,附图中的流程图和框图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上,流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分,所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意,在有些作为替换的实现方式中,方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如,两个连续的方框实际上可以基本并行地执行,它们有时也可以按相反的顺序执行,这依所涉及的功能而定。也要注意的是,框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合,可以用执行规定的功能或动作的专用的基于硬件的***来实现,或者可以用专用硬件与计算机指令的组合来实现。
另外,在本发明各个实施例中的各功能模块可以集成在一起形成一个独立的部分,也可以是各个模块单独存在,也可以两个或两个以上模块集成形成一个独立的部分。
所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时,可以存储在一个计算机可读取存储介质中。基于这样的理解,本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来,该计算机软件产品存储在一个存储介质中,包括若干指令用以使得一台计算机设备(可以是个人计算机,服务器,或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括:U盘、移动硬盘、只读存储器(ROM,Read-Only Memory)、随机存取存储器(RAM,Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。需要说明的是,在本文中,诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来,而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且,术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下,由语句“包括一个……”限定的要素,并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
以上所述仅为本发明的优选实施例而已,并不用于限制本发明,对于本领域的技术人员来说,本发明可以有各种更改和变化。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。应注意到:相似的标号和字母在下面的附图中表示类似项,因此,一旦某一项在一个附图中被定义,则在随后的附图中不需要对其进行进一步定义和解释。