Scientific Journal of Control Engineering August 2013, Volume 3, Issue 4, PP.288-294
Four Wheeled Omni-directional Drive Vehicle Motion Control Method Research Huajin Yan †1, Runxuan Li 2, Yongzhi Yang 1, Dexue Bi1 1. College of Mechanical Engineering, Tianjin University of Science and Technology, Tianjin 300222, China 2. College of precision instrument, Tianjin University, Tianjin 300192, China †Email:
lmt198@sohu.com
Abstract In view of the rapid development of China's domestic market demand, the liberation of human complex labor, the full enjoyment of modern life. With high efficiency of four wheeled omni-directional robot cleaning vehicle as the research object, based on AVR motion control theory, the robot model is analyzed in detail, the radius of curvature of the method of model simplification, by constantly changing the radius of curvature of the control car, so that a robot could quickly, has good real time. The system uses modular programming, obstacle avoidance module to join, improved the readability, easy debugging, to realize the obstacle avoidance, better achieve the predetermined path, and has a certain practicality. Keywords: Radius of Curvature; Motion Model; Path to Achieve
四轮全向驱动小车运动控制方法研究 闫化锦 1，李润萱 2，杨永志 1，毕德学 1 1. 天津科技大学 机械工程学院，天津 300222 2. 天津大学 精仪学院，天津 300192 摘 要：本文针对我国快速发展的家务市场需求，解放人的繁杂的劳动，充分享受现代生活，以效率较高的四轮全向机器人 清扫小车为研究对象，基于 AVR 单片机的运动控制原理，通过对机器人小车模型的详细分析，采用曲率半径的方法对模型 进行简化，通过不断地改变曲率半径实现对小车的控制，使机器人小车能快速地反应，具有较好的实时性。系统采用模块化 编程，模块便于加入，提高了可读性，容易调试，实现避障，较好地实现预先设定的路径，具有一定的实用性。 关键词：曲率半径；运动模型；路径实现
引言 随着中国工业的快速发展和技术进步，我国家务市场开始成型，人们的生活水平逐年提高，对物质和精 神生活的质量也不断提高，迫切需要从繁重的清洁工作中解脱出来。由此诞生了一种家用服务型清扫机器人， 四轮全向机器人清扫小车正是这一机器人的有益尝试。
1 四轮全向驱动机器人的设计 任何机器人的运动和控制都依赖于机器人的本身，即机械结构部分，这是机器人的基础。 机器人与计算机相区别的一个重要特征，就是机器人能够运动。要运动就必须有动力部件，以及由这些 动力部件驱动的结构。机器人的驱动子系统、传感子系统和控制决策子系统是一个机器人最基本的三部分。 驱动控制是基于 AVR 单片机的运动控制原理来实现。AVR 单片机不仅具有强大的高速运算处理能力， 而且在片内集成了丰富的电机控制外围部件。电动小车控制系统的主要构件为控制芯片单片机和滤波电路， - 288 http://www.sj-ce.org/
4 个轮毂电机驱动。电机控制系统框图见图 1。
控制芯片单片机
三相电压
控制电压
PWM 控制电压 滤波电路
电机控制器
轮毂电机
信号 图 1 电机控制系统框 图 设计指标：爬坡能力：最大 10°斜坡 ；路况摩擦系数：0.2～0.45；最大前进速度：120mm/s；舵机最大转角：36° ；传感器 最大探测距离：20cm。
以上设计指标是受家庭环境和其它环境限制的，是机器人小车的基本指标，设计时机器人小车必须满足 以上指标，以便更好地满足清扫要求，达到良好效果。
2 路径实现方法 2.1 运动学建模过程分析 运动学是对机械系统如何运行的最基本的方法。必须建立相应的运动学模型，这是移动机器人运动学最 根本的问题。 为整个机器人运动推导一个模型，是一个由底向上的过程。各单个轮子对机器人的运动作贡献，同时又 对机器人运动施加约束。根据小车底盘的几何特性，四个轮子是连在一起的。所以它们的约束联合起来，形 成对机器人底盘的整个运动约束。在整个分析过程中，我们把机器人建模成轮子上的刚体，运行在水平面上。
图 2 建模分析示意图
为了确定机器人在平面中的位置，建立全局参考框架和机器人局部参考框架之间的关系（如图2）。轴XI 和YI将平面上任意一个惯性基定义为从某原点O：{XI，YI }开始的全局参考框架。为了确定机器人的位置，选 择机器人的底盘中心点P作为它的位置参考点。基于{XR,YR}定义了机器人底盘相对于P的两个轴，从而定义了 机器人的局部参考框架。在全局参考框架上，P的位置由坐标x和y确定，全局和局部参考框架之间的角度差由θ 给定。我们可以将机器人的姿态描述为具有这3个元素的向量。下标I是阐明该姿态的基石全局参考框架： x I y
(1)
用分量的移动描述机器人的移动，需要把沿全局参考框架的运动映射成沿机器人局部参考框架轴的运 动。映射的是机器人当前位置的函数。该映射用正交旋转矩阵来完成： - 289 http://www.sj-ce.org/
cos（） sin（） 0 R( ) sin（） cos（） 0 0 0 1
(2)
可以用该矩阵将全局参考框架{XI，YI }中的运动映射到局部参考框架{XR,YR}中的运动。这个运算由
R(θ) I 标记，该运算取决于 θ 的值：
R R( ) I 2
(3)
给定在全局参考框架中的某个速度（ x ， y ，θ ），我们就可以计算沿机器人局部轴 XI 和 YI 的运动分量。
2.2 轮子运动学约束 机器人运动学模型的第一步是表征加在单独轮子上的约束。 为了便于分析，我们假定轮子平面总是保持垂直，且在所有的情况下，在轮子与地面之间只有一个单独 的接触点。 在这些条件下，对于每一个轮子，我们提出两个约束。第一个约束滚动接触，即当运动在适当方向发生 时，轮子必须滚动。第二个约束无横向滑动，即在正交于轮子的平面，轮子必须无滑动。 小车轮子属于小脚轮，现在轮子的接触点在位置 B，它被固定长度 d 的刚性杆 AB 连接到点 A，从而固 定了垂直轴的位置，B 围绕该轴进行操作。 车轮有两个随时间改变的参数， (t)表示了随时间的转动，β(t)表示了 AB 随时间的操纵角度和方向，其 简化分析图如图 3 所示。
图 3 小车轮分析示意图
对小脚轮而言，在与轮子平面一致的运动期间，轴偏移不起作用，其滚动约束方程为：
X R cos(α+β+γ)+ YR sin(α+β+γ)+ L sin(β+γ)+d sinγ=R
(4)
然而，小脚轮的几何特性，确实对滑动约束有重大影响。关键性的问题是在点 A 发生对轮子的横向力， 因为这是轮子对底盘的接触点。由于相对于 A 点的地面偏移接触点，横向运动为零的约束是错误的，此约束 反而很像是一个滚动约束。因此垂直轴的适当转动必定产生： - X R sin(α+β+γ)+ YR cos(α+β+γ)+ L cos(β+γ)+ d cosγ=0
(5)
上述两个式子是一个轮子的运动方程，将四个轮子的方程联立即为小车的运动模型。
2.3 运动学模型的简化 2.3.1 模型的简化原理 上述运动模型是四轮小车机器人的万能模型，对于小车要走的不同路径，前进的不同方向，可以有无数 - 290 http://www.sj-ce.org/
种选择，模型有多种解，实现方法多样。为便于计算和编程，实现不同的路径轨迹，必须对模型进行简化。 任何一条曲线，都是由无数半径不同的小圆弧组成的，每一段都有一确定的曲率半径相对应，这是我们 对模型进行简化的基础。由于小车每个轮子都配有舵机，都能进行转向，每个轮子都能灵活运动，故可以利 用这一点按曲率半径来控制小车的运动轨迹。转弯时小车四个车轮的法线相交于曲线该点处的曲率中心。 2.3.2 模型简化的实现
图 4 小车运行示意图
以图 4 中小车向右转为例，我们进行详细分析实现模型的简化，并计算小车舵机的转角。AB 为曲线此 点处的曲率半径 R，A 为小车底盘中心（局部坐标的原点），B 处为曲率中心，C 点为舵机转轴，D 处为小 车车轮。其中 AB=92mm，CD=75mm，车轮半径 r = 30mm，α=49°，轮子与 CD 舵机连杆方向夹角 γ=53°。 其分析过程如图 4 所示。 BD 方向为车轮法线方向，现在在△ ABC 和△ BCD 中计算舵机转角（如图 5）：
图 5 计算舵机转角示意图
在△ ABC 中，已知条件：AB 为由小车路径曲线计算而来的曲率半径，AC 为小车局部坐标原点与舵机 转轴中心的距离，∠BCA 为已知。由上述条件， 利用余弦定理和正弦定理可求得 BC 长度和∠ABC 的值： BC2=AC2+R2-2×AC×R×Cos(∠BCA)； Sin(ABC )
AC Sin（BAC） BC
(6)
在△ BCD 中，已知条件：BC 长度由上一步已经求得，BD 垂直于车轮 EF 方向，∠CDF=γ。有上述已知 条件，利用余弦定理和正弦定理可求得 BD 长度和∠BCD 的值： BD2+CD2-2×BD×CD×Cos(90+∠CDF)=BC2； Sin(BCD)
BD Sin（BDC） BC
- 291 http://www.sj-ce.org/
(7) (8)
由于舵机连杆初始位置在 AC 方向，由几何关系可以计算出∠DCG 的值，即为曲线此点位置处小车舵机 的转角： ∠DCG=∠BAC-+∠ABC-∠BCD
(9)
其 C 语言编写的程序如下 double jiaodujs1(double k) { double l,a,aa,b,bb,zj,m,t,tt,mm; t=49*pi/180; tt=131*pi/180; l=pow((pow(92.2,2)+pow(k,2)-2.0*92.2*k*cos(t)),0.5); a=92.2*sin(t)/l; aa=asin(a)/pi*180; mm=(75*cos(tt)+pow((pow((75*cos(tt)),2)-(pow(75,2)-pow(l,2))),0.5)); if (mm>abs1(l-75) & mm<(l+75)) m=mm; else m=(75*cos(tt)-pow((pow((75*cos(tt)),2)-(pow(75,2)-pow(l,2))),0.5)); b=m*sin(tt)/l; bb=asin(b)/pi*180; zj=49+aa-bb; return(zj); } 对于远离曲率中心的小车外侧轮子其计算方法与上述计算过程类似，篇幅所限不再赘述，只给出其实现 程序： double jiaodujs2(double k) //圆外圈轮子 { double l,a,t,tt,aa,m,b,bb,zj,mm; t=pi*131/180; tt=pi*49/180; l=pow((pow(92.2,2)+pow(k,2)-2*92.2*k*cos(t)),0.5); a=92.2*sin(t)/l; aa=180*asin(a)/pi; mm=(75*cos(tt)+pow(pow(75*cos(tt),2)-(pow(75,2)-pow(l,2)),0.5)); if (mm>abs1(k-75) & mm<(l+75)) m=mm; else m=75*cos(tt)-pow(pow(75*cos(tt),2)-(pow(75,2)-pow(l,2)),0.5); b=m*sin(tt)/l; bb=180-asin(b)*180/pi; zj=131+aa-bb; return(zj); - 292 http://www.sj-ce.org/
} 其中 abs1()函数用于计算绝对值： double abs1(double x) { return(x>0 x:-x); } 由于小车几何结构和路径实现方法的特点，虽然左右两侧舵机转角大小不同，但小车同侧的舵机具有相 同的转角，只是前后舵机在程序中具有不同的符号，故上述两个函数即可计算出舵机的转角。 由数学知识，任意曲线的曲率半径 K 得： K=| yˊ|/( 1 + yˊ2)
3/2
(10)
给定了机器人小车路径曲线，就可根据上式计算求得曲线任意处的曲率半径 R。根据求得的曲率半径， 上面的内容就可全部求得的，实现设定的机器人小车路径。 2.3.3 模型简化中要解决的问题 转弯问题 平面坐标系中的曲线有凹、凸两种，曲率中心可能落在平面曲线的两侧（如图 6），每一种小车具有相 反的转向趋势，这就提出了小车如何转向的问题。 Y
Pn P1
P2
X O 图 6 曲率分析
对此，我们定义任意曲线都是平滑过渡的，即曲率半径是均匀变化的。我们利用函数的凹凸性(见图 7) 来解决这个问题。 f( y
y f(x1)
O x1
f(x1) f(x2) f(x2) 2 x1 x 2 f( ) 2 x1 x 2 2
x1 x 2 ) 2
f(x1) f(x2) 2
f (x1)
x
O
x2
f(x2)
x1 x1 x 2 2
x x2
图 7 凹凸函数
函数 f(x)在区间 I 上连续，如果对 I 上任意两点 x1,x2 恒有 f( 果恒有 f(
f（x1） f（x 2） x1 x 2 )＞ ，则函数为凸函数。 2 2 - 293 http://www.sj-ce.org/
x1 x 2 f(x1) f(x2) )＜ ，则函数为凹函数，如 2 2
如果没有这样的限制，小车将只有一个方向的运动趋势，为此我们默认函数为凸时小车有向左的运动趋 势，函数为凹时小车有向右的运动趋势，这样小车转弯的问题就解决了。 曲率半径过小或过大。 由于受到小车几何结构和舵机架安装的限制，舵机转角并不能按给定的参数完全发挥功效，转角范围有 所限制。 当曲线某处曲率半径过小时，令舵机按允许的最大转角范围运动，确保小车的安全，防止舵机被卡死， 烧坏舵机。也即小车有一最小曲率半径，当程序计算的曲率半径小于此最小曲率半径时，小车按最小曲率半 径运动。 当曲线某处曲率半径过大时，由 2.3.2 内容计算得来的舵机转角较小，甚至为零，故当曲率半径大于某 一值时，默认小车直线前进。
3 结论与展望 简化的模型减少了计算规模，提高了控制效率，简化了机器人操作的步骤，使机器人能快速地反应，具 有较好的实时性。由于采用模块化编程，避障模块便于加入，提高了可读性、容易调试，较好地实现了避障， 具有一定的实用性。 转弯问题只是简单的介绍了公知的凹凸函数上两点比较，深入的分析仍是今后需要解决的问题。
REFERENCES [1] R·西格沃特, I·R·诺巴克什. 自主机器人导论. 西安: 西安交通大学出版社, 2006: 49-57 [2] 金钟夫, 杜刚. AVR Atmega128 单片机 C 程序设计与实践. 北京: 北京航空航天大学出版社, 2008: 1-5 [3] 肖雄军, 蔡自兴. 服务机器人的发展. 自动化博览, 2004, (6): 1-5 [4] 徐德, 邹伟. 室内移动式服务机器人的感知、定位与控制. 北京: 科学出版社, 2008:21-22 [5] Arkin, R.C., Behavior-Base Robotics. Cambridge MIT Press, MA, 1998:12-40 [6] Bundy, A. (editor), Artificial Intelligence Techniques, a Comprehensive Catalogue. New York, Spinger-Verlag, 1997:47-56 [7] Russell, S., Norvig, P., Artificial Intelligence, a Modern Approach. Prentice Hall International, 1995:1-10
【作者简介】 #1
闫化锦（1987-） ，男，汉族，硕士研究
李润萱（1991-），男，汉族，大学本科在读，主要从事测控
生在读，主要从事机械设计理论和机电
技术与仪器的学习与实践活动。
控制的学习和研究工作。
杨永志（1986-），男，汉族。大学本科毕业，现企业从事机
Email: lmt198@sohu.com
电产品工作。 毕德学（1969-），男，汉族，教授，从事机电产品和机器人 研究工作。
- 294 http://www.sj-ce.org/