CN106547275A - 一种新型旋翼类无人机自动定位操控方法 - Google Patents

Abstract

本发明涉及无人机技术领域，具体公开了一种新型旋翼类无人机自动定位操控方法，具体为用带有方向感应功能和位置定位功能的电子设备，多旋翼类无人机按照电子设备的方向和位置来控制自身的方向和位置，从而实现旋翼类无人机的方向和位置随动。主要是构建了动态的相对坐标系，测量信息从测量坐标系转换至相对坐标系下，使用了定位电子设备的姿态和位置信息，其采用了AGPS、陀螺加磁罗盘的定位定姿方法，降低了相对测量受成本和精度影响的问题，从而保证了较高精度的定位定姿手段。利用相对信息进行多旋翼无人机的相对位置和姿态控制，并最终将其转换至测量坐标系下进行绝对控制。

Description

一种新型旋翼类无人机自动定位操控方法

技术领域

本发明涉及无人机技术领域，特别是指一种新型旋翼类无人机自动定位操控方法。

背景技术

近年来，多旋翼类无人机技术得到了快速的发展，其具有飞行稳定性好、易操控、低噪声、结构简单和危害性小等特点，且可以方便有效的搭载各种任务负载来执行各种任务，在军事上和民用上均取得了十分广泛的应用，如航空拍摄、农用植保、电力线路巡检和军事侦察等。

现有的旋翼类无人机多通过遥控器操控，由操作手通过遥控器的摇杆来控制旋翼类无人机的航姿角和位置、速度等。这种操控方式的难度较大，操作手需要通过多个通道的联动控制，来实现自己期望的飞机状态。为了有效降低多旋翼无人机的操控门槛，借助于智能化的自主控制技术，实现与操控者的智能交互控制。满足其空间相对定位控制要求。利用该发明，可满足多旋翼无人机定向凝视功能上的自动实现，相关的操控方法目前在文献中未检索到。

发明内容

本发明的目的是提供一种新型旋翼类无人机自动定位操控方法，主要应用在无人机定向凝视应用场景和室外的相对辅助定位场景下，该方法可以改变现有此类无人机利用遥控器进行精密手动操控的模式，从而大大方便旋翼类无人机的使用和应用推广。

为了实现上述目的，本发明采用了以下的技术方案：一种新型旋翼类无人机自动定位操控方法，包括带有方向感应功能和位置定位功能的电子设备，所述电子设备通过无线方式和旋翼类无人机建立联接，具体步骤如下：

(1)电子设备给出在测量坐标系下的经度、纬度、高度以及航向角，定义为λ,B,h,γ，在测量坐标系下，该值为地理坐标系的位置以及航向角。

(2)利用无人旋翼机所具有的机上测量装置，可以获得在测量的地理坐标系下的经纬度、高度和航向角，定位为λ_T,B_T,h_T,γ_T。

(3)构建相对坐标系，基于绝对测量位置获得相对位置关系：

假设在有限范围内，忽略地球的曲率变化，定义当地的基准坐标系，坐标原点为λ,B,h，x轴指向电子设备的航向角γ方向，y轴定位为垂直地面，向上为正，则z轴构成右手坐标系。

根据电子设备所具有的地理位置和航向，以及多旋翼无人机所具有的地理位置和航向，进行坐标变换，计算得到相对坐标系下多旋翼无人机的相对位置和航向角。将地理坐标系位置进行地心坐标系的变化，根据GPS接收机的信号，其基于美国WGS84坐标系，进行如下坐标变化，得到其经纬高如下：

首先定义基本地球坐标参数

地球长半径：a＝6378137±2(m)；

地球引力和地球质量的乘积：GM＝3986005×108m³/s²±0.6×108m³/s²；

正常化二阶带谐系数：C20＝-484.16685×10^-6±1.3×10^-9；

地球重力场二阶带球谐系数：J2＝108263×10^-8；

地球自转角速度：ω＝7292115×10^-11rad/s±0.150×10^-11rad/s；

扁率f＝0.003352810664

以此为基础进行GPS接收数据的地心坐标系转换。

Lr＝L·π/180

x＝(N+H)cos(B)cos(Lr) (1)

y＝(N+H)cos(B)sin(Lr)

z＝(N(1-e²)+H)sin(B)

式中，L,B,H为接收机直接接收得到的信号。根据所得到经纬高，将其转换为地心坐标系下的坐标x,y,z。

(4)根据电子设备基于接收机得到的坐标系x_h,y_h,z_h，以及磁罗盘给出的方位角γ，将电子设备通过无线方式和旋翼类无人机联通，电子设备可以将相应的控制信号传送至旋翼类无人机上。

(5)旋翼类无人机上的控制器接收到电子设备感应到的方向信息和其位置信息后，在此基础上生成自身的方向控制给定和位置控制给定：

旋翼类无人机根据自身定位设备所得到的地心坐标系坐标x_a,y_a,z_a，进行相对位置计算。定义地面坐标系如下：以基准电子设备所在位置作为坐标原点，x轴指向以方位角为准，所在位置与地心连线设为y轴，向上为正，Z轴构成右手坐标系。

假设飞机距离电子设备在千米范围内，可忽略地球曲率，认为地面坐标系x轴沿地面延伸。基于飞行的地心坐标，则可以得到飞行相对于电子设备的相位位置关系如下：

则有地面坐标系下可以得到飞行相对于电子设备所在地表点的相关坐标如下：

根据电子设备本身的高度信息，修正计算得到基于电子设备的相对位置为

旋翼类无人机上的控制器收到电子设备感应到的方向信息和其位置信息后，生成位置控制给定，也可以叠加一个可变或不变量后再将其作为旋翼类无人机的方向控制给定。

(6)以电子设备的位置为圆心，以旋翼类无人机同电子设备之间的相对距离为半径作圆；以电子设备的位置为直线上的一点，以电子设备的方向为斜率，作一条射线；所作的射线和圆必有一个交点，将该交点作为位置控制的给定。

根据式(4)，可以得到飞行器相对于电子设备的同心圆半径为：

则有相对位置控制指令为：

x_rc＝R,y_rc＝y_r,z_rc＝0 (6)

同时飞行器的航向角指令可计算为：

(7)在控制指令生成后，基于所需要的控制指令，进行位置与航向的控制实现。对于一般的旋翼类无人机的位置回路，其主要的控制机构为电机转速，方位的控制通过姿态控制实现，则可以给出以下的实现步骤。

位置控制通道可分为水平控制通道和高度控制通道，由于高度控制与角度不存在耦合关系，因此可以直接进行，得到如下的控制量。

根据期望的高度角速度，反向阶段升力的输入信号，根据以下的旋翼动力学方程获取相关值。

式中，分别取系数为k_p3＝2,k_d3＝0.8。

得到的总升力一般根据旋翼的个数进行分解，根据四旋翼、六旋翼和八旋翼进行相应的转速分解。此外还需要考虑水平位置控制通过的俯仰和滚转角进行反解算。

水平位置控制采用PID控制器，以期望目标相对位置(x_rc,z_rc)和实际相对位置(x_r,z_r)的差作为输入信号，形成如下的控制形式：

式中，系数分别设定为k_p1＝2.3,k_i1＝0.3,k_d1＝0.8；k_p2＝2.3,k_i2＝0.3,k_d2＝0.8；

将水平位置的加速度值作为反结算模块的输入信息，基于多旋翼飞行器的运动学和动力学方程，做以下的推导。在多旋翼悬停与慢速飞行过程中，可假设角速度较小，则运动学方程中交叉项约为零，可将质心的动力学和运动学模型写为

对式(11)中的方程1两边乘以sinψ，方程2两边乘以cosψ，方程1减去方程2可得

而对式(11)中的第一个方程变形，则可以得到

则此时可以获得姿态内回路的俯仰角、滚转角以及偏航角的指令(φ_c,θ_c,ψ_c)，同时还可以获得基本的控制量U₁。

(8)根据姿态指令进行机动过程中的姿态控制，并在位置到达后将姿态指令全部置零，除了航向角保持指令航向角。则可以得到多轴电机融合后的输入控制量为：

综合以上则可以得到多旋翼无人机的典型四路输入信号U₁,U₂,U₃,U₄。

(9)根据旋翼的配置数目进行各电机的转速分配，以典型4旋翼、6旋翼为例进行说明：

四旋翼：根据四旋翼的一般配置，可以得到四个电机的组合方式如下：

根据已知的控制律输出的已知信号，反向求解电机的转速控制信号大小，可得

以此信号作为驱动电机的脉冲调宽信号输入值，则可以得到经过电子调速器输出的电机驱动信号。

六旋翼：六旋翼的配置，只需要更改电机的组合方式，实现信号的分解驱动。根据基本配置，可以得到六个电机的组合方式为：

由于存在6个位置量，四个方程，可以附加假设条件则可以求解得到各电机控制量分别为：

旋翼类无人机按照以上生成的方向控制给定和位置控制给定来控制自身的方向和位置，从而实现旋翼类无人机的方向和位置随动。

本发明的有益效果在于：采用上述装置后，电子设备通过无线方式和旋翼类无人机建立联接，将自身感应到的方向信息和自身位置信息传送给多旋翼类无人机，多旋翼类无人机上的控制器接收到该方向信息和位置信息后，按照生成的方向控制给定和位置控制给定来控制自身的方向和位置，从而实现旋翼类无人机的方向和位置随动，不需要操作手直接控制旋翼类无人机的方向和位置，大大减轻了其操作负担，方便其对旋翼类无人机的操控。可实现户外的辅助相对定位，并满足多旋翼无人机的确定距离下的自动凝视飞行，改变原有采用手动精确控制的方法。

附图说明

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

图1为本发明的结构示意图；

图2为四旋翼无人机的电机组合方式示意图；

图3为六旋翼无人机的电机组合方式示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

一种新型旋翼类无人机自动定位操控方法，包括带有方向感应功能和位置定位功能的电子设备，所述电子设备通过无线方式和旋翼类无人机建立联接，具体步骤如下：

(1)电子设备给出在测量坐标系下的经度、纬度、高度以及航向角，定义为λ,B,h,γ，在测量坐标系下，该值为地理坐标系的位置以及航向角。

(2)利用无人旋翼机所具有的机上测量装置，可以获得在测量的地理坐标系下的经纬度、高度和航向角，定位为λ_T,B_T,h_T,γ_T。

(3)构建相对坐标系，基于绝对测量位置获得相对位置关系：

假设在有限范围内，忽略地球的曲率变化，定义当地的基准坐标系，坐标原点为λ,B,h，x轴指向电子设备的航向角γ方向，y轴定位为垂直地面，向上为正，则z轴构成右手坐标系。

根据电子设备所具有的地理位置和航向，以及多旋翼无人机所具有的地理位置和航向，进行坐标变换，计算得到相对坐标系下多旋翼无人机的相对位置和航向角。将地理坐标系位置进行地心坐标系的变化，根据GPS接收机的信号，其基于美国WGS84坐标系，进行如下坐标变化，得到其经纬高如下：

首先定义基本地球坐标参数

地球长半径：a＝6378137±2(m)；

地球引力和地球质量的乘积：GM＝3986005×108m³/s²±0.6×108m³/s²；

正常化二阶带谐系数：C20＝-484.16685×10^-6±1.3×10^-9；

地球重力场二阶带球谐系数：J2＝108263×10^-8；

地球自转角速度：ω＝7292115×10^-11rad/s±0.150×10^-11rad/s；

扁率f＝0.003352810664

以此为基础进行GPS接收数据的地心坐标系转换。

Lr＝L·π/180

x＝(N+H)cos(B)cos(Lr) (1)

y＝(N+H)cos(B)sin(Lr)

z＝(N(1-e²)+H)sin(B)

式中，L,B,H为接收机直接接收得到的信号。根据所得到经纬高，将其转换为地心坐标系下的坐标x,y,z。

(4)根据电子设备基于接收机得到的坐标系x_h,y_h,z_h，以及磁罗盘给出的方位角γ，将电子设备通过无线方式和旋翼类无人机联通，电子设备可以将相应的控制信号传送至旋翼类无人机上。

(5)旋翼类无人机上的控制器接收到电子设备感应到的方向信息和其位置信息后，在此基础上生成自身的方向控制给定和位置控制给定：

旋翼类无人机根据自身定位设备所得到的地心坐标系坐标x_a,y_a,z_a，进行相对位置计算。定义地面坐标系如下：以基准电子设备所在位置作为坐标原点，x轴指向以方位角为准，所在位置与地心连线设为y轴，向上为正，Z轴构成右手坐标系。

假设飞机距离电子设备在千米范围内，可忽略地球曲率，认为地面坐标系x轴沿地面延伸。基于飞行的地心坐标，则可以得到飞行相对于电子设备的相位位置关系如下：

则有地面坐标系下可以得到飞行相对于电子设备所在地表点的相关坐标如下：

根据电子设备本身的高度信息，修正计算得到基于电子设备的相对位置为

旋翼类无人机上的控制器收到电子设备感应到的方向信息和其位置信息后，生成位置控制给定，也可以叠加一个可变或不变量后再将其作为旋翼类无人机的方向控制给定。

(6)如图1所示，以电子设备的位置为圆心，以旋翼类无人机同电子设备之间的相对距离为半径作圆；以电子设备的位置为直线上的一点，以电子设备的方向为斜率，作一条射线；所作的射线和圆必有一个交点，将该交点作为位置控制的给定。

根据式(4)，可以得到飞行器相对于电子设备的同心圆半径为：

则有相对位置控制指令为：

x_rc＝R,y_rc＝y_r,z_rc＝0 (6)

同时飞行器的航向角指令可计算为：

(7)在控制指令生成后，基于所需要的控制指令，进行位置与航向的控制实现。对于一般的旋翼类无人机的位置回路，其主要的控制机构为电机转速，方位的控制通过姿态控制实现，则可以给出以下的实现步骤。

位置控制通道可分为水平控制通道和高度控制通道，由于高度控制与角度不存在耦合关系，因此可以直接进行，得到如下的控制量。

根据期望的高度角速度，反向阶段升力的输入信号，根据以下的旋翼动力学方程获取相关值。

式中，分别取系数为k_p3＝2,k_d3＝0.8。

得到的总升力一般根据旋翼的个数进行分解，根据四旋翼、六旋翼和八旋翼进行相应的转速分解。此外还需要考虑水平位置控制通过的俯仰和滚转角进行反解算。

水平位置控制采用PID控制器，以期望目标相对位置(x_rc,z_rc)和实际相对位置(x_r,z_r)的差作为输入信号，形成如下的控制形式：

式中，系数分别设定为k_p1＝2.3,k_i1＝0.3,k_d1＝0.8；k_p2＝2.3,k_i2＝0.3,k_d2＝0.8；

将水平位置的加速度值作为反结算模块的输入信息，基于多旋翼飞行器的运动学和动力学方程，做以下的推导。在多旋翼悬停与慢速飞行过程中，可假设角速度较小，则运动学方程中交叉项约为零，可将质心的动力学和运动学模型写为

对式(11)中的方程1两边乘以sinψ，方程2两边乘以cosψ，方程1减去方程2可得

而对式(11)中的第一个方程变形，则可以得到

则此时可以获得姿态内回路的俯仰角、滚转角以及偏航角的指令(φ_c,θ_c,ψ_c)，同时还可以获得基本的控制量U₁。

(8)根据姿态指令进行机动过程中的姿态控制，并在位置到达后将姿态指令全部置零，除了航向角保持指令航向角。则可以得到多轴电机融合后的输入控制量为：

综合以上则可以得到多旋翼无人机的典型四路输入信号U₁,U₂,U₃,U₄。

(9)根据旋翼的配置数目进行各电机的转速分配，以典型4旋翼、6旋翼为例进行说明：

四旋翼：结合图2，根据四旋翼的一般配置，可以得到四个电机的组合方式如下：

根据已知的控制律输出的已知信号，反向求解电机的转速控制信号大小，可得

以此信号作为驱动电机的脉冲调宽信号输入值，则可以得到经过电子调速器输出的电机驱动信号。

六旋翼：结合图3，六旋翼的配置，只需要更改电机的组合方式，实现信号的分解驱动。根据基本配置，可以得到六个电机的组合方式为：

由于存在6个位置量，四个方程，可以附加假设条件则可以求解得到各电机控制量分别为：

旋翼类无人机按照以上生成的方向控制给定和位置控制给定来控制自身的方向和位置，从而实现旋翼类无人机的方向和位置随动。

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

Claims (1)

1.一种新型旋翼类无人机自动定位操控方法，其特征在于：包括带有方向感应功能和位置定位功能的电子设备，所述电子设备通过无线方式和旋翼类无人机建立联接，具体步骤如下：

(1)电子设备给出在测量坐标系下的经度、纬度、高度以及航向角，定义为λ,B,h,γ，在测量坐标系下，该值为地理坐标系的位置以及航向角；

(2)利用无人旋翼机所具有的机上测量装置，可以获得在测量的地理坐标系下的经纬度、高度和航向角，定位为λ_T,B_T,h_T,γ_T；

(3)构建相对坐标系，基于绝对测量位置获得相对位置关系：

假设在有限范围内，忽略地球的曲率变化，定义当地的基准坐标系，坐标原点为λ,B,h，x轴指向电子设备的航向角γ方向，y轴定位为垂直地面，向上为正，则z轴构成右手坐标系；

根据电子设备所具有的地理位置和航向，以及多旋翼无人机所具有的地理位置和航向，进行坐标变换，计算得到相对坐标系下多旋翼无人机的相对位置和航向角；将地理坐标系位置进行地心坐标系的变化，根据GPS接收机的信号，其基于美国WGS84坐标系，进行如下坐标变化，得到其经纬高如下：

首先定义基本地球坐标参数

地球长半径：a＝6378137±2(m)；

地球引力和地球质量的乘积：GM＝3986005×108m³/s²±0.6×108m³/s²；

正常化二阶带谐系数：C20＝-484.16685×10^-6±1.3×10^-9；

地球重力场二阶带球谐系数：J2＝108263×10^-8；

地球自转角速度：ω＝7292115×10^-11rad/s±0.150×10^-11rad/s；

扁率f＝0.003352810664

以此为基础进行GPS接收数据的地心坐标系转换；

Lr＝L·π/180

$N=a/\sqrt{1-e^{2}sin{\left(B\right)}^2}$

x＝(N+H)cos(B)cos(Lr) (1)

y＝(N+H)cos(B)sin(Lr)

z＝(N(1-e²)+H)sin(B)

式中，L,B,H为接收机直接接收得到的信号。根据所得到经纬高，将其转换为地心坐标系下的坐标x,y,z；

(4)根据电子设备基于接收机得到的坐标系x_h,y_h,z_h，以及磁罗盘给出的方位角γ，将电子设备通过无线方式和旋翼类无人机联通，电子设备可以将相应的控制信号传送至旋翼类无人机上；

(5)旋翼类无人机上的控制器接收到电子设备感应到的方向信息和其位置信息后，在此基础上生成自身的方向控制给定和位置控制给定：

旋翼类无人机根据自身定位设备所得到的地心坐标系坐标x_a,y_a,z_a，进行相对位置计算；定义地面坐标系如下：以基准电子设备所在位置作为坐标原点，x轴指向以方位角为准，所在位置与地心连线设为y轴，向上为正，Z轴构成右手坐标系；

假设飞机距离电子设备在千米范围内，可忽略地球曲率，认为地面坐标系x轴沿地面延伸；基于飞行的地心坐标，则可以得到飞行相对于电子设备的相位位置关系如下：

$G=\left[\begin{array}{ccc}-\sin \mathrm{\γ}\sin L-\cos \mathrm{\γ}\sin B\cos L& \sin \mathrm{\γ}\cos L-\cos \mathrm{\γ}\sin B\sin L& \cos \mathrm{\γ}\cos B\\ \cos B\cos L& \cos B\sin L& \sin B\\ -\cos \mathrm{\γ}\sin L+\sin \mathrm{\γ}\sin B\cos L& \cos \mathrm{\γ}\cos L+\sin \mathrm{\γ}\sin B\sin L& -\sin \mathrm{\γ}\cos B\end{array}\right]---\left(2\right)$

则有地面坐标系下可以得到飞行相对于电子设备所在地表点的相关坐标如下：

$\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=G\left[\begin{array}{c}{x}_a\\ y_a\\ z_a\end{array}\right]---\left(3\right)$

根据电子设备本身的高度信息，修正计算得到基于电子设备的相对位置为

$\left[\begin{array}{c}{x}_r\\ y_r\\ z_r\end{array}\right]=\left[\begin{array}{c}x\\ y-H\\ z\end{array}\right]---\left(4\right)$

旋翼类无人机上的控制器收到电子设备感应到的方向信息和其位置信息后，生成位置控制给定，也可以叠加一个可变或不变量后再将其作为旋翼类无人机的方向控制给定；

(6)以电子设备的位置为圆心，以旋翼类无人机同电子设备之间的相对距离为半径作圆；以电子设备的位置为直线上的一点，以电子设备的方向为斜率，作一条射线；所作的射线和圆必有一个交点，将该交点作为位置控制的给定；

根据式(4)，可以得到飞行器相对于电子设备的同心圆半径为：

$R=\sqrt{x_r^2+y_r^2+z_r^2}---\left(5\right)$

则有相对位置控制指令为：

x_rc＝R,y_rc＝y_r,z_rc＝0 (6)

同时飞行器的航向角指令可计算为：

$\left\{\begin{array}{c}r+\mathrm{\&pi;},\mathrm{\&gamma;}<\mathrm{\&pi;}\\ \mathrm{\&gamma;}-\mathrm{\&pi;},\mathrm{\&gamma;}\&GreaterEqual;\mathrm{\&pi;}\end{array}\right.---\left(7\right)$

(7)在控制指令生成后，基于所需要的控制指令，进行位置与航向的控制实现。对于一般的旋翼类无人机的位置回路，其主要的控制机构为电机转速，方位的控制通过姿态控制实现，则可以给出以下的实现步骤；

位置控制通道可分为水平控制通道和高度控制通道，由于高度控制与角度不存在耦合关系，因此可以直接进行，得到如下的控制量；

$\stackrel{\&CenterDot;\&CenterDot;}{y}=k_{p3}(y_{rc}-y_r)+k_{d3}\frac{d(y_{rc}-y_r)}{dt}---\left(8\right)$

根据期望的高度角速度，反向阶段升力的输入信号，根据以下的旋翼动力学方程获取相关值；

$U_1=\frac{1}{\cos \mathrm{\&phi;}\cos \mathrm{\&theta;}}\left(k_{p3}\right(y_{rc}-y_r)+k_{d3}({\stackrel{\&CenterDot;}{y}}_{rc}-{\stackrel{\&CenterDot;}{y}}_r)+mg)---\left(9\right)$

式中，分别取系数为k_p3＝2,k_d3＝0.8；

得到的总升力一般根据旋翼的个数进行分解，根据四旋翼、六旋翼和八旋翼进行相应的转速分解。此外还需要考虑水平位置控制通过的俯仰和滚转角进行反解算；

水平位置控制采用PID控制器，以期望目标相对位置(x_rc,z_rc)和实际相对位置(x_r,z_r)的差作为输入信号，形成如下的控制形式：

$\stackrel{\&CenterDot;\&CenterDot;}{x}=k_{p1}(x_{rc}-x_r)+k_{i1}\&Integral;\left(x_{rc}-x_r\right)dt+k_{d1}({\stackrel{\&CenterDot;}{x}}_{rc}-{\stackrel{\&CenterDot;}{x}}_r)$

(10)

$\stackrel{\&CenterDot;\&CenterDot;}{z}=k_{p1}(z_{rc}-z_r)+k_{i1}\&Integral;\left(z_{rc}-z_r\right)dt+k_{d1}({\stackrel{\&CenterDot;}{z}}_{rc}-{\stackrel{\&CenterDot;}{z}}_r)$

式中，系数分别设定为k_p1＝2.3,k_i1＝0.3,k_d1＝0.8；k_p2＝2.3,k_i2＝0.3,k_d2＝0.8；

将水平位置的加速度值作为反结算模块的输入信息，基于多旋翼飞行器的运动学和动力学方程，做以下的推导。在多旋翼悬停与慢速飞行过程中，可假设角速度较小，则运动学方程中交叉项约为零，可将质心的动力学和运动学模型写为

$\stackrel{\&CenterDot;\&CenterDot;}{x}=(sin\mathrm{\&theta;}cos\mathrm{\&phi;}cos\mathrm{\&psi;}+sin\mathrm{\&phi;}sin\mathrm{\&psi;})\frac{U_1}{m}$

(11)

$\stackrel{\&CenterDot;\&CenterDot;}{z}=(sin\mathrm{\&theta;}cos\mathrm{\&phi;}sin\mathrm{\&psi;}-sin\mathrm{\&phi;}cos\mathrm{\&psi;})\frac{U_1}{m}$

对式(11)中的方程1两边乘以sinψ，方程2两边乘以cosψ，方程1减去方程2可得

$m\stackrel{\&CenterDot;\&CenterDot;}{x}sin\mathrm{\&psi;}-m\stackrel{\&CenterDot;\&CenterDot;}{z}cos\mathrm{\&psi;}=U_{1}sin\mathrm{\&phi;}$

${\mathrm{\&phi;}}_d=arcsin\&lsqb;\frac{m}{U_1}(\stackrel{\&CenterDot;\&CenterDot;}{x}sin\mathrm{\&psi;}-\stackrel{\&CenterDot;\&CenterDot;}{y}cos\mathrm{\&psi;})\&rsqb;---\left(12\right)$

而对式(11)中的第一个方程变形，则可以得到

$sin\mathrm{\&theta;}=\&lsqb;\frac{m\stackrel{\&CenterDot;\&CenterDot;}{x}}{U_1}-sin\mathrm{\&phi;}\sin \mathrm{\&psi;}\&rsqb;/cos\mathrm{\&phi;}cos\mathrm{\&psi;}$

(13)

则此时可以获得姿态内回路的俯仰角、滚转角以及偏航角的指令(φ_c,θ_c,ψ_c)，同时还可以获得基本的控制量U₁；

(8)根据姿态指令进行机动过程中的姿态控制，并在位置到达后将姿态指令全部置零，除了航向角保持指令航向角；则可以得到多轴电机融合后的输入控制量为：

$U_2=k_{p\mathrm{\&phi;}}{e}_{\mathrm{\&phi;}}+k_{d\mathrm{\&phi;}}\frac{{\mathrm{de}}_{\mathrm{\&phi;}}}{dt}+k_{i\mathrm{\&phi;}}\&Integral;e_{\mathrm{\&phi;}}$

$U_3=k_{p\mathrm{\&theta;}}{e}_{\mathrm{\&theta;}}+k_{d\mathrm{\&theta;}}\frac{{\mathrm{de}}_{\mathrm{\&theta;}}}{dt}+k_{i\mathrm{\&theta;}}\&Integral;e_{\mathrm{\&theta;}}---\left(14\right)$

$U_4=k_{p\mathrm{\&psi;}}{e}_{\mathrm{\&psi;}}+k_{d\mathrm{\&psi;}}\frac{{\mathrm{de}}_{\mathrm{\&psi;}}}{dt}+k_{i\mathrm{\&psi;}}\&Integral;e_{\mathrm{\&psi;}}$

综合以上则可以得到多旋翼无人机的典型四路输入信号U₁,U₂,U₃,U₄。