CompactRIO-的家居监控机器人.doc

CompactRIO 的家居监控机器人本方案的背景是国 家863多机器人系 统控制项目的一个主角，家居监控机器 人，它的设计概念主 要是面向未来老年家庭，负责通过网络监 控家居环境中的各种 家电，具有语音和人脸识别等人机交互功 能，具有双机械臂完 成一些取物、递送等任务，并且可以作为 智能轮椅载人移动。 在2010世博会沪上生态家案例馆中展 出。- 申 纯太, 上 海电气集团股份有限 公司中央研究院The Challenge:图 1主控制器 Compact RIO的安装位置需要 CompactRIO 同时控制多个设备运 动。2根5自由度机 械臂运动，头部1自 由度旋转运动，双主 动轮底盘运动控制。CompactRIO 内要部署机械臂运动 控制算法， LIDAR数据采集 和导航避障算法，基 于编码器和光雷达数 据以及FPGA的移动位置控制算法 等。The Solution:将机器人的功能进行 划分为两部分。将双 5自由度机械臂运 动，头部1自由度的 旋转运动，双主动轮 底盘的运动，光雷达数据采集处理和编码 器数据采集，自主导 航，轮椅模式切换判 断，机器人体表 LED状态情感或信 息显示，电池电源管理等基本行为和设备 层控制归为一部分， 由“小脑”控制；将 家庭智能家居网络交 互，其他智能机器人 监控，语音识别对话和人脸识别的人机交 互功能等归为一部 分，由“大脑”控 制。Author (s):申 纯太 - 上海 电气集团股份有限公 司中央研究院 谭 福生 - 上海 电气集团股份有限公 司中央研究院 杨 军 - 上海电 气集团股份有限公司 中央研究院Yang Jun - Shanghai Electric Group Co., Ltd. Central Academe Shen Chuntai - Shanghai Electric Group Co., Ltd. Central Academe Tan Fusheng - Shanghai Electric Group Co., Ltd. Central Academe选用NI CompactRIO 作为机器人的“小 脑”。NI CompactRIO 是美国NI公司的工 业级嵌入式控制器， 集成以太网接口和 RS232串行接 口，具有体积小，高 可靠性，高性能，低 功耗等优点。将各种设备的控制算 法例如机械臂运动控 制，头部运动控制， 导航算法，基于以太 网的光雷达数据采 集，与大脑通讯等程 序部署在RT中；将 数字信号采集，例如 限位开关和紧急停止 逻辑还有编码器数据 采集和处理，地盘运 动伺服控制等算法部 署在FPGA中。选用工业嵌入式触摸 平板电脑IPC作为 机器人的“大脑”。 部署WinCE，具 有人机界面功能以及 触摸屏交互功能，部 署控制人脸识别模块 以及语音识别算法， 智能家居监控管理算 法等。用FSM来封装每个 设备对象：Finite- state machine (FSM)称为有限 状态机广泛用于数字 电路和计算机程序。 我们使用了两种状态 机来封装设备。 String Based Queued State Machine基于 字符串队列状态机和 普通基于枚举的状态 机。使用状态机作为 一种机制来处理单个 设备的状态转换，或 者说，用状态机这种 行为模型来用于单个 设备的事件处理。图 2 LIDAR 状态机例如光雷达的状态机 框图，它具有4个状 态，Idle， Start， Running， Stop。在 Running状态 中，程序不断向光雷 达发送指令读取数 据，然后由一个 Shared Variable来传出数据。使用一个 LIRAR.vi来 封装这个状态机。1/6图 3 Remote Control Reaction 状态机图3是一个远程控制 指令解析状态机，它 使用String Based Queued State Machine来实 现。这样的好处是， 可以使用队列来组织 动作，较为灵活地实 现各种不同组合以及 复杂度。每一个设备的状态机 都用一个独立的vi 来封装，并且单独进 行测试，这样也有利 于工程化。多状态机的协作：单个状态机只能完成 部分功能，我们需要 把所有设备状态机集 成起来，按照一定的 通讯机制拼装成一个 完整的机器人。由于所有的设备状态 机都由一个VI来封 装，只需要把他们拖 入一个主vi中，就 能调用他们。如图所 示：图 4状态机集成状态机之间是通过 Shared Variable来 实现接受外部指令输 入以及自身状态输出 的。同样，这些 Shared Variable都 用一个VI进行封装 以便进行管理。这样，一个状态机便 可视为一个子系统。 既然是系统，自然有 系统的输入和输出， 通过get，set 等方法的vi，外部 系统便可对这些子系 统进行操作或者交 互。如图所示，底盘 状态机使用 getCommand. vi方法获取外部命 令。图 5 getCommand. vi使用 setCommand. vi方法让外部系统 传输命令给地盘状态 机。2/6图6 MobileBase. lvlib: setCommand. vi底盘使用 sendState. vi发送底盘状态机 状态信息。如图所 示：图 7 MobileBase. lvlib. sendState. vi使用 getState. vi方法获取地盘状 态机信息。图 8 getState. vi导航算法：3/6图 9家居监控机器 人定位导航软件控制 框图总体设计针对移动机器人家居 环境下的定位问题， 提出了一种结合平直 线段匹配、角匹配和 里程计的组合定位方 法。该系统采用了 Labview开发 平台和 CompactRIO 控制器，得到了很好 的实时性效果。机器 人首先通过二维激光 测距仪通过 TCP/IP得到环 境点信息，然后通过 迭代适应点 （IEPF）算法得 到环境线段及最小二 乘法得到线段参数。 在基于线段基础上， 得到局部的平直线段 和角特征，再与已知 平直线段和角特征做 匹配，通过平直线段 和角匹配算法实时更 新机器人位置和姿 态。分析里程计定 位、平直线段匹配定 位和角匹配定位的误 差，分配不同的权重 得到优化的组合定位 算法。软件控制框图基于以上的设计，定 位导航的软件控制模 块设计如图7所示。 中心模块为定位导航 模块，该模块读取起 点、目标点、运动模 式数据及编码器数 据，同时读取激光测距仪数据提取角、平 直线段特征和已知特 征做匹配，其匹配算 法输出机器人位姿 。对于给定的目标 位姿，该模块将计算 出驱动指令 （Forward Speed、 Delta angle、 Turnspd Right、 Turnspd Left对应前进速 度、方向转角、左 转、右转）和状态信 息如（bPos, bAngle）来判 断机器人是否到达目 标位置和姿态。图 10家居监控机 器人定位导航的输入 输出信息部分图10为定位导航模 块部分的输入输出部 分，上半部分的输入 信息中的已知角特征 和全局线段为为已知 地图信息，下半部分 的部分输出信息包括 直线段提取示意图， 路径任务数组信息 图，实时显示的机器 人当前位置信息、速 度信息等。线段特征提取4/6为得到环境的几何线 段信息，需要对激光 测距仪的点集进行分 割，其分割算法可分 为下列步骤：坐标变 换、区域分割、 IEPF线段提取和 最小二乘法计算线段 参数。其中区域分割：从初始点i=0 开始检测两相邻点 和 的距离，如距离值 小于阈值D，则认为 属于同一区域点集， 否则开始一个新的区 域。该过程遍历所有 点集。如果某区域点 数目小于4个，则认 为噪声区域，舍弃这 些噪声点。图 11 IEPF线 段提取IEPF提取线段： 对于上述得到区域可 能含有多条线段， IEPF线段提取算 法是一种有效的线段 提取方法。如图11 所示， Pm点到线 段PsPe的距离大 于阈值T，该方法把 点集 分为和IEPF为迭代算 法，对于 和 重复上述的算法 知道 小于阈值 。该算法也有分割 过细的时候，如图9 右半部中间两条线段 可通过检查线段参数 的办法，对于斜率倾 角误差小于一定阈值 的线段加以合并处 理。线段参数可通过 最小二乘法计算求 得。如图12所示为通过 Labview实现 的IEPF算法的区 域分割算法，其中子 VI 为