这个项目做了很长的时间，核心算法就是PID，目标就是让一个三轮车（轮子为全向轮）站到足球上并可以进行平移旋转等平面运动。
使用三个橡胶全向轮，通过联轴器连接减速电机，斜向下45°连接于车身，车身上承载多层台面，下层放置电路板及电池，上层作为承物平台，选用摩擦力大的5号橡胶足球作为摩擦球，如图1所示：
板与电机的连接部分采用3D打印方案，3D打印精度可达0.1mm，完全满足需要，3D模型如图2：
如图3所示，通过433MHz的无线串口透传模块，将控制指令传送至单片机，单片机通过IIC接口获取当前姿态信息，通过自定义的并口获取当前速度信息。在控制器中完成角度闭环和速度闭环。通过PWM将给电机的指令信号传给FPGA，FPGA对每个电机分别做速度闭环，同时将速度信息回传至MCU。
两个方向的独立控制方程可以看作两个一维倒立摆，倒立摆模型如图4：
采用动力学分析建模，最终经过线性化和拉普拉斯变换后的两个传递函数分别如下所示：
式中各符号的含义如下：
Ib、Ip | 惯性力矩 |
mp | 机器人主体质量 |
mb | 球体质量 |
r | 球体直径 |
h | 摆杆质心到球心的长度 |
θ | 摆杆倾角 |
ψ | 摆杆与球的相对角度 |
M | 作用在球上的力矩 |
g | 重力加速度 |
单球轮平衡机器人的控制问题本质上是对三个全向轮的控制问题，将机器人模型分为三个坐标系x、y、z，如图5所示。
通过在x轴、y轴上安装的加速度计和陀螺仪来检测两个轴上的角度和加速度变化，进而计算出等效的两个虚拟轮上的速度vx 、vy ， vx、vy 由相应的加速度ax 、ay 积分求得，ax 、ay 分别如下式所示：
由虚拟轮的速度 vx、 vy最终推导出三个全向轮的转速vs2 、vs2 、vs3 分别如下式所示：
对于控制器来说，需要同时进行角度闭环和速度闭环，来增加小车稳定时的状态同时增加阻尼。为了使后端的功放环节更像一个线性的放大环节，减少非线性，我们加入FPGA使后端电机驱动部分自成一个速度闭环，使单片机可以获得更为准确的速度反馈。
单片机从传感器同时获得角度和角速度，通过增量式PID进行角度反馈，输出PWM，同时读入FPGA返回的速度值，控制小车不发生较为严重的振荡。
如图7为控制框图：
如图8，单片机上电之后先进行系统初始化，并读取参数，然后从测量元件获得测量数据，通过其中加速度计和陀螺仪的数据，输入融合算法可确定小车的姿态，结合由编码器得到的FPGA速度值以及期望数值计算得到小车的速度输出。值得再次说明的是，此系统中的速度闭环是由控制器（单片机）实现的，而FPGA中速度反馈的功能是提高电机的线性度。
三路PWM速度信号由单片机输入FPGA，FPGA由编码器的速度反馈信号及输入的期望速度进行PID控制算法后将控制信号以PWM的形式输入电机驱动，并将得到的速度反馈值传给单片机。
控制器的硬件电路的设计，必须首先决定主控制器采用的芯片，然后将控制算法编制成相应的控制程序，实现对被控对象的控制。究竟选择哪一种单片机，取决于系统对速度的要求和该应用系统的复杂性，在本设计中选用通用数字单片机，其它器件的选择遵循控制系统的精度要求，同时满足体积小、耗电少、可靠性高的要求，因此尽可能选用功耗小，性能稳定的集成电路芯片。
在众多单片机产品中，我们选择了Atmel公司的AVR单片机，该单片机具有编程简单，算法库和驱动库众多，方便快速开发，同时该单片机为8位单片机，主时钟16MHz，运算速度不算太快，但可以满足50Hz-100Hz的系统控制频率，该单片机具有片上8通道10位ADC，6个硬件PWM，一路I2C，一路串口，一路SPI接口，接口齐全，能满足各类传感器的需求。是一款通用性极佳，开发极容易的单片机。如图10为单片机最小系统电路。
为了实现3路电机的各自速度闭环，单片机没有足够的资源可以使用，而且速度不够，我们选用了Xlinx公司的Spartan6系列的FPGA XC6SLX9，用来做并行的3路闭环辅助处理器，单片机通过6路PWM将信号传给FPGA,每个电机用两路PWM来区别转向，同时FPGA每个电机另外产生两路PWM来驱动电机，同时FPGA捕获每个电机编码器的的两路正交信号进行速度反馈。
我们需要测得小车精确的角度变化量，及时修正来保证小车的相对稳定性。常见的测量角度的仪器有陀螺仪和加速度计。
陀螺仪可以测量角速度,具有高动态特性,但是它是一个间接测量器件,它测量的是角度的导数，即角速度，显然我们要将角速度对时间积分才能得到角度。积分会存在积分误差，积分误差的来源主要有两个：一个是积分时间。积分时间Dt越小，输出角度越准；一个是器件本身的误差，假设陀螺仪固定不动，理想角速度值是0dps(degreeper second)，但是如果有一个偏置0.1dps加在上面,测量出来则是0.1dps,积分一秒之后,得到的角度是0.1度,1分钟之后是6度,一小时之后是360度,转了一圈。所以说陀螺仪在短时间内有很大的参考价值。
加速度计可以测量加速度,包括重力加速度。但是加速度传感器不会区分重力加速度与外力加速度，所以当系统在三维空间做变速运动时，它的输出就不正确了或者说它的输出不能表明物体的姿态和运动状态。在静止或匀速运动（匀速直线运动）的时候,加速度计仅仅测量的是重力加速度,而重力加速度与绝对坐标系是固连的,通过这种关系,可以得到加速度计所在平面与地面的角度关系也就是横滚角和俯仰角，计算公示如下：
俯仰角ρ=-arctan(AY/AZ)
横滚角ψ=arctan(AX/AZ)
综合考虑，加速度计是极易受外部干扰的传感器，但是测量值随时间的变化相对较小。陀螺仪可以积分得到角度关系，动态性能好，受外部干扰小，但测量值随时间变化比较大。可以看出，它们优缺点互补，结合起来才能有好的效果。我们可以设计算法在短时间尺度内增加陀螺仪的权值，在更长时间尺度内增加加速度权值，这样系统输出角度就更真实了。
选择MPU6050传感器，该芯片集成3轴陀螺仪和3轴加速度计，陀螺仪量程可选择±250,±500,±1000,±2000°/s，加速度计量程可选择±2g, ±4g,±8g, ±16g。且输出增加低通滤波，解决温度漂移问题，更有利于单片机信号采集。该传感器的ADC分辨率均为16位，对单片机的数据接口为I2C，工作频率最高为400kHz。
我们将传感器配置在±250°/s，±2g，即可满足我们的要求，同时可以达到较高精度。因为系统在稳定的时候晃动不超过5°，晃动频率不超过1Hz，所以选用最低量程完全满足我们的要求，且量程越低精度越高。控制频率在50Hz-100Hz，传感器灵敏度可以满足我们的需求。在控制器内，我们以400kHz的频率获取原始数据，进行卡尔曼滤波数据融合获取3轴角度，该数据和3轴角速度以控制频率频率被控制器使用。另外为了减轻处理器负担，我们可以使用MPU6050内部的DMP运算内核，该内核基于DSP，内部固化官方的滤波算法，可以在芯片内FIFO中直接读取四元数，角速度等值，同时DMP内核具有一定的抑制零漂算法。
在使用时为了抑制零漂，我们采用在设备上加装水平尺的方式，每次上电前十秒采用人工校正来保证在本次工作中系统传感器的数据真实度。
如图为MPU6050部分原理图：
采用磁强计让小车工作时可以得知自己的绝对朝向，同时方便陀螺仪进行Z轴校准，以此获得系统相对于地球的绝对方位角，传感器精度要求不高，采用HMC5883传感器。该传感器的特点和优点如下：
特点 | 优点 |
三轴磁阻传感器和ASIC都被封装在3.0×3.0×0.9mmLCC表面装配中 | 体积小高集成产品。只需添加一个微处理器接口，外加两个外部SMT电容。专为大批量、成本敏感的OEM生产而设计，易于装配并与高速SMT装备件兼容。 |
12-bit ADC与低干扰AMR传感器，能在±8高斯的磁场中实现5毫高斯分辨率。 | 能让罗盘航向精度精确到1°~2°。 |
内置自检功能。 | 产品组装后能进行低成本功能性测试。 |
低电压工作（2.16V—3.6V）和超低功耗（100uA）。 | 适用于电池供电的应用场合。 |
内置驱动电路。 | 带置位/复位和偏置驱动器用于消磁、自测和偏移补偿。 |
I2C数字接口 | 适用于消费类电子设备应用中通用双线串行数据接口 |
无引线封装结构 | 符合RoHS标准 |
磁场范围广（+/-80e） | 传感器能在强磁场环境中罗盘航向精度达到1°~2°。 |
有相应软件及算法支持 | 可获得罗盘航向、硬磁、软磁以及自动校准库。 |
最大输出频率可达160Hz | 能应用于个人导航系统和LBS |
该传感器使用简单的I2C数字接口，内置12位ADC。最大输出频率为160Hz，高于控制频率，比较合适。在PCB布线安装的时候要远离电机，防止被电机磁场干扰。回流焊要求温度不能高于260℃，否则会损伤芯片内部结构，这一点要特别注意。如图为HMC5883部分原理图：
光电编码器
采用光电编码器，该编码器与电机一体，做在了一个轴上，电机本身减速比64:1，码盘为12CPR，则测角精度为360/(12*64)=0.468°，同时该光电编码器为AB相输出。编码器要注意供电问题，小心烧掉光电管,如图13，电源加入保护电阻，输出增加上拉电阻。
电机 | 类型 | 参数 | 特点 |
1 | Faulhaber2342L012空心杯减速电机带编码器 | 输出功率：17W 扭矩：大扭矩(1.72Nm) 空载电流：75mA 堵转电流：1400mA 减速箱型号：日本定制（全金属精密行星减速箱） 减速比：64：1 编码器：光电式 输出路数：ＡＢ双路输出 每圈脉冲：12CPR（脉冲每圈）工作电压: 12V 空载转速：8100RPM（转每分钟） 减速后速：120RPM（转每分钟）
| 减速组 自带编码器，有虚位 力矩大 |
2 | 42步进电机 | 重量：0.4kg 步距角：1.8±5％度/步 电压：12V 电流：1.2A/phase的 阻力位：10.0±10％Ω/相 电感值：20±20％MH/相位 铅风格：AWG26 UL1007 配备4引线 保持转矩：48N.cm敏 制动扭矩：2.6N.cm最大 转子的力矩：68g.cm2 | 便宜，力矩大，无需速度闭环，没有虚位 |
3 | 无刷电机+电调 | 1400kv 30A | 力矩大，无需做闭环 |
最终我们选择了Faulhaber2342L012空心杯减速电机带编码器，看中其功率大，驱动简单，自带较高精度编码器减少机械设计难度，如图14：
电机最大功率17W，最大工作电压15V，可以估算出每个电机的工作电流不超过2A，选择L298N芯片作为电机驱动。该芯片可以驱动两路电机，最高工作电压35V，稳态工作电流2A，峰值工作电流3A，可以满足功率需要。输入信号为两路PWM，可控制电机的正反转，PWM信号频率约为40kHz。该芯片在使用时要特别注意散热，芯片发热量很大，并且板上布线线宽至少40mil，远离数字芯片。如图15为驱动电路的原理图：
图16轮胎采用双层橡胶全向轮，3个轮呈120度排布，同时向下倾斜45度，以便与球面全面贴合。我们最终选择了如图全向轮，直径7cm，六方形联轴器。
采用5号橡胶足球，与橡胶轮胎的摩擦较大，球与轮胎摩擦系数约为0.5。原则上尽量增加摩擦力。同时保证轮子可以全部与球接触
每个电机耗电峰值达到2A，整个系统考虑峰值要留出冗余，确定放电能力6.5A，电机工作电压在14V左右，电压越高，电机工作特性越好，故采用3000mah，14.8V，4s，35C放电能力的锂电池，估算最大放电电流可达9A，用来保证电机的供电。
控制器，传感器的供电电压为5V和3.3V，FPGA部分供电为3.3V和1.2V，控制部分功率较小，采用7805三端稳压芯片将电池电压降至5V，后接1117-3.3将其降压至3.3V，后接1117-1.2，将其降压至1.2V。由于7805压降较大，发热量较大，需要增加散热片以确保稳压芯片的正常工作。电源部分原理图如图17所示：
主控部分代码可参照:https://github.com/BG2CRW/ballBalacne
