无人机|四旋翼运动动力学建模及位置控制仿真

前言

本文将实现对无人机动力学以及运动学的公式推导完成建模，该模型以电机转速为输入，以无人机的状态量为输出。并在此基础上实现位置控制，以期望位置作为输入，使用串级pid结合无人机模型生成控制指令并对无人机进行控制。

无人机模型推导

对于任意刚体运动，均可分解为转动和平动，其中转动由动力学模型进行分析，平动可由运动学模型进行分析。对于无人机的糕体运动，如下示意

其中合力以及合力锯均由电机带动浆的转动产生
在忽略阻力、保证无人机小角度飞行的情况下，对无人机进行建模
在无人机机体系下，得到合力以及合力矩

动力学模型

带入并化简得

定义姿态角为机体系按ZYX旋转与惯性系平行时绕各轴的旋转角度

求解旋转矩阵并化简


由此求解得到无人机动力学模型

运动学模型

在不考虑外部扰动的情况下，加速度积分为速度，速度积分为位移，此时无人机的运动学模型为简单二阶模型。

无人机模型仿真

由上面推到过程可以得到，无人机模型主要分为电机转速-力及力矩、力及力矩-角加速度、角加速度-角速度、角速度-姿态角速度、姿态角速度-姿态角、姿态角-加速度、加速度-速度、速度-位置等环节，其中位置、速度、角速度、姿态角、角速度为无人机模型常用状态量，通过对这些状态量进行反馈实现对无人机的闭环控制
整体架构如下

各模块均用MATLAB Fuction模块进行实现。

控制器设计

本仿真使用简单的串级PID进行无人机位置控制，并将无人机模型简化为输入为合理及力矩的对象，集成为一个标明输入输出的模块，如

即以力和力矩作为无人机模型的输入，以15个状态量作为输出，并分别反馈至位置速度控制器以及姿态控制器。整体框架如

其中位置-速度为纯p控制，速度-加速度使用PID控制，加速度-姿态角需符合动力学方程。期望速度-期望姿态角的框架如

再由期望姿态角根据模型算得期望力矩，并将力矩作为控制量输入到无人机模型。由此完成无人机的位置控制。
初步得到无人机位置的闭环控制

根据仿真结果对控制器参数进行调整。