从零开始做手机云台/稳定器

最近完成手机云台/稳定器的产品化，特意记录整个手机云台的开发过程 (内容持续细化中...)

一. 电机驱动篇

硬件描述: 直流无刷电机，7对极，控制板mcu为stm32f0。

驱动方式: FOC 控制，驱动板采样两路电流，并配置有编码器。

网上针对FOC的控制文章较多，做起来也不难，在这里主要记录驱动编写的过程及关键点。

1. 首先是编码器的问题，手头的AS5600 编码器，采用SPI接口，1k HZ的频率读取物理角度，在驱动电机过程发现及其不稳定，有噪音，转动速度越快越不稳定。经过分析数据，终于找到原因：驱动过程相位角在变化，而1KHZ的更新速率（PWM更新频率是30K）无法及时跟随到最新的位置。 解决办法：插值，根据物理角度变化速度计算最新相位角。

2. 控制PWM问题，由于采用7段式SVPWM控制，所以定时器一定要采用中央对齐模式，也即三角波。这方面可以参考：https://wenku.baidu.com/view/9d420682bceb19e8b8f6ba9d.html，说的很详细。还有matlab 仿真。

3. 力矩控制问题，基本流程是，根据采样电流，通过Clarke变化和Park变换（记得是电流变换，不是电压变换），计算得出当前的磁链和力矩（绝对值没意义，相对值才有意义）。这时候可以使用pid反馈控制，计算输出 电压和磁链。然后再经过反Park变换和反Clarke变换就可以得出三相输出电压，然后转PWM波即可。整个过程的关键是PID控制参数的选择，一般都直接采用PI控制即可。参数调整需要实践慢慢实验。

4. 关于电流采样的问题，其实可以不要电流采样，理论上来说依然是FOC 控制。首先,采用电流环控制确实有它的优点，力矩响应肯定要快速，能耗也要低一些（动态），但缺点也明显，控制复杂度增加，电流环的PI控制参数要根据实际电路/电机调整，而且积分容易饱和，电流采样时序也要把握好。其次，在更换电机过程中，电流环控制容易出现各种异常现象，都是电流采样引起的。 由于云台属于慢控制系统，对动态力矩响应要求并不高，所以在实际使用中可以去掉电流环（个人意见）。

处理好以上几个问题，电机驱动基本上就完成了。反正我没遇到其他大问题，至于说电气角度的校准之类的问题都是小问题。顺便说下，之前也试过不用编码器，直接使用两个线性霍尔传感器计算物理角度。遇到几个麻烦问题：1.安装精确度要求高，两个传感器必须垂直90度，误差不能超过该4度，否则力矩明显减少。2. 需要做温度补偿。 3. 磁环位置和强度要求配合。总之效果不怎么好，由于时间关系没有继续采用这种方案做，后面再投入一些时间研究下。

最近需要电机发声（省的安装蜂鸣器了），同时云台需要正常运转，找了一些资料看，都没有好的简单的办法，突然想到，直接再控制力矩上增加高频载波是否可行？写了几行测试代码，果然可以发出不同频率的声音，简单搞定。

二. 云台坐标系篇

根绝云台的结构以及控制情况，整个云台包含三个坐标系：

a. 参考坐标系（地球坐标系），手机根据陀螺仪计算出来的欧拉角就属于该坐标系。

b. 机体坐标系, 陀螺仪传感器输出的角速度属于该坐标系。

c. 电机坐标系，电机物理角度及控制力矩属于该坐标系。

各坐标系之间的关系公式推导：

1. 欧拉角坐标系旋转矩阵

2. 欧拉角变化率与机体坐标系的角速度关系

这里有个地方要注意，由于欧拉角俯仰的范围是正负90度，在俯仰接近90度时横滚值会完全不准。但云台的俯仰姿态大于90度的情况很多，所以解决的办法是在计算欧拉的时候把横滚和俯仰调换位置。在这种情况下，该公式就不适合了，根据坐标映射法，推导出的公式是：

经过matlab仿真，可以使用，注意：该公式只适合Roll在正负60度区间，俯仰可以正负180度。

3. 机体坐标系到电机坐标系

基于以上公式，可以完成云台的所有坐标系转换。

实际在使用过程中发现，俯仰（Y） 的角速度（机体坐标系到电机坐标系）这部分可以不要，否则再某些角度容易引起震动。

三. 控制篇

全部控制采用PID 控制，控制环境主要包括:

1. PID 速度环

姿态err = 目标姿态 - 当前姿态

目标速度_e = 姿态err×Kp

整个速度环PID参数，积分和微分参数设置为0即可。

2. PID力矩控制环

目标速度 _b = 参考坐标系转机体坐标系(目标速度 _e)

速度Err_b = 目标速度 _b - 陀螺仪角速度

速度Err_m = 机体坐标系转电机坐标系(速度Err_b)

输出力矩 = PID(速度Err_m)

3. 横拍竖拍模式

竖拍模式，即手机竖直拍摄，但横滚电机角度到达限定值，云台就直接切换为竖拍模式，在竖拍模式下，手机实际姿态是：横滚90度（或负90度），俯仰正负45度。

4. 航向跟随模式

实现方式很简单：

a. 确定手柄姿态（是否手电筒模式），确定控制航向是原航向电机还是横滚电机

b. 计算电机当前位置与目标位置err， 目标航向 = 当前航向 + err*KP

四. 通信篇

1. 三轴间通信

三轴间通信结构图:

俯仰为主机，横滚和航向为从机，通信速率为1M,主机没500us 发起一次通信。主机发送控制命令，从机将电机位置，控制板状态等信息作为返回。

2.  云台对外接口通讯

由于俯仰作为云台主控端没有直接对外提供串口（不是没有，是硬件不方面，接线麻烦），所以只能通过航向板转发了，通讯又不能影响原本三轴通讯，所以采用的办法是再原来三轴通讯协议上附带转发数据，这样不影响原有流程正常运行。实际运行效果还不错，在不影响云台正常运行的情况下，可以通过PC软件直接设置参数，查看运行曲线。