ardupilot开发 --- 无人机数学模型与控制律分解 篇

坐标系

• 机体系
  FRD，右手系，原点取在几何中心或质心。
  由于定义欧拉角；
  用于建立动力学模型、运动学模型、动力分配模型；
  是所有方程物理量需要归一化的基准坐标系。
• 导航坐标系
  地固坐标系，NED。
  是欧拉角的参考原点、基准系。
  用于导航与制导。
• 全球系
  定位传感器如GPS所在的坐标系，需要把GPS位置信息转换到导航坐标系NED下。
• 传感器坐标系
  方向与机体系同，原点取传感器几何中心。
  FRD。
  用于卡尔曼滤波、状态估计。

数学模型

运动学模型

基于欧拉角

$$\left[\begin{array}{c}\dot{p_x}\\ \dot{p_y}\\ \dot{p_z}\end{array}\right]=\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]$$
$$\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}1& \sin \varphi \tan \theta & \cos \varphi \tan \theta \\ 0& \cos \varphi & -\sin \varphi \\ 0& \sin \varphi \sec \theta & \cos \varphi \sec \theta \end{array}\right]\left[\begin{array}{c}{w}_x\\ w_y\\ w_z\end{array}\right]$$
其中：

• $[p_x,p_y,p_z],[v_x,v_y,v_z]$分别是导航坐标系NED下的位移矢量和速度矢量。
• $\varphi ,\theta ,\psi$是欧拉角。注意，欧拉角的基准是导航系。
• $w_x,w_y,w_z$是机体系下的角速度，是陀螺仪的测量值。

基于四元素

$$\left[\begin{array}{c}\dot{p_x}\\ \dot{p_y}\\ \dot{p_z}\end{array}\right]=\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]$$
$$\left[\begin{array}{c}{\dot{q}}_0\\ {\dot{q}}_1\\ {\dot{q}}_2\\ {\dot{q}}_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]\otimes \left[\begin{array}{c}0\\ w_x\\ w_y\\ w_z\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}{q}_w& -q_x& -q_y& -q_z\\ q_x& q_w& -q_z& q_y\\ q_y& q_z& q_w& -q_x\\ q_z& -q_y& q_x& q_w\end{array}\right]\left[\begin{array}{c}0\\ {\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]$$
或者把上式拆成${}^{b}w=[w_x,w_y,w_z]$作为输入的形式：

其中：

• $q_0$：标量部分。
• $q_v=[q_1,q_2,q_3{]}^T$：矢量部分。
• $q_0^2+q_1^2+q_2^2+q_3^2=1$。

动力学模型

控制分配模型

动力系统模型

控制律

控制分配