一种无人机重心偏移的补偿方法及系统与流程

本发明涉及无人机技术领域，尤其涉及一种无人机重心偏移的补偿方法及系统。

背景技术：

现有技术中，带云台的无人航拍飞行器都会存在飞行器重心与飞行器机体的几何中心不一致的情况，由此产生的重心偏移会导致控制的效果不够理想，尤其在手动模式下，无人机明显会往重心偏移的方向漂移，大大影响了用户尤其是新手的飞行体验和可操作性。因此，有必要针对无人机重心偏移的问题对其进行准确辨识并补偿控制。

技术实现要素：

本发明针对现有技术中存在的问题，提供了一种无人机重心偏移的补偿方法及系统，能够对无人机进行重心偏移补偿控制，有效提升无人机的动态性能和抗干扰能力，进而提升用户的飞行体验。

本发明就上述技术问题而提出的技术方案如下：

一方面，本发明提供一种无人机重心偏移的补偿方法，包括：

获取无人机的目标控制力矩；

获取所述无人机由重心偏移得到的附加力矩；

采用递推最小二乘法辨识出所述无人机所需的补偿力矩；

将所述附加力矩和所述补偿力矩添加到目标控制力矩中进行补偿控制；

将补偿后的力矩分配给四个电机，以使所述四个电机驱动所述无人机达到目标姿态。

进一步地，所述获取无人机的目标控制力矩，具体包括：

根据所述无人机的目标姿态角以及由所述无人机当前姿态解算得到的姿态角，计算获得姿态角误差；

根据所述姿态角误差获得目标角速度，并根据所述目标角速度以及由所述无人机当前姿态解算得到的角速度，计算获得姿态角速度误差；

根据所述姿态角速度误差获得所述目标控制力矩。

进一步地，所述采用递推最小二乘法辨识出所述无人机所需的补偿力矩，具体包括：

基于俯仰角方向的总力矩、俯仰角速度、滚转角加速度和偏航角速度，采用所述递推最小二乘法辨识出所述无人机在所述俯仰角方向上所需的补偿力矩；

基于滚转角方向的总力矩、滚转角速度、俯仰角加速度和偏航角速度，采用所述递推最小二乘法辨识出所述无人机在所述滚转角方向上所需的补偿力矩；

基于偏航角方向的总力矩、偏航角加速度、滚转角速度和俯仰角速度，采用所述递推最小二乘法辨识出所述无人机在所述偏航角方向上所需的补偿力矩。

进一步地，所述基于俯仰角方向的总力矩、俯仰角速度、滚转角加速度和偏航角速度，采用所述递推最小二乘法辨识出所述无人机在所述俯仰角方向上所需的补偿力矩，具体包括：

获取所述无人机俯仰角方向的力矩平衡方程其中，mx为无人机俯仰角方向的总力矩，为滚转角加速度，为俯仰角速度，为偏航角速度，ixx、iyy、izz为无人机的转动惯量，为俯仰角方向的补偿力矩；

设线性方程组ax＝b，则a(k,:)x＝bk,其中，k＝1,2,…n，a(k,:)＝[ak1,ak2,…,akn]，x＝[x1,x2,…,xn]^t；

令b＝mx，采用递推最小二乘法辨识出所述无人机在所述俯仰角方向上所需的补偿力矩

所述递推最小二乘法为：

x^k+1＝x^k+q^k[bk-a(k,:)x^k]，k＝1,2,…,n；

p^k+1＝[i-q^ka(k,:)]p^k；

其中，随机产生初始权值向量x⁰＝rand(n,1)，设p⁰＝αi∈r^n×n，α＝10⁶～10¹⁰，i∈r^n×n是单位矩阵。

进一步地，所述将补偿后的力矩分配给四个电机，以使所述四个电机驱动所述无人机达到目标姿态，具体包括：

基于所述无人机的重量、转动惯量以及电机到质量中心的臂长，依次将所述俯仰角方向的补偿力矩、所述滚转角方向的补偿力矩、所述偏航角方向的补偿力矩分配给所述四个电机，以调整所述四个电机的转速，使所述四个电机驱动所述无人机达到目标姿态。

另一方面，本发明提供一种无人机重心偏移的补偿系统，包括：

第一获取模块，用于获取无人机的目标控制力矩；

第二获取模块，用于获取所述无人机由重心偏移得到的附加力矩；

辨识模块，用于采用递推最小二乘法辨识出所述无人机所需的补偿力矩；

补偿模块，用于将所述附加力矩和所述补偿力矩添加到目标控制力矩中进行补偿控制；以及，

分配模块，用于将补偿后的力矩分配给四个电机，以使所述四个电机驱动所述无人机达到目标姿态。

进一步地，所述第一获取模块具体包括：

姿态角误差计算单元，用于根据所述无人机的目标姿态角以及由所述无人机当前姿态解算得到的姿态角，计算获得姿态角误差；

姿态角速度误差计算单元，用于根据所述姿态角误差获得目标角速度，并根据所述目标角速度以及由所述无人机当前姿态解算得到的角速度，计算获得姿态角速度误差；以及，

获取单元，用于根据所述姿态角速度误差获得所述目标控制力矩。

进一步地，所述辨识模块具体包括：

第一辨识单元，用于基于俯仰角方向的总力矩、俯仰角速度、滚转角加速度和偏航角速度，采用所述递推最小二乘法辨识出所述无人机在所述俯仰角方向上所需的补偿力矩；

第二辨识单元，用于基于滚转角方向的总力矩、滚转角速度、俯仰角加速度和偏航角速度，采用所述递推最小二乘法辨识出所述无人机在所述滚转角方向上所需的补偿力矩；以及，

第三辨识单元，用于基于偏航角方向的总力矩、偏航角加速度、滚转角速度和俯仰角速度，采用所述递推最小二乘法辨识出所述无人机在所述偏航角方向上所需的补偿力矩。

进一步地，所述第一辨识单元具体包括：

方程获取子单元，用于获取所述无人机俯仰角方向的力矩平衡方程其中，mx为无人机俯仰角方向的总力矩，为滚转角加速度，为俯仰角速度，为偏航角速度，ixx、iyy、izz为无人机的转动惯量，为俯仰角方向的补偿力矩；

设定子单元，用于设线性方程组ax＝b，则a(k,:)x＝bk,其中，k＝1,2,…n，a(k,:)＝[ak1,ak2,…,akn]，x＝[x1,x2,…,xn]^t；以及，

辨识子单元，用于令b＝mx，采用递推最小二乘法辨识出所述无人机在所述俯仰角方向上所需的补偿力矩

所述递推最小二乘法为：

x^k+1＝x^k+q^k[bk-a(k,:)x^k]，k＝1,2,…,n；

p^k+1＝[i-q^ka(k,:)]p^k；

其中，随机产生初始权值向量x⁰＝rand(n,1)，设p⁰＝αi∈r^n×n，α＝10⁶～10¹⁰，i∈r^n×n是单位矩阵。

进一步地，所述分配模块具体用于：

基于所述无人机的重量、转动惯量以及电机到质量中心的臂长，依次将所述俯仰角方向的补偿力矩、所述滚转角方向的补偿力矩、所述偏航角方向的补偿力矩分配给所述四个电机，以调整所述四个电机的转速，使所述四个电机驱动所述无人机达到目标姿态。

本发明实施例提供的技术方案带来的有益效果是：

在无人机重心偏移后，采用递推最小二乘法对无人机所需的补偿力矩进行准确辨识，并将辨识得到补偿力矩添加回目标控制力矩进行补偿控制，以便将补偿后的力矩分配给无人机的四个电机，使无人机达到目标姿态，从而有效提升无人机的动态性能和抗干扰能力，进而提升用户的飞行体验。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例一提供的无人机重心偏移的补偿方法的流程示意图；

图2是本发明实施例一提供的无人机重心偏移的补偿方法中的补偿原理图；

图3是本发明实施例一提供的无人机重心偏移的补偿方法中添加附加力矩后的俯仰角输出波形图；

图4是本发明实施例一提供的无人机重心偏移的补偿方法中补偿控制后的俯仰角输出波形图；

图5是本发明实施例一中无人机实际飞行时对附加力矩的辨识波形图；

图6是本发明实施例二提供的无人机重心偏移的补偿系统的结构示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。

实施例一

本发明实施例提供了一种无人机重心偏移的补偿方法，参见图1，该方法包括：

s1、获取无人机的目标控制力矩；

s2、获取所述无人机由重心偏移得到的附加力矩；

s3、采用递推最小二乘法辨识出所述无人机所需的补偿力矩；

s4、将所述附加力矩和所述补偿力矩添加到目标控制力矩中进行补偿控制；

s5、将补偿后的力矩分配给四个电机，以使所述四个电机驱动所述无人机达到目标姿态。

需要说明的是，在无人机重心偏移后，获取无人机由重心偏移得到的附加力矩以及无人机的目标控制力矩，同时由递推最小二乘法得到无人机所需的补偿力矩，然后将附加力矩和补偿力矩添加到目标控制力矩中，进行补偿控制，进而由无人机参数将补偿后的力矩分配给四个电机，得到四个电机的转速，从而根据四个电机的转速和传感器数据解算出姿态角信息，以使无人机达到目标姿态。

进一步地，所述获取无人机的目标控制力矩，具体包括：

根据所述无人机的目标姿态角以及由所述无人机当前姿态解算得到的姿态角，计算获得姿态角误差；

根据所述姿态角误差获得目标角速度，并根据所述目标角速度以及由所述无人机当前姿态解算得到的角速度，计算获得姿态角速度误差；

根据所述姿态角速度误差获得所述目标控制力矩。

需要说明的是，如图2所示，由目标姿态角rtarget与当前姿态解算得到的姿态角rcurrent，计算获得姿态角误差。由姿态角误差经pd控制器得到目标角速度ωd,然后与当前姿态解算得到的角速度ω，计算获得姿态角速度误差ωerr。由姿态角速度误差ωerr经pid控制器得到目标控制力矩，然后在补偿控制模块将由重心偏移得到的附加力矩δmx和由最小二乘辨识得到的补偿力矩添加到目标控制力矩中，进行补偿控制。进而由无人机参数，例如重量、转动惯量、臂长等，将扭矩分配给四个电机得到四个电机的转速，最后由电机转速和四旋翼飞行器动力学模型加上传感器数据解算姿态角信息rcurrent。其中，姿态角包括俯仰角、滚转角和偏航角。

进一步地，所述采用递推最小二乘法辨识出所述无人机所需的补偿力矩，具体包括：

基于俯仰角方向的总力矩、俯仰角速度、滚转角加速度和偏航角速度，采用所述递推最小二乘法辨识出所述无人机在所述俯仰角方向上所需的补偿力矩；

基于滚转角方向的总力矩、滚转角速度、俯仰角加速度和偏航角速度，采用所述递推最小二乘法辨识出所述无人机在所述滚转角方向上所需的补偿力矩；

基于偏航角方向的总力矩、偏航角加速度、滚转角速度和俯仰角速度，采用所述递推最小二乘法辨识出所述无人机在所述偏航角方向上所需的补偿力矩。

需要说明的是，在对无人机的力矩进行补偿时，会从俯仰角方向、滚转角方向和偏航角方向的力矩分别进行补偿。

进一步地，所述基于俯仰角方向的总力矩、俯仰角速度、滚转角加速度和偏航角速度，采用所述递推最小二乘法辨识出所述无人机在所述俯仰角方向上所需的补偿力矩，具体包括：

获取所述无人机俯仰角方向的力矩平衡方程其中，mx为无人机俯仰角方向的总力矩，为滚转角加速度，为俯仰角速度，为偏航角速度，ixx、iyy、izz为无人机的转动惯量，为俯仰角方向的补偿力矩；

设线性方程组ax＝b，则a(k,:)x＝bk,其中，k＝1,2,…n，a(k,:)＝[ak1,ak2,…,akn]，x＝[x1,x2,…,xn]^t；

令b＝mx，采用递推最小二乘法辨识出所述无人机在所述俯仰角方向上所需的补偿力矩

所述递推最小二乘法为：

x^k+1＝x^k+q^k[bk-a(k,:)x^k]，k＝1,2,…,n；

p^k+1＝[i-q^ka(k,:)]p^k；

其中，随机产生初始权值向量x⁰＝rand(n,1)，设p⁰＝αi∈r^n×n，α＝10⁶～10¹⁰，i∈r^n×n是单位矩阵。

需要说明的是，本实施例以获取俯仰角方向的补偿力矩为例进行说明。无人机的力矩平衡方程为：其中，mx，my，mz分别为无人机x、y、z三个方向的总力矩，为无人机的转动惯量，p、q、r为角速度，为对应的角加速度，为由于重心偏移在x、y、z三个方向产生的附加力矩。其中，坐标系定义为：原点取在无人机的质心，坐标系与机体固连；x轴与机身设计的纵轴平行，且处于无人机的对称平面内，指向前方；y轴垂直于无人机对称平面指向右方；z轴在无人机对称平面内，且垂直于x轴指向下方。整个坐标系符合欧拉坐标系右手定则。

由于考虑无人机在小角度变化飞行的条件下，近似处理后，以x方向，即俯仰方向为例，获得其中，ixx、izz-iyy、为待辨识参数，mx、为已知量(通过传感器滤波得到或控制回路直接输出)。将递推最小二乘法辨识出的添加到控制回路中，对附加力矩δmx进行补偿控制。同理，可以辨识出滚转角方向和偏航角方向的补偿力矩。

进一步地，所述将补偿后的力矩分配给四个电机，以使所述四个电机驱动所述无人机达到目标姿态，具体包括：

基于所述无人机的重量、转动惯量以及电机到质量中心的臂长，依次将所述俯仰角方向的补偿力矩、所述滚转角方向的补偿力矩、所述偏航角方向的补偿力矩分配给所述四个电机，以调整所述四个电机的转速，使所述四个电机驱动所述无人机达到目标姿态。

需要说明的是，理想情况下，四旋翼无人机的动力学模型可以描述为：将其视为有一个力和三个力矩的三维刚体。如下给出了小角度变化条件下的四旋翼无人机的近似动力学模型：

其中，x、y分别为水平面的坐标；z为垂直位置；ψ为绕z轴的偏航角，θ为绕y轴的俯仰角，φ为绕x轴的滚转角；u为源自无人机底部向上的推力；τψ，τθ，τφ分别为偏航力矩、俯仰力矩、滚转力矩。

动态动力学模型：

四个电机提供升力，一般升力正比于转速的平方。若不考虑空气阻力，那么四旋翼的模型简化如下：

其中，φ、θ、ψ分别是机身围绕x轴、y轴、z轴旋转的角度(逆时针)；ix、iy、iz分别是机身在三个方向的转动惯量；jr是转动惯量；l是电机到质量中心的臂长；m是机身的质量；g是加速度(数值为9.81)；u1,u2,u3,u4,ω由下列方程得出：

ω＝ω2+ω4-ω1-ω3

其中,ω1,ω2,ω3,ω4分别为四个电机的转速，b,d为待定系数。

利用本发明实施例的方法进行仿真实验。

利用matlab软件搭建无人机的仿真模块，在输入端给定俯仰角φ为平均值为0的方波信号，先假设附加力矩δmx＝0.1，将附加力矩添加到控制回路中，俯仰角φ的波形会出现一个偏移量，如图3所示。添加递推最小二乘法辨识反馈模块后，将的辨识结果补偿到控制回路中，俯仰角φ的变化情况如图4所示。结果表明，辨识收敛后，方波的控制输出满足预设要求。

将递推最小二乘法和补偿控制添加到基于c++的飞控源码中，每次起飞的辨识结果如图5所示(红线突变表示无人机起飞和着陆)，结果基本稳定在一个固定值附近。从实际飞行效果观察发现，在室内(无风环境下)手动模式下起飞，无人机基本垂直起飞，没有漂移现象；在室外(有风环境下)手动模式下起飞，无人机在外界扰动情况下会存在一定小范围的漂移，但与未将重心偏移所产生的附加力矩进行补偿处理相比，改善效果已非常明显。

本发明实施例能够在无人机的部分电机发生故障时，根据故障信息对四个电机的控制分配信息进行优化，使四个电机的控制指令与无人机所要达到的姿态和高度呈线性关系，从而使无人机根据优化后的控制分配信息达到其所要达到的姿态和高度，以迅速克服故障对无人机的影响，保证姿态和高度的可控性，保持无人机姿态的稳定性，避免了事故的发生。

实施例二

本发明实施例提供了一种无人机重心偏移的补偿系统，能够实现上述无人机重心偏移的补偿方法的所有流程，参见图6，所述无人机重心偏移的补偿系统包括：

第一获取模块1，用于获取无人机的目标控制力矩；

第二获取模块2，用于获取所述无人机由重心偏移得到的附加力矩；

辨识模块3，用于采用递推最小二乘法辨识出所述无人机所需的补偿力矩；

补偿模块4，用于将所述附加力矩和所述补偿力矩添加到目标控制力矩中进行补偿控制；以及，

分配模块5，用于将补偿后的力矩分配给四个电机，以使所述四个电机驱动所述无人机达到目标姿态。

进一步地，所述第一获取模块具体包括：

姿态角误差计算单元，用于根据所述无人机的目标姿态角以及由所述无人机当前姿态解算得到的姿态角，计算获得姿态角误差；

姿态角速度误差计算单元，用于根据所述姿态角误差获得目标角速度，并根据所述目标角速度以及由所述无人机当前姿态解算得到的角速度，计算获得姿态角速度误差；以及，

获取单元，用于根据所述姿态角速度误差获得所述目标控制力矩。

进一步地，所述辨识模块具体包括：

第一辨识单元，用于基于俯仰角方向的总力矩、俯仰角速度、滚转角加速度和偏航角速度，采用所述递推最小二乘法辨识出所述无人机在所述俯仰角方向上所需的补偿力矩；

第二辨识单元，用于基于滚转角方向的总力矩、滚转角速度、俯仰角加速度和偏航角速度，采用所述递推最小二乘法辨识出所述无人机在所述滚转角方向上所需的补偿力矩；以及，

第三辨识单元，用于基于偏航角方向的总力矩、偏航角加速度、滚转角速度和俯仰角速度，采用所述递推最小二乘法辨识出所述无人机在所述偏航角方向上所需的补偿力矩。

进一步地，所述第一辨识单元具体包括：

方程获取子单元，用于获取所述无人机俯仰角方向的力矩平衡方程其中，mx为无人机俯仰角方向的总力矩，为滚转角加速度，为俯仰角速度，为偏航角速度，ixx、iyy、izz为无人机的转动惯量，为俯仰角方向的补偿力矩；

设定子单元，用于设线性方程组ax＝b，则a(k,:)x＝bk,其中，k＝1,2,…n，a(k,:)＝[ak1,ak2,…,akn]，x＝[x1,x2,…,xn]^t；以及，

辨识子单元，用于令b＝mx，采用递推最小二乘法辨识出所述无人机在所述俯仰角方向上所需的补偿力矩

所述递推最小二乘法为：

x^k+1＝x^k+q^k[bk-a(k,:)x^k]，k＝1,2,…,n；

p^k+1＝[i-q^ka(k,:)]p^k；

其中，随机产生初始权值向量x⁰＝rand(n,1)，设p⁰＝αi∈r^n×n，α＝10⁶～10¹⁰，i∈r^n×n是单位矩阵。

进一步地，所述分配模块具体用于：

基于所述无人机的重量、转动惯量以及电机到质量中心的臂长，依次将所述俯仰角方向的补偿力矩、所述滚转角方向的补偿力矩、所述偏航角方向的补偿力矩分配给所述四个电机，以调整所述四个电机的转速，使所述四个电机驱动所述无人机达到目标姿态。

本发明实施例在无人机重心偏移后，采用递推最小二乘法对无人机所需的补偿力矩进行准确辨识，并将辨识得到补偿力矩添加回目标控制力矩进行补偿控制，以便将补偿后的力矩分配给无人机的四个电机，使无人机达到目标姿态，从而有效提升无人机的动态性能和抗干扰能力，进而提升用户的飞行体验。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。