无人机基础扫盲：从坐标系到自主悬停控制
无人机基础扫盲：从坐标系到自主悬停控制
无人机技术近年来在消费级市场和工业级应用中快速发展，使得越来越多的人对无人机技术产生了兴趣。本文将从坐标系定义、基本控制原理、飞行姿态表示方法以及自主悬停控制等方面，为大家介绍无人机的基础知识。
坐标系定义
飞行器的坐标系参考的是当地水平坐标系，即地理坐标系（经纬度）。与自身相互连接的叫做载体坐标系。通常把X轴作为飞行器的前方。针对飞行器主要是三个角度方面的数值：翻滚角 Roll（记作ϕ）、俯仰角 Pitch（记作θ）和航向角 Yaw（记作ψ）。
- 俯仰角θ（pitch）：为载体绕着Y轴旋转的角度
- 偏航角ψ（yaw）：围绕Z轴旋转的角度
- 滚转角Φ（roll）：围绕X轴旋转的角度
姿态解算引入：导航坐标系是地理坐标系，作为基准坐标系，用于描述地球表面上任意点的位置。载体坐标系是相对于载体（飞机）的局部坐标系，用于描述载体内部的位置和方向。
在求解载体航行姿态时，需要将载体坐标系内测量到的数据转换到地理坐标系中，然后进行姿态解算。这是因为地理坐标系是一个固定的参考坐标系，而载体坐标系随着载体的运动而变化。
在载体坐标系中，载体坐标系的原点Ob位于载体的质心或中心，Xb轴指向载体水平方向向前，Yb轴指向朝着载体正前方看的左侧方向，Zb轴垂直于XY平面沿载体竖轴向上，满足右手法则。
因此，为了进行姿态解算，需要先将载体坐标系内测量到的数据转换到地理坐标系中，以便在地理坐标系中进行准确的姿态分析和计算。这样可以保证姿态解算的准确性和可靠性。
无人机基本控制
电机布局：四轴飞行器的四个电机呈十字形排列，驱动四片桨旋转产生向上的推力。由于四个电机的轴距几何中心的距离相等（即电机中心之间的距离），所以当对角两个轴产生的升力相同时能够保证力矩的平衡，四轴不会向任何一个方向倾转。而当四个电机一对正转一对反转时，可使得绕竖直轴方向旋转的反扭矩平衡，保证了四轴航向的稳定。
动力学原理：通过控制四个电机的转速，控制升力，力矩来进行控制。具体如图，箭头代表升力变化。
飞行姿态以及表示方法
欧拉角转换
欧拉角转换是用来描述刚体在三维空间的指向。在三维空间里面的参考系，坐标取向都可以用三个角来表示。章动角θ、进动角ψ 和自转角φ。章动角θ是描述刚体绕固定点转动的角度，进动角ψ是描述刚体绕第二个固定点转动的角度，自转角φ是描述刚体绕自身轴线的旋转角度。这三个角参量合称为欧拉角，是用来确定定点转动刚体位置的三个一组独立角参量。用一个图来直观的显示这三个角所对应的转换。
地理坐标系0XnYnZn与载体坐标系XbYbZb之间的坐标变换矩阵即为方向余弦矩阵可以通过三次基本旋转得到。对应的矩阵变换为：
使用欧拉角微分方程可以方便的对四轴飞行器姿态进行解算：
上面公式中左侧是更新后的欧拉角，右侧是上个周期测算出来的角度，而三个角速度则由安装在四轴飞行器上的三轴陀螺仪测得。因此，求解这个微分方程就能解算出当前的欧拉角。
欧拉微分方程的好处是可以直观地看出来物体状态的变换。但是有两个缺点：第一个是大量的三角函数运算，对于实时解算的要求比较高；第二点是万向节死锁的问题出现。
万向节中间有一根竖轴，穿过一个金属圆盘。金属圆盘称为转子，竖轴称为旋转轴。转子用金属制成，竖轴外侧是三层嵌套的圆环，它们互相交叉，带来了三个方向自由度的旋转。
他的工作方式和欧拉角一样。
桶滚:
俯仰:
偏航:
在小范围的Pitch Yaw Roll的情况下万向节都可以通过自身调节保持平衡。
但是在剧烈的角度变换下，万向节出现死锁的情况，无法自动调节：
此时在进行调节，发现失去了自身的调节能力：
这就是死锁。滚筒的自由度丧失，无法恢复原状。这个时候可以通过修改优先级来避免这个死锁，即：Y>X>Z的情况。（优先级：Y轴可以带动X,Z轴去旋转 .X轴可以带动Z轴去旋转）去避免上述的死锁。但是这属于拆东墙补西墙，这样子的优先级还会带来新的死锁。（想要了解的可以去哔哩哔哩自行了解）
所以，结合以上，欧拉角只适用于水平姿态变化不大的情况，不适用于全姿态的飞行器姿态锁定。
四元数法
四元数是复数的不可交换延伸，如把四元数的集合考虑成多维实数空间的话，四元数就代表着一个四维空间，相对于复数为二维空间。如果三维空间里的一个点的笛卡尔坐标为 (x,y,z)，则它用纯四元数（类似于纯虚数，即实部为0的四元数）xi+yj+zk 表示
数学形式：q=q0+q1i+q2j+q3k
四元数的集合考虑成多维实数空间的话，四元数就代表着一个四维空间，相对于复数为二维空间。
四元数是简单的超复数。复数是由实数加上虚数单位i组成，是由实数加上三个虚数单位i、j、k组成，而且它们有如下的关系：每个四元数都是i、j、k的线性组合，即四元数可用下面的公式进行表示，其中q0、q1、q2和q3都是实数，ω是转动的角度，n为旋转轴。
采用四元数表示姿态变换时，由四元数的运算法则，将其中的四元数按照元素展开并按照运算符法则进行计算可以得到四元数表示的方向余弦矩阵：
最后综合上述，可以得到欧拉角和四元数之间的转换关系：
相对于另几种旋转表示法（矩阵，欧拉角，轴角），四元数具有某些方面的优势，如速度更快、提供平滑插值、有效避免万向锁问题、存储空间较小等等。所以基本上是使用四元数来进行模型的构建。
动力学模型建立方法
无人机的姿态信号主要是姿态角。俯仰角，偏航角，滚转角。测试姿态的主要器件是陀螺仪，角速度计与磁力计。
加速度计：加速度直接测量的是重力矢量在机体坐标系三个轴上的分量，由于有绝对的参照物即重力轴，因此在无外力的加速的情况下，能够准确测量出俯仰角和滚转角，并且不会存在累积误差。虽然经过低通滤波器滤除了大部分由于电机振动而引入的噪声信号，但是当飞行器在三维空间做变速运动时，加速度传感器同样会检测到变速运动的加速度信号，从而导致姿态角的解算出现偏差。
陀螺仪：陀螺仪测量飞行器绕机体坐标系三个轴的旋转角速度，通过对角速度的积分进而得到旋转角。在给定初始旋转角的前提下，可以测量出飞行器的俯仰、偏航和滚转角。工作原理决定了其测量值是相对某个惯性空间的，不需要外部的参照物。因此陀螺仪数据只在较短的时间内是有意义的。即使经过滤波后的陀螺仪信号噪声很小，但随着时间的增加，其积分误差也将逐渐增大，导致测量出的姿态角出现误差。
磁强计：磁力计直接测量的是地磁场强度矢量在机体坐标系三个轴上的分量，与加速度计一样也有绝对的参照物地磁场，因此在水平面检测时可以准确检测地磁场方向用于飞行器偏航角的解算，但当飞行器倾斜时，其计算结果就需要根据倾斜角进行相应的补偿。
以上三个传感器的输出信号经过滤波、融合等处理后便可以得到俯仰角、滚转角和偏航角信息。
大体的姿态借宿的流程如下：姿态解算的核心在于旋转，旋转共有4种表示方式：矩阵、欧拉角、轴角和四元数。其中矩阵适合变换向量，欧拉角最直观，轴角则适合几何推导，而在组合旋转方面，四元数表示最佳。因为姿态解算需要频繁组合旋转并用旋转变换向量，所以应采用四元数来保存飞行器的姿态（地理坐标系中的俯仰/翻滚/航向角）。下图是姿态解算的整个流程：
自主悬停&高度融合探究
无人机在空中是一个三维(x,y,z)坐标的位置，涉及一个是水平面上的悬停，还有一个是垂直方向上的悬停。
在水平面上，一般使用GPS（室外）光流传感器（室内）进行定位（这两个传感器我到时候会在整理）。
在竖直面上，一般使用气压计进行定位，或者是激光测距，超声波测距。
具体流程如下：在垂直层面，通过气压传感器采集的大气压值计算出来，将气压传感器采集值进行校正后，再通过温度二阶补偿，得到准确的大气压值，最后经过气压转换高度公式就可以得到一个相对于起飞地点的绝对高度。因为气压计有误差，因此需要融合加速度计互补滤波得到比较合适的高度值，Z轴速度值和加速度值。用高度作外环，速度作内环形成高度双环PID控制器，调节输出油门实现Z轴的自主悬停。以下给大家分享一下大体的控制逻辑，以便大家更好的理解整个控制逻辑。
Z轴悬停的基本值：由于电池电压的不同，所以在针对于不同电压之下的油门基本值也要不相同，所以这个需要针对于不同的电机和电压测试出合适的电压的基准值，如果选取不当或者系统参数发生变化时，就会出现往上飞或者掉高的问题。与此同时，还需要设置最小油门值，避免无人机下降过快的问题。避免电压不同，飞机动力不同，所以还要将输出和电池电压挂钩，避免无法受控的问题。