四旋翼无人机飞行轨迹的自主导航控制_王源

Ｔ 表 示 机 体 角 速 度 矢 量； 度， ｒ） Ｓ 为 矩 阵 乘 子。 Ω＝ （ ｐ， ｑ，
则姿态角变化率与机体角速度矢量的关系为 １ ｔ ａ ｎ ｓ ｉ ｎ ａ ｎ ｃ ｏ ｓ θ θ ｔ 熿 燄 · （ ） ｃ ｏ ｓ ｉ ｎ １ Θ ＝ Ｓ·Ω ＝ ０ －ｓ Ω ０ ｓ ｅ ｃ ｓ ｉ ｎ ｓ ｅ ｃ ｃ ｏ ｓ 燀 燅 θ θ Ｔ 表示四旋翼无人机空间位置坐 ｘ， ｚ） 定义 Ｗ ＝ （ ｙ，
２ ０ １ ４ 年第 ４ 期
江 苏 航 空 Ｊ Ｉ ＡＮＧ Ｓ ＵＡ Ｖ Ｉ Ａ Ｔ Ｉ ＯＮ
５
四旋翼无人机飞行轨迹的自主导航控制
南京航空航天大学 ◎ 王 源 郑祥明 昂海松／ 强 耦 合 性， 如果把对 四旋翼无人机系 统 具 有 多 变 量 、 信号响应速度不同 的 各 个 变 量 混 杂 在 一 起 设 计 控 制 系 统 会引起系统不稳定 ， 需按时间尺度将控制变量分组进行控 制系统设计 。 有关文 献 运 用 奇 异 摄 动 理 论 将 固 定 翼 飞 行 器系统按照各变量对输入信号响应速度不同进行分组 ， 分 别设计内环 、 中环以及外环控制律 。 传统飞行运动控制一般采用航迹速度 、 爬升角和倾斜 角作为控制变量来 控 制 飞 行 轨 迹 。 四 旋 翼 无 人 机 在 飞 行 可通过调整 姿 态 角 度 来 控 制 其 质 心 运 动 ， 质心运 过程中 ， 动与姿态控制具有高 度 一 致 性 ， 因 此， 本文提出了一种基 于直接位置坐标的飞行轨迹控制方法 ， 通过期望的质心轨 迹运动 ， 解算出飞行 器 姿 态 预 期 以 及 所 需 推 力 ， 并设计了 一组虚拟控制力矩来 控 制 四 旋 翼 无 人 机 俯 仰 、 滚 转、 偏航 以及上下运动 。 刚体运动方程分 为 运 动 学 方 程 和 动 力 学 方 程 。 四 旋 翼无人机在运动的时候可以视为刚体 ， 因此在讨论其运动 方程的时候 ， 视之为质点 。 １ ． １ 运动学方程 定义 四 旋 翼 运 动 过 程 中 表 示 滚 转 角 ， θ表示俯仰 Ｔ ， （ ， ， ） 角， 表示偏 航 角 表 示 姿 态 角 矢 量； Θ＝ θ ψ ｐ 为机 ψ 体滚转角速 度 ， ｒ为机体偏航角速 ｑ 为 机 体 俯 仰 角 速 度，
３ 飞行轨迹自主导航控制方法
本文采用直接位 置 坐 标 的 方 法 控 制 飞 行 轨 迹 。 控 制 方法是通过期望质心轨迹运动 ， 解算出飞行器预期的姿态 角以及需用推力 ， 设计了一组虚拟控制力矩来分别控制四 旋翼无人机俯仰 、 滚转 、 偏航以及上下运动 。 ３ ． １ 位置误差控制 控制系统设计的本质是对误差进行抑制 ， 因此本文选 择对无人机空间位置 Ｗ 的误差进行抑制 。 假设 位 置 误 差 为 Ｗｅ ， 指令位置为 Ｗｃ ， 则下列关系式成立 图 ３ 四旋翼无人机飞行轨迹控制流程 ３ ． ３ 仿真验证
（ ） １ ２
３ ｚ ｚ燅 燀 燅 燀 ０ ０ λ ｃ－ ２ Ｔ ） ， 将 并 假 设 ψ＝ 可以得到如 ｕ ｕ ｕ ４ ｖ＝ ［ １， ２， ３ ］ 代入式 （ ｃ， ψ 下表达式
ｕ ｃ ｏ ｓ ｕ ｓ ｉ ｎ １ ｃ＋ ２ ｃ ψ ψ 烄 ＝ ａ ｒ ｃ ｔ ａ ｎ θ ｕ ３＋ ｇ ｕ ｓ ｉ ｎ ｕ ｃ ｏ ｓ １ ｃ－ ２ ｃ ψ ψ ａ ｒ ｃ ｓ ｉ ｎ ＝ ２ ２ ２ （ ｕ ＋ ｕ ＋ ｕ ＋ ２ ３ ｇ） 槡１ 烅 Ｔ＝ｍ［ ｕ ｓ ｉ ｎ ｃ ｏ ｓ ｃ ｏ ｓ ｓ ｉ ｎ ｓ ｉ ｎ θ １（ ｃ ｃ ψ ＋ ψ ＋ ） ｕ ｓ ｉ ｎ ｓ ｉ ｎ ｃ ｏ ｓ － ｃ ｏ ｓ ｓ ｉ ｎ θ ２（ ｃ ｃ ψ ψ ＋
江 苏 航 空 达式
总第 １ ３ ９期
） ） 将式 （ 和式 （ 合并写成矩阵的形式当作控制四旋 ３ ６ 翼无人机的输入变量
¨ （ ） Ｗ ＝¨ Ｗｃ ＋λ Ｗｃ － Ｗ） Ｗｃ － Ｗ ） １ ０ ＋λ １（ ２（ ） ， ， 从式 （ 可以看出 等号右边的变量 是状态 反 馈 量 而 １ ０ Ｗ
采用 ＭＡ Ｔ Ｌ Ａ Ｂ 对该 算 法 进 行 仿 真 验 证。 电 机 最 大 （ ） 转速为 ３ ／ ， 四旋翼无人机沿着竖直 方 向 上 升 ， 机 ８ ０ ０ ０ｒ ｍ ｉ ｎ 体参数和初始条件设置如下 ： 机体参数为 ； 无人机质量 ： ｍ＝ ０ ． ４ ７ｋ ｇ 臂长 ： Ｉ＝ ０ ． ２ ｍ；
第４期 ： 惯性张量 （ 单位 ）
王 源， 等 ：四旋翼无人机飞行轨迹的自主导航控制
７
（ ， ， ） ； Ｉ ｉ ａ ４ ． ７ ｅ １ ３ ４ ． ７ ｅ １ ３ ９ ． ３ ｅ １ ３ ｇ ｆ ＝ｄ ： 旋翼惯性矩 （ 单位 ） 。 Ｉ ３ ． ４ ３ ｅ １ ５ ｒ＝ ； 。 空气参数为 ： ２ ． ９ ８ ｅ １ ６ ｋ＝ １ ． １ ４ ｅ １ ７ μ＝ 初始条件为 ， ， ） ； 初始位置 ： Ｗ０ ＝ （ ０ ０ ０ ： （ ， ， ） （ ， ， ） ； 初始速度 ｘ ｙ ｚ ＝ ０ ０ ０ ； 初始姿态角和姿态角变化率均为 ０ ， ， ） ； 结束条件为 ： Ｗｔ＝ （ ０ ０ ５ ０ 。 仿真时间 ： １ ５ｓ 带宽选取 （ ， ， ） ； （ ， ， ） ； ｉ ａ ２ ２ ２ ｉ ａ １ １ １ λ λ ｇ ｇ １ ＝ｄ ２ ＝ｄ （ ， ， ） ； ｉ ａ ２ ０ ０ ２ ０ ０ ２ ０ ０ λ ｇ ｄ ＝ｄ （ ， ， ） 。 ｉ ａ １ ０ ０ １ ０ ０ １ ０ ０ λ ｇ ｐ ＝ｄ 仿真结果如图 ４ ７ 所示 。 ～ 图 ６ 滚转 、 俯仰和偏航角速度
图 １ 四旋翼无人机结构组成
示第ｉ 个旋翼的 角 速 度 ， ｉ 表 示ｉ 第 个 旋 翼 产 生 的 升 力 。 ｆ 则四旋翼无人机总的升力可以表示为
４ ４
２ （ ） ３ 四旋翼无人机的旋翼按照旋转方向可以分为两组 ， 即 ｉ ｉ ＝１ ＝１ （ ， ） ， ） ， 和（ 两组旋翼的旋转方向相反 ， 用于平衡扭矩 。 其中 μ 为与空气粘 度 和 桨 叶 形 状 有 关 的 参 数 。 定 义 向 量 １ ３ ２ ４ Ｔ 假定初始时刻 ４ 个旋翼的转速相同 ， 则四旋翼无人机可能 ｅ ， ， ） ， 则惯性坐标系 下 四 旋 翼 无 人 机 动 力 学 方 程 ０ ０ １ ｚ＝（ 的运动状态与两组旋翼的转速关系如表 １ 所示 。 可以表示为 表 １ 四旋翼无人机运动状态控制 （ ） ｍ ｖ ＝Ｌ Ｔ ｅ ｅ ４ －ｍ ｅ ｚ） ｚ ｇ ｆ（ 状态 旋翼 １ 旋翼 ２ 旋翼 ３ 旋翼 ４ 、 定 义的虚拟控制力矩τ 有 ３ 个分量 ， 分别为τ 以及 τ τ １ ２ ３。
） （ １ ３
烆 （ ｕ ｃ ｏ ｓ ｃ ｏ ｓ θ ３＋ ｇ） ］ ， 定义误差函数 Θ Θ ΘΘ ｅ＝ ｃ－ ｅ 按指数规律收敛 。 同 样
图 ２ 按照时间尺度设计控制系统流程 内环控 制 变 量 包 括 角 速 度 Ω 和 姿 态 角 Θ ， 角速度变 量对输入信号的响 应 速 度 最 快 。 外 环 控 制 变 量 为 位 置 变 量 Ｗ， 对输 入 信 号 的 响 应 速 度 最 慢 。 外 环 输 入 量 为 期 望 的四旋翼无人机运动轨迹 ， 由轨迹误差可以解算出无人机 采用 Ｐ 得到误差控制方程为 Ｉ Ｄ 控制 ， ¨ Θ λ Θｅ ＋λ Θ ｅ＋ ｄ ｅ ＝０ ｐ （ ） １ ４ （ ） 。 和 均 为 正 定 矩 阵 一 般 为 对 角 阵 将 方 其中λ λ ｄ ｐ ） ） ） ， 程（ 和式 （ 代入式 （ 并忽略由于内环响应比外环相 １ ７ １ ４ 则通过简单的推导可以得到如下 应快得多而带来的影响 ， 控制力矩表达式
Ｔ＝
ｉ ＝μ ｉ ∑ｆ ∑ω
竖直 （ 上） 竖直 （ 下） 滚转 俯仰
增加 降低 保持 降低
增加 降低 增加 增加
增加 降低 保持 降低
增加 降低 降低 增加
源自文库
各个力 矩 的 作 用 分 别 为 ： τ τ １ 控制滚转运动， ２ 控制偏航运 动， ３ 个分量的表达式如下 τ ３ 控制俯仰运动 。 （ ｌ ω ４ －ω ２） τ １ μ 熿 燄 熿
（ ） １ １
Ｉ Ｉ ｒ ｒ ｋ ｋ １ － （ ） ） ） Ｉ ｉ ａ ０ ω ω －１ －１ ｇ Ｒ ＝ｄ ｉ， ｉ， ∑（ ∑（ Ｉ Ｉ ｘ ｋ＝１ ｘ ｋ＝１ ） ） 和式 （ 即为四旋翼无人机的动力学方程 。 ４ ７ 式 （
４
４
２ 基于时间尺度的内外环分层控制
四旋翼无人机系统 具 有 多 变 量 、 强 耦 合 性， 各个变量 对信号响应速度各不相同 ， 如果将这些变量混合在一起设 需要按照时间尺度（ 响 计控制系统会引起系 统 的 不 稳 定 ， 应快慢程度 ） 将控制系统进行分组设计 。 相应的设计流程 图如图 ２ 所示 。
（ ） ６
２ 燀 燅 燀－ｋ ｋ －ｋ ｋ燅 τ ３ 燀 燅 ω ４
最后可以总结出如下矩阵形式的关系式 Ｉ Ｉ Ω ＝τ－ Ω × （ Ω） Ω ＋Ｉ Ｒ ｆ ｆ 其中
（ ） ７
ｕ １ 熿 燄 Ｕ ＝ ｖ＝ ¨ Ｗ ｕ ２ ｕ 燀 燅 ３
） ） 将式 （ 代入式 （ 可以得到 １ １ １ ０ １ ¨ ０ ｘ ｕ λ １ １ ｃ
０ ｘ ｘ ｃ－ 燄 熿 燄 ２ ¨ ０ ｙ ｕ ２ ＝ ｙ ｃ ＋ ０ λ ｃ－ １ ｙ ＋ ３ ¨ ｚ ｕ ｘ燅 ｚ 燀 燅 燀 燅 燀 ０ ０ λ ３ ｃ燅 燀 ｃ－ １
熿 燄 熿 燄 熿
λ ２ 熿
１
０
０ ｘ ｘ ｃ－ 燄 熿 燄 ０
ｃ－ ｙ ｙ
２ ０ λ ２
（ ） ９ 。将式（ ） 其中λ 一 般 为 对 角 阵） 代 ８ １ 和λ ２ 均为正定矩阵 （ ） 入式 （ 得到实时状态 Ｗ 二阶导表达式 ， 即实时 加 速 度 表 ９
Ｗｅ ＝ Ｗｃ － Ｗ ， 采用 控制 得到误差控制方程为 Ｐ Ｉ Ｄ ¨ Ｗｅ ＋λ Ｗｅ ＝ ０ λ １Ｗｅ ＋ ２
等号左边的变量 Ｗ 是 实 时 更 新 量 ， 这些物理量都可以通 过Ｇ Ｐ Ｓ 测得 。 ３ ． ２ 位置误差控制与姿态控制解算 引入虚拟输入 Ｕ， 满足下列关系
Ｔ 熿 燄 熿μ τ １ τ ２
＝ ０
μ μ ｌ ０ －μ
０
ω １ 熿 燄 μ燄 ２ ｌ ω ２ μ ω
２ ３
２
ｌ －μ
ｌ ０ μ
则可以得到空间位置与速度矢量的关系式 量， ｘ ＝ｖ ｘ
烄 ｙ ＝ｖ ｙ 烅 ｚ ＝ｖ 烆 ｚ
（ ） ２
） ） 和式 （ 即为四旋翼无人机运动学方程 。 １ ２ 式 （ １ ． ２ 动力学方程 在推导四旋翼无人机动力学方程的时候 ， 引入了两个 虚拟 输 入 量， 分别为控制无人机升降的虚拟输入推力 Ｔ 。 以及控制无人机滚转 、 俯仰和偏航的虚拟输入力矩τ 本文中四旋翼 无 人 机 动 力 学 方 程 的 推 导 基 于 如 下 ３ 条假设 ： （ ） 整机模型视为刚体 ； １ （ ） 如 ２ 机体分 别 关 于 Ｏ ｘｆ ｚ ｚ ｙ ｆ ｆ 以及Ｏ ｆ ｆ ｆ 平 面 对 称， ； 图 １ 所示 （ ） 低速状态下忽略气动力 。 ３ 定义 Ｉ Ｉ ω ｒ 表示旋翼惯性矩 ， ｉ 表 ｆ 表示机体惯性张量 ，
２ ２
燄
２ ２ （ （ ） ｌ ５ τ＝ τ ω ２ ＝ μ ３ －ω １） ２ ２ ２ ２ （ 燀 燅 燀 燅 ｋ τ ３ ω ４ ＋ω ２ －ω ３ －ω １ 其中 ｋ 为与空气粘度和 桨 叶 形 状 有 关 的 参 数 ； ｌ为 旋 翼 桨
６ 盘中心到无人机质心 Ｏｆ 的距离 ， 称为臂长 。
Ｔ 标， ｖ ＝（ ｖ ｖ ｖ ｘ， ｚ ） 表示无人 机 在 惯 性 坐 标 系 下 的 速 度 矢 ｙ，
１ 四旋翼无人机非线性模型的建立
在设计控制律之前 ， 首先要对无人机进行建模 。 四旋 翼无人机机 体 呈 现 “ 状 并 且 轴 对 称， 由４个小旋翼组 ＋” 成， 每个旋翼由单 独 的 电 机 驱 动 ， ４个电机按逆时针方向 编号 ， 如图 １ 所示 。
１ １ １
（ Ｓ Ｉ ） ＩＳ－ ｐ （ Ｉ Ｓ－ Ｉ Ｓ－ λ Ｓ Θ Ω－ Ω － ｃ －Θ） ｄ ｆ ｆ 运动的姿态角和推 力 。 将 解 算 出 的 姿 态 角 和 推 力 作 为 内 τ ＝ Ω × ｆΩ ＋ ｆ λ Ｉ 环的输入 ， 通 过 姿 态 角 可 以 解 算 出 控 制 四 旋 翼 无 人 机 滚 ＋ ＲΩ 式中姿态角 及 其 变 化 率 可 以 通 过 相 关 传 感 器 测 量 得 到 。 俯仰以及偏航所 需 力 矩 ， 将力矩和推力作为电机控制 转、 由上述步骤可以得 到 基 于 位 置 坐 标 的 四 旋 翼 无 人 机 飞 行 器的输入 ， 进而控制 电 机 的 转 速 ， 从而实现对无人机飞行 轨迹控制流程图 ， 如图 ３ 所示 。 轨迹的控制 。