虚拟NXT的NXTway-GS自行平衡两轮机器人教程（7）

旭日东升

7． 控制器模型（单精度浮点运算）

该章节描述nxtway_gs_controller.mdl中控制程序，任务配置和模块项目。

7.1  控制程序简介

状态机框图

图 7-1 NXTway-GS控制器的状态机框图。通过两种驱动的模式。一种是自动驱动模式，另一种为外部模拟PC手柄远程控制工具实现。

图7-1   NXTway-GS 控制器的状态机

任务设置

NXTway-GS控制器有四个任务描述如表7-1.
表7-1 NXTway-GS控制任务

Task	Period	Works
task_init	仅在初始化时候	初始化设置
task_ts1	4 [ms]	平衡和驱动控制 数据日志 陀螺仪误差校准
task_ts2	20 [ms]	障碍物检测
task_ts3	100 [ms]	时间检测 电池电压平均值

鉴于陀螺仪传感器的1秒内最大采样数,我们使用4[ms]间隔驱动控制任务来设置。同样的原因，我们用20[ms] 间隔完成超声波传感器的障碍检查任务。在4.2控制器设计描述控制器来完成平衡及驱动控制。

数据类型

我们采用单精度浮点数据的数据类型完成平衡及驱动控制设计，用以减少计算误差。在NXT智能模块中ARM7处理器没有FPU（浮点数处理单元），但我们可以通过使用GCC提供的浮点运算库软件方式实现单精度浮点运算。

7.2 模型总结

   Nxtway_gs_controller.mdl 是基于Embedded Coder Robot NXT框架设计。

图 7-2  nxtway_gs_controller.mdl

设备接口

我们使用传感器和伺服电机的Embedded Coder Robot NXT库提供设备接口来实现设备的组建。

图7-3  DC 马达接口

调度和任务

ExpFcnCalls调度模块配置任务，其中包括任务名称，任务周期，平台，和堆栈大小。由连接调度到被调功能子系统信号线完成函数在任务子系统调用。

图7-4  调度和任务

优先级

我们必须在根级别来设置设备块和ExpFcnCalls的调度块的优先级排序顺序为1.设备输入→2.任务→3.设备的输出。越小的数字表示越高的优先级，并允许出现负数。

要显示优先级，右击模块同时选择[模块属性

图7-5  优先级设置

共享数据

使用数据内存模块共享任务之间数据。

图7-6  共享数据

7.3 初始化任务 : task_init

这个任务设置初始化值。我们手动切换驱动模式。（自动驱动模式和远程控制驱动模式）

图7-7  task_init 子系统

7.4 4ms 任务: task_ts1

这是陀螺偏移校正，平衡及驱动控制，数据记录的任务。陀螺仪校正后的平衡及驱动控制运行。定义time_start作为在param_controller.m的陀螺偏移校准时间。

图7-8  task_ts1 子系统

平衡&驱动控制子系统如控制器在4.2控制器中描述

图 7-9 平衡和驱动控制的控制器模块

离散系统的微分和积分模块

离散微分模块通过后向差分法方式计算，同时离散积分模块通过前向欧拉法方式计算。

图 7-10  离散微分和积分模块

参考计算（Cal Reference subsystem）

该子系统使用低通滤波器，以减少过冲的速度参考信号的快速变化引起的参考坐标。

图 7-11  参考计算

状态计算（Cal x1）

该子系统用从传感器输出来计算状态。我们使用了陀螺仪一段时间输出平均值，作为消除陀螺仪偏移误差的影响和以及通过低通滤波器以消除速度信号噪声。

图 7-12  状态计算

PWM 计算(Cal PWM)

基于框图4-4和最大电机电压方程式（6.1），该子系统计算PWM占空比。

图 7-13 PWM计算

数据记录

NXT的手柄ADC数据记录块被放置在该子系统中，通过它用于记录传感器的输出和计算结果。若变更记录期间，在param_controller.m中定义变更次数log_count。 NXT的手柄ADC数据记录块可以记录传感器输出和8位，16位有符号整型数据。它用于日志记录浮点数据，除适当的值量化是必要的，因为它不直接支持浮点数据。分母除以操作被称为斜杠或LSB，对应每1位的值。我们可以得到原始值乘以所记录的数据和斜杠（LSB）。

图 7-14 数据记录

7.5 20ms 任务: task_ts2

这个任务用超声波传感器来检查的障碍。当发现障碍物时， NXTway-GS自动控制方式下向右转动，在远程控制下向后倒退

图7-15  task_ts2 子系统

7.6100ms 任务: task_ts3

这个任务完成检查时间和平均电池电压。在sound_dur [ms]期间当陀螺仪校准完成NXTway-GS响一声

图7-16  task_ts3 子系统

7.7 标定参数

在nxtway_gs_controller.mdl使用的所有参数都定义在param_controller.m文件中。表7-2显示了平衡及驱动器控制标定参数。，因为部件，模块，传感器和伺服电机是不同的，你可能需要调节这些参数。

表7-2 标定参数

参数	描述
K_f	伺服控制反馈增益
K_i	伺服控制积分增益
k_thetadot	速度给定增益
k_phidot	转速参考增益
k_sync	P为轮子同步控制增益
a_b	平均电池电压的低通滤波器系数
a_d	消除速度噪声的低通滤波器系数
a_r	消除过冲低通滤波器系数
a_gc	陀螺仪校准低通滤波器系数
a_gd	消除陀螺漂移影响低通滤波器系数
pwm_gain	摩擦补偿增益
pwm_offset	摩擦补偿误差
dst_thr	避障阈值（距离）
turn_angle	在自主驱动模式下旋转角度