独轮机器人姿态控制与惯性导航的设计与实现

李文良，王子豪，徐媛媛，唐笠雄，颜斌，陈龙

（杭州电子科技大学 电子信息学院，浙江杭州，310018）

0 引言

独轮车机器人是一种依靠单轮接地平衡的非稳定性系统，比起两轮平衡车自平衡系统，虽然独轮机器人自平衡系统的结构精简，将地面接触面积降到最小，但也因此需要通过多维的动态平衡控制来实现系统达到自平衡状态[3]。而独轮机器人系统概念的提出，必然引出很多难题：如系统的结构设计、动力学建模和姿态控制等，进一步引起姿态更新算法与控制策略趋于多维模型的创新和变革。

截至目前，研究关于独轮机器人的系统方面的论文百花齐放，深入到独轮机器人系统的方方面面，例如：动力学建模与控制策略[4~6]、姿态更新算法的创新与延伸[7~11]等等。本文则是提出一种可行性高、响应速度快、抗干扰性强以及导航精准度高的独轮机器人姿态控制与惯性导航的实现方法。

1 系统总体方案设计

系统总体设计框图如图1 所示。实际车模图如图2 所示。在系统硬件设计上，独轮机器人系统的核心控制器使用英飞凌公司的TC264 单片机，将其作为主控模板。姿态传感器模块则选用九轴传感器IMU963RA，电机控制模块由直流电机模块和无刷电机模块组成，速度传感器模块选用1024 线霍尔编码器，此外还在主板上增设了许多配件模块。

图1 系统总体设计框图

图2 实际车模图

而在系统软件设计上，主要功能实现使用四元数融合滤波解算姿态角，姿态控制选用串级PID 控制，采用简略捷联式惯性导航确定位置坐标。在调试过程中的辅助功能：使用OLED 模块与按键模块相配合实现在OLED 屏幕上显示参数和参数调整功能；使用UART 串口实时传输滤波数据到电脑上的匿名上位机进行调试；使用SD 卡模块存储独轮机器人调试时的参数。

2 系统硬件电路设计

2.1 硬件电路总体设计

硬件总体设计采用TC264 为运算核心的设计方案，整套硬件系统可分为三个模块，分别为主控模块、电机驱动模块、电源模块。

2.2 主控模块设计

主控模块的设计采用TC264 单片机，由于此单片机功率较大，在电源模式选择中选用外部3.3V、内部LDO1.3V电源模式供电[12]。主控核心原理图如图3 所示。

图3 主控核心原理图

2.3 电机模块设计

电机驱动电路一般采用驱动芯片和MOS 管组合搭建H桥，对于行进轮有刷直流电机我们采用两片全桥驱动芯片BTN8962，该芯片虽然导通内阻较大发热较严重、死区时间较长，但鉴于独轮机器人速度较慢，使用BTN8962 完全足够。有刷电机驱动电路如图4 所示。

图4 有刷电机驱动电路图

平衡飞轮则使用直流无刷电机驱动。由于电机自带驱动电路，所以只需接线即可。直流无刷电机驱动原理如图5所示。

图5 直流无刷电机驱动原理图图

2.4 电源模块设计

电源模块主要分成三个部分：主板电源模块、3.8V 电源转换、3.3V 电源转换。

主板电源模块为主板提供所需要的各种电源，主电源由锂电池提供，此电池提供的电压经测量范围为22.2V~25.6V，首先需要对24V 进电电源进行滤波。同时，对电压大小进行检测，实时充电。电源原理如图6 所示。

图6 电源原理图

3.8 V 电源由24V 电源转换得到，采用TPS5430 开关电源进行转换，MP1584 电压输入范围4.5V~28V，最大载流可达3A，开关频率可达1.5MHZ，效率很高，虽然是开关电源，在合理选择参数和布局的情况下输出纹波可以控制在50mV 以下。24V 转3.8V 原理如图7 所示。

图7 24V 转3.8V 原理图

3.3 V 电源由AMS1117 转换得到，作为一款经典的线性稳压芯片，AMS1117 具有输出电压稳定、电源纹波小、电路简单的特点，实际运用中采用AMS1117 为TC264 供电来减少电源纹波的干扰。AMS1117转换3.3V原理如图8所示。

图8 AMS1117 转换3.3V 原理图

3 系统软件设计

3.1 软件流程设计

系统软件部分主要由：串级PID 平衡控制、四元数姿态解算、简略捷联式惯性导航系统这三个部分组成。独轮机器人系统在串级PID 平衡的控制下可以保持直立，在惯性导航确定目标点后，可以通过速度环以动态速度到达目标点位，还能够使用路径规划，规划不同的路径到达。

3.2 串级PID 控制

串级PID 控制由三个大环组成：Pitch 直立环、Yaw 转向环、Roll 直立环；其中Pitch 直立环用于前后姿态控制；Roll 直立环用于左右姿态控制；Yaw 转向环用于独轮机器人的转向与压弯调控。总体串级PID 控制结构设计如图9所示。

图9 总体串级PID 控制结构设计图

串级PID 控制是基于PID 控制器，将可观测到的多个输入量全部用于统筹控制输出量。如图10 所示就是以Pitch 直立环为例子演示串级系统的构成与应用：将可观测到的速度（编码器）、角度（四元数姿态解算）、角速度（陀螺仪）用于控制电机实现控速、平衡、抗干扰性强、响应速度快、纹波小等功能。

图10 单环串级PID 流程图

其中角度环与角速度环采用非线性PD 控制[13]，速度环采用积分分离PID 控制[14]。正常形式的PID 的原理如公式(1)所示。

式中：Kp为比例时间系数；Ki为积分时间系数；KD为微分时间系数。

而非线性PD 控制的原理如公式(2)所示。

式中：Kp为比例时间系数；Ki为积分时间系数。

对比正常形式的PID 算法，在串级系统中内环去除不必要的积分时间系数，减小积分累积带来不必要的波纹以及干扰，以便于系统迅速到达并且稳定在期望值附近。

3.3 四元数姿态解算

在本文中四元数姿态解算采用madgwick算法，也被称为AHRS(姿态和航向参考系统)，包含方向估计算法，提供相对于重力和地球磁场方向的完整方向测量将不同的传感器数据融合成最优的方向角度估计[15]。

如图11~图13 则是四元数姿态解算的代码应用实际效果。

图11 Pitch 姿态角对比示意图

由图11 可知：解算出的Pitch 姿态角响应速度快，并且精准度高，抗抖性强。

由图12 可知：采样原始加速度计数据的毛刺多，受环境影响较大；而在解算出的Pitch 姿态角中几乎不含毛刺。

图12 Pitch 姿态角毛刺示意图

由图13 可知：Yaw的漂移非常微小，实测数据：大约30 分钟才会造成1°的偏差。如果再加上三轴磁力计数据后就会如同Ya*w，会根据三轴磁力计数据进行自我矫正漂移，在没有外来磁场干扰采样地磁场时，可以做到Ya*w几乎不存在漂移。

图13 六轴Yaw 与九轴Ya*w 对比图

4 结语

本文设计与实现了独轮机器人姿态控制与惯性导航系统。

在三大环串级PID 系统的控制中：其中两大环（Pitch直立环与Roll 直立环）用于控制系统的平衡，并且Pitch直立环可以任意人为定义与改变独轮机器人的速度，Roll直立环还可以与Yaw 转向环进行辅助的转向控制，Yaw 转向环实现两种转向控制，使独轮机器人可以实现高速平滑的转向，精准到达所需方位。

使用九轴传感器进行四元数解算，获取独轮机器人的姿态角，用以实现简略捷联式惯性导航系统；其中还包括编码器采集的速度数据对系统进行不断修正，减少环境对于九轴传感器所带来的干扰，实现高分辨率，低漂移的简略捷联式惯性导航系统。并且在此系统上可以进行人为定义的目标点的设定以及路径的规划，实现不同方位，多变化地到达目标点位。

最终设计并实现了可行性高、响应速度快、抗干扰性强以及导航精准度高的独轮机器人姿态控制与惯性导航的系统。但是系统中还未进行在不同恶劣道路状况下的应用实践，无法确定是否能克服恶劣的道路环境和轮胎打滑情况。其功能性和实用性还有待加强，功能模块上可加入摄像头识别功能与机械臂搬运功能，算法上还可尝试改进并细化捷联式惯性导航的初始对准问题，以实现精度更高的惯性导航。