动力学和姿态数学模型(控制算法)

INS
GPS
• GPS，全球卫星定位系统
• 为全球或附近任何地点的用户提供精确三 维位置和速度数据 • 瞬时准确，误差增长受限制，数据率低
高度表
• 高度表
• 测量高度，满足受控空域的高度精度标准，限制 惯性导航系统垂直通道中误差的增长 • 气压计，精度1m • 超声波测距仪，测量范围4cm到500cm 深圳大学轨道交通学院
Βιβλιοθήκη Baidu
数据融合

深圳大学轨道交通学院
加速度计、数字罗盘解算姿态—观测向量

速度计可以测得重力加速度在载体坐标系X轴、Y轴上 的分量 A、 ，根据几何关系得到 Ay x
arcsin Ay / g arcsin Ax / g

偏航角 通过数字罗盘得到
YH XH Y cos Z sin arctan X cos Y sin sin Z cos sin
Kd ( U 4 K afz ) / I z Kl


输出量 输入量 控制俯仰
U1 控制三个位置x，y，z； , U 4 控制横滚角； ,U 3
角； ,U 2 控制偏航角 深圳大学轨道交通学院
捷联惯性导航—姿态解算

硬件


加速度计、陀螺仪、数字罗盘 加速度计输出基于载体坐标系的加速度，数字罗盘输出基 于载体坐标系的磁场强度，二者结合可解算出三轴姿态角 （横滚、俯仰、偏航） 陀螺输出三个轴向的角速度，积分运算得到载体的姿态角 低成本固态陀螺的性能较差，能保证短时间内的测量精度， 响应速度快，但长时间产生漂移 采用加速度计和数字罗盘联合输出值数据稳定，补偿陀螺 仪的漂移
四旋翼无人直升机
——基本数学模型和原理
深圳大学轨道交通学院
内容

四旋翼无人直升机背景及意义 四旋翼无人直升机模型 姿态解算与导航
深圳大学轨道交通学院
背景

有人驾驶机-》无人驾驶机，固定翼飞机-》旋翼式飞机 四旋翼 结构特征

四个旋翼呈十字交叉结构推进； 飞行动作完全由四个螺旋桨的转速决定
+ vk +
Kk
选取状态矢量
+
xk
Ck
Xk|k-1
Ak-1
delay
卡尔曼滤波算法就是预测和修正的不断 递归。预测新的状态和协方差，然后利 用观测向量对状态和协方差进行修正。 深圳大学轨道交通学院
INS-GPS组合导航
• INS，惯性导航系统
• 测量飞行器的加速度(惯性)并进行积分运 算，获得飞行器瞬时速度和瞬时位置 • 数据率高，噪声低，会随时间偏移
( dU 2 K afx ) / I x (dU 3 K afy ) / I y
Kd ( U 4 K afz ) / I z Kl
深圳大学轨道交通学院
传递函数矩阵

选择后四行进行控制
z (U1 K dtz y mg ) / m
(dU 2 K afx ) / I x (dU 3 K afy ) / I y
模型简化

Fi的组合简化
Z方向拉力 滚转俯仰力 滚转俯仰力 偏航力

简化三角函数

忽略二次项相乘，认为是高阶小项
深圳大学轨道交通学院
模型简化
将 U1-U4 代入力和力矩平衡方程，得到：
x ( U1 K dtx x ) / m y (U1 K dty y ) / m z (U1 K dtz y mg ) / m
四旋翼结构图
深圳大学轨道交通学院
四旋翼直升机模型—平动动力学模型

平动动力学模型
m Ff Kdt mG
m-四旋翼直升机量
-直升机平位置， x y z
G -重力加速度，G 0 0 g Ff -直升机四螺旋升力 K dt -平动拖拽力系数
深圳大学轨道交通学院
* arctan
深圳大学轨道交通学院
陀螺仪解算姿态—状态向量
陀螺仪通过单轴积分法获得三轴姿态角
y k ( x b)
• • •
x为陀螺仪输出电压， y为旋转角速度 k为输出电压到旋转角速度的比例系数 b为陀螺仪输出电压零点。
深圳大学轨道交通学院
多传感器融合—卡尔曼滤波
计算观测向量
四旋翼直升机模型—平动动力学模型

其中

Fi—螺旋桨i产生的升力 其中，

R 为从地面坐标系到载体坐标系的坐标转换矩阵
深圳大学轨道交通学院
四旋翼直升机模型建立

平动动力学模型

转动动力学模型
深圳大学轨道交通学院
四旋翼直升机非线性运动方程
力和力矩平衡方程（四个输入6个输出，欠驱动系统）
深圳大学轨道交通学院
四 旋 翼 实 物 图 四旋翼结构图
深圳大学轨道交通学院
意义

优势

螺旋桨小，飞行安全 结构简单，控制灵活 军用：侦查、监视、诱饵、通信中继 民用：大气监测、交通监控、森林防火 基于惯性导航的自主飞行控制 基于视觉导航的自主飞行控制 多智能体群控制

用途



国际四旋翼无人直升机研究方向