具有立体视觉的球形机器人及其运动控制.pdfVIP

第 4 9卷第 1 1期 2 0 1 3 年 6 月 机械工程学报 J OURNAL OF M ECHANI CAL ENG1 NEERI NG VO 1 4 9 N O 1 1 J u n 2 0 1 3 DoI l 0 3 9 0 1 J M E 2 0 1 3 1 1 0 0 8 具有立体视觉的球形机器人及其运动控制水 叶 平 韩亮亮 张天石 王 轩 孙汉旭 f 北京邮电大学 自动化学院北京 1 0 0 8 7 6 摘要 针对现有球形机器人存在环境感知能力较弱 运动控制精度不高的问题 提出一种具有立体视觉的新型球形机器人 V i s i o n b o t 介绍 V i s i o n b o t机器人的机械结构 采用具有非完整约束的拉格朗日方程建立该球形机器人的动力学模型 针对 状态变量过多难以用于控制系统设计的问题 对球形机器人的动力学模型进行合理简化 利用模型解耦 线性化 以及状态反 馈等方法 建立该球形机器人的运动控制模型 包括内环力矩控制器和外环路径跟踪控制器 采用延时环节对视觉反馈通道 的时延进行建模 建立具有视觉反馈的球形机器人运动控制模型 通过运动仿真试验 研究机器人重摆质量 视觉反馈延时 对其运动控制性能的影响 并验证系统设计的可行性 研究结果为 V i s i o n b o t 机器人的优化设计和样机研制提供理论基础 关键词 球形机器人运动控制立体视觉路径跟踪 中图分类号 T P 2 4 2 De s ig n a n d M o t io n Co n t r o l o f Ba l l s h a pe d Ro b o t wit h S t e r e o Vis io n YE P in g HAN L ia n g l ia n g ZHANG Tia n s h i W ANG Xu a n S UN Ha n x u A u t o ma t io n S ch o o l B e i j in g U n i v e r s it y o f P o s t s a n d T e l e co mmu n i ca t io n s B e i j in g 1 0 0 8 7 6 Ab s t r a ct Be ca u s e e x i s t in g b a l l s h a p e d r o b o t s h a v e ma n y d r a wb a ck s s u ch a s we a k ca p a b il it y o f e n v i r o n me n t a l p e r ce p t io n and l o w p r e ci s i o n o f mo t io n c o n t r o l a n o v e l b a l l s h a p e d r o b o t wi t h s t e r e o v is io n r e f e r r e d t o a s Vis i o n b o t is p r o p o s e d Af t e r in tro d u ct i o n o f me ch anica l s t r u ct u r e o f t h e Vi s i o n b o t d y n a mi c mo d e l o f t h e r o b o t is d e v e l o p e d b y u s i n g t h e co n s t r a in e d g e n e r a l L a g r a n g i an f o r mu l a t io n a s we l l a s mo d e l s imp l i fi ca t io n u n d e r r e a s o n a b l e a s s u mp t i o n s Ba s e d o n a l i n e a r i z a t i o n a p p r o a ch d y n am i c mo d e l o f t h e r o b o t is d e co mp o s e d a n d mo t i o n co n t r o l l e r i s d e s ig n e d i n cl u d i n g t wo i n n e r t o r q u e co n tro l l e r s a n d a l l o u t e r p a t h t r a ck i n g co n t r o l l e r A mo t i o n co n tr o l a l g o ri t h m wi t h d e l a y e d v is u a l f e e d b a ck is d e s i gn e d f o r the r o b o t Effe ct s o f p e n d u l u m ma s s a n d d e l a y e d v i s u a l f e e d b a ck o n p e r f o rm anc e o f the co n tr o l a l g o ri t h m is s t u d ie d b y s imu l a t e d e x p e ri me n t s E x p e ri me n t a l r e s u l t s n o t o n l y v a l id a t e the f e a s ib il ity o f s y s t e m d e s ign b u t a l s o C a l l b e u s e d t o d o the o p t i mum d e s i gn and p r o t o typ e d e v e l o p me n t o f the r o b o t Ke y wo r d s Ba l l s h a p e d r o b o t Mo t i o n co n tro l S t e r e o v i s i o n P a t h tra ck in g 0 前言 球形机器人是指一类将运动机构 传感器 控 制器等内置于一个球壳内部 利用球壳作为滚动行 走装置的移动机器人 与轮式 履带式等移动机器 人相 比 球形机器人的特点在于L 1 J 运动灵活 能够穿过孔径略大于其直径的曲折通道 具有类 似不倒翁的特性 行走过程 中与障碍物或其他的运 动机构发生碰撞也会 在经过短暂的不规则运动后恢 复稳态 当发生从高处跌落等危险情况时 仍然 国家 自然科学 5 1 1 7 5 0 4 8 中央 高校基本科研业务费专项基金 2 0 0 9 R C 0 6 0 1 和高等学校科技创新工程重大培育 7 0 8 0 1 1 资助项 目 2 0 1 2 0 7 1 2收到初稿 2 0 1 3 0 4 0 9收到修改稿 可 以继续工作 不存在翻仰问题 因此 球形机器 人在野外作业 反恐及灾难救援和社会服务等领域 具有广阔的应用前景 至今 研究人员对球形机器人的机构设计 运 动学 动力学建模 运动控制等方面 已经开展了大量 的研究工作 4 J 但是 受限于全封闭球壳和特殊的 滚动行走方式 现有球形机器人往往缺少外部测量 传感器 如激光传感器 视觉相机等 其环境感知 能力较弱 难以准确地获得球形机器人的位置 速 度等运动参数 因而运动控制方法主要采用远程遥 控或开环控制 7 8 J 存在控制精度不高 累计误差较 大且抗干扰能力较差等 问题 球形机器人 的运动控 制 问题 已经成为 阻碍球 形机器人进一步应用和发展 的关键 问题 移动机器 学兔兔 w w w x u e t u t u c o m 学兔兔 w w w x u e t u t u c o m 1 0 机械工程学报 第 4 9卷第 ll 期 义 s in 和 C O S O 由图 2所示的坐标系定 I S C B c 一 s s B I 一 l 一 s B 0 C B 料 co嚣 j so s 1 X r C S 8 8 s Y 2 s s p 3 2 0 由于 前 两个 运 动 方程 是 不 可积 分 的 因而 V is io n b o t 机器人具有非完整约束 2 2 拉格朗 日方程 针对 V is io n b o t 机器人 分别推导均质球壳 对 一 tO l l 吱 j 1 y k b 2 j 因而对称框架的平动动能 m 协 式中 为从 到 的 旋转变换矩阵 即 R o t z c 一 l S C o 0 I 0 0 1 j i 1 l 2 1 2 I 2 i 式中 和 分别为 的三个分量 厶 硅i 厂 i 5 学兔兔 w w w x u e t u t u c o m 2 0 1 3年 6月 叶平等 具有立体视觉的球形机器人及其运动控制 重摆的动能和势能分别为 寓 器 E 矗 md g zd 因此 重摆 的拉格朗 日函数 d 一 d 7 2 3 系统动力学方程 由式 4 5 7 求得 V is i o n b o t 机器人系统的 拉格朗日函数 L 厶 采用具有非完整约束的拉格 朗日方程 求得系 统动力学方程 g N q 口 g Bu 8 式中 M q R 和 N q 圣 R 分别为系统的惯 性矩阵和非线性项 且有 f 1 0 0 一 s p 0 0 1 0 l o 一 一 s p 0 0 J g X 8 l B 0 0 0 0 0 0 0 0 1 0 0 l 0 0 由式 3 运动约束方程 可知 a q q 0 将 矩 阵 a q 写 成 由 A I 和 组 成 的 分 块 矩 阵 即 A A 1 A 4 一r C Sp 0 令 c 一 可以得到 圣 c g 圣 2 求导 得 C 留 2 C q q 2 9 式中 q 2 将式 9 代入式 8 同时左乘 CT g 得到系统 降阶的动力学方程 C q M q C g 2 c g I B u N q C q q 2 一 M q C q q 2 l 1 0 式 中 C g g C g 为 5 5 对称正定矩阵 可视 为以霉 为广义坐标 的新系统惯性矩阵 2 4 系统动力学模型化简 由式 1 0 给出的降阶系统动力学方程 其状态 变量仍然较多 难 以直接用于控制系统设计 需要 对动力学模型进行合理简化 V is i o n b o t机器人采用重摆 改变系统质心实现 运动 由于重摆 的质量往往 比球壳和框架的总质量 要大很多 因而可 以假设在机器人滚动行走的过程 中 重 摆 偏 离 轴 z 的 角 度 很 小 即 0和 B 9 9 0 o o 将 0 0 和 9 0 一 代入式 1 0 中 选取新的 广义 坐标 季 得 到系 统新 的动 力学 方 程为 牙 牙 1 1 髓 I I I S I i z 0 I C B 1 0 0 I I zc p 0 I m r 2 S 2B 1 c丘 雪 T 面 乏 F I I s 8 p 2 s B C B p 波一 1 一 户 厶 肼一 F S 8 矗 p n r I b 一m r S 8 C 8 j B 一 z b i d 式中 为机器人绕 转动 的摩擦力矩 由于角速 度 相对于 和 要大得多 因而忽略绕 和 的摩擦力矩 3 具有视觉反馈的运动控制策略 由式 1 1 可知 简化的系统动力学模型具有多 变量耦合和非线性 所需要设计的运动控制策略复 杂且不易在线实现 本文通过模型解耦 线性化和 状态反馈 设计内环控制器 实现对机器人角速度 反馈控制 同时 基于曲率控制设计外环控制器 实现对机器人路径跟踪控制 采用延时环节来描述 立体视觉定位算法所需的计算时间 并添加在反馈 学兔兔 w w w x u e t u t u c o m 1 2 机械工程学报 第 4 9 卷第 1 1期 通道中 从而建立具有视觉反馈的运动控制模型 3 1 系统动力学模型线性化 由于在实 际运 动过程 中机器人基本保持在 垂 直位置 且直线行走速度要远大于转向速度 因而 可以合理假 设 fl 2 且 是较小的量 即s in e 1 c o s fl 0 户 禾 0 用 上述假设对式 1 1 进行线性化近似 得到 k k 厶 r 2 一 一 尬 1 2 b r 2 1 一 r t 由式 1 2 可知 角加速度 与 西 以及 等变量无关 因而可将系统解耦为两个单输入的系 统 一个描述系统的自转角 输入控制量为 可 以通过设计单独的闭环控制率来控制角速度 另 一 个描述系统 的进动角和章动 角 输入控制量为 由于角速度 夕经过独立的反馈控制后 可以看 作一个定值 因而关于进动角和章动角的系统模型 可简化为线性定常系统 A x Gr 2 1 3 式中 X A和 G可 由式 1 2 推导得到 经分析可知 该子系统是能控能观的 3 2 状态反馈控制器 利用式 1 2 和式 1 3 两组解耦的系统状态方程 可将球形机器人的控制任务分解为两个子任务 一 个是运动速度控制 即控制机器人角速度 另一 个是运动姿态控制 即控制机器人角速度 和 其中 速度控制是基础 要求在运动过程中机器人 的角速度 稳定在期望角速度 附近 在此前提 下 利用 控制机器人 的运动姿态 设计两个 内环力矩控制器 分别实现对机器人 运动姿态和运动速度 的闭环控制 如图 3所示 其 中 运动速度控制器设计为 一 户 I f 一 户 1 4 运动姿态控制器 设计为 r E k 1 d f 一 0 一 1 5 利用式 1 4 和式 1 5 实现 V i s i o n b o t 机器人的运 动控制 其稳态 为 户 f a r e f 0 0 若 0 则 控 制 机 器 人 直 线 运 动 若 f 0 则控制机器人转弯 3 3 具有视 觉反馈的路径跟踪控制器 在内环控制器的基础上 基于运动路径 的曲率 丽 噍 图3 具有视觉反馈的运动控制模型 控制设计外环控制器 以实现 V is io n b o t 机器人的路 径跟踪控制 为使机器人的运动路径能够跟踪一条 期望 的直线 设计 曲率控制器为 l b J 二 一 k a k一 一 一 1 6 式中 出 为机器人运动路径的曲率 k对运动路 径弧长 S的导数 和 为大于零的常数 I s 和 分别为机器人 的方 向角和期望路径的方向角 为机器 人 当前位 置 到期望 直线 路径 的垂 直距 离 若 A d 0 表明机器人位于期望路径 的左边 若 A d 0 表 明机器人位于期望路径的右边 如果 机器人 已经位于期望的直线路径上 则 A d 0 将立体视觉定位算法所需的计算时间描述为 反馈通道 的延时环节 从而建立如图 3所示的具有 视觉反馈 的运动控制模型 4 仿真试验研究 由于立体视觉定位算法复杂 计算量大 因而 需要搭载具有强大计算能力的控制器 拟采取的控 制器方案 由 I n t e l i7 2 6 0 0处理器 4 G B 内存 Min i I T X主板 3 0 GB固态硬盘和 P C I E接 口的图 像采集卡等组成 其外形尺寸约为 1 7 c mx 1 7 c mx 1 0 c m 因此 为了满足控制器 以及其他部件的搭载条 件 V is io n b o t 机器人的球壳半径设计为 0 2 5 m 同时 机器人直线行走速度和转弯速度的设计指标 分别为 1 m s 和 0 4 r a d s 依据上述设计要求 本文 开展了 V is io n b o t 机器人的初步设计 已经确定的参 数 均质球壳质量 z b 4 k g 对称框架质量 m i约为 2 5 以及重摆的摆杆长度 0 2 m 基于已经确定的设计参数 利用 A D A MS和 Ma t la b S i mu lin k建立联合仿真模型 其中 V is io n b o t 机器人的 ADA MS模型由均质球壳 对称框架和重 摆等三部分组成 能够更好地反映真实的机器人样 机系统 的动力学特性 在此基础上 通过运动仿真 研究 验证所建立的运动控制模型的正确性 同时 重点研究重摆质量和视觉反馈延时对机器人运动控 r l 学兔兔 w w w x u e t u t u c o m 学兔兔 w w w x u e t u t u c o m 学兔兔 w w w x u e t u t u c o m 2 0 1 3年 6月 叶平等 具有立体视觉的球形机器人及其运动控制 1 5 4 B H A T 1 IAC HA A S A GR A WAL S K S p h e rica l r o l l i n g r o b o t A d e s ig n a n d mo t io n p l a n n i n g s t u d ie s J I E E E T r ans o n R o b o t ics a n d Au t o ma t io n 2 0 0 0 1 6 6 8 3 5 8 3 9 5 P E T C H J S A T H AP OR N L De s ign o f a t h r e e l e g g e d r e co n fignr a b l e s p h e rica l s h a p e r o b o t C P r o ce e d i n g s o f I E EE Co n f e r e n c e o n Ad v an ce d I n t e l l i g e n t Me ch a t r o n i c J u l y 1 4 1 7 2 0 0 9 S in g a p o r e Ne w J e r s e y I E EE 2 0 0 9 1 73 0 1 733 6 J OO NG C Y S U NG S A S p h e rica l r o b o t wit h n e w t y p e o f t wo p e n d u l u m d r iv i n g me ch anis m C P r o ce e d i n g s o f I E EE Co n f e r e n ce o n I n t e l l i g e n t E n g i n e e ri n g S y s t e ms J u n e 2 3 2 5 2 0 1 0 P o p r a d Ne w J e r s e y I EE E 2 01 0 2 7 5 2 7 9 7 赵勃 王鹏飞 孙立宁 等 双偏心质量块驱动球形机 器人的直线运动控制 J 机械工程学报 2 0 1 1 4 7 1 1 1 6 ZHAO Bo WANG P e n g i S UN Lin in g e t a 1 Lin e a r mo t io n co n t r o l o f tw o p e n d u l u ms d r iv e n s p h e ri ca l r o b o t J J o u r n a l o f Me ch ani ca l E n g i n e e ri n g 2 0 1 1 4 7 1 l 1 1 6 8 Z H AN Q iang J I A C h u a n MA Xi a o h u i e t a 1 Me ch a n i s m d e s i gn and mo t io n ana l y s is o f a s p h e rica l mo b il e r o b o t J C h i n e s e J o u r n a l o f Me ch ani ca l E n g i n e e ri n g 2 0 0 5 1 8 4 5 4 2 5 45 9 L E O N AR D J DU R R A NT WHY T E H F Mo b i l e r o b o t l o ca l i z a t io n b y t r a ck in g g e o me t r i c b e a co n s J I E E E T r ans o n R o b o ti cs and A u t o ma t i o n 1 9 9 1 7 3 3 7 6 3 8 2 1 O 蔡自兴 肖正 于金霞 动态环境中移动机器人地图构 建的研究进展 J 控制工程 2 0 0 7 1 4 3 2 3 1 2 3 5 CA Zi x i n g XI AO Zh e n g YU J i n x i a Ad v ance s o n ma p b u i l d in g wi th mo b i l e r o b o t s in d y n a mi cs e n v i r o n me n t s J C o n tr o l E n g in e e r i n g o f C h in a 2 0 0 7 1 4 3 2 3 1 2 3 5 1 1 丁良宏 王润孝 冯华山 等 立体视觉测程研究现状 J 机器人 2 0 1 1 3 3 1 1 1 9 1 2 8 DI NG Liang h o n g WANG Ru n x i a o F E NG Hu a s h a n e t a 1 Ad v a n ce s in r e s e a r ch o f s t e r e o v i s i o n o d o me t r y J R O B 0T 2 0 l 1 3 3 1 l 1 9 1 2 8 1 2 N I S T E R D NA R OD I T S K Y 0 B E R G E N J V is u a l o d o me t r y C P r o ce e di n g o f I E E E C o n f e r e n ce o n Co mp u t e r Vi s i o n an d P a t t e r n Re co gn i t i o n J u n e 2 0 0 4 Ne w J e r s e y I E EE 2 0 0 4 6 5 2 6 5 9 1 3 K I T T B G E I GE R A L A T E G AH N H V is u a l o d o me t r y b a s e d o n s t e r e o i ma g e s e q u e n ce s with RANS AC b a s e d o u t l ie r r e j e ct i o n s ch e me C P r o ce e d i n g s o f I E E E I n t e l l ig e n t Ve h i cl e s S y mp o s i um J un