一种四轮驱动全向底盘的控制装置及控制方法
【专利摘要】一种四轮驱动全向底盘的控制装置及控制方法，适用于机器人底盘移动控制领域，无线串口通信模块用来接收来自与计算机连接的无线串口通信模块的指令，指令信息包括四轮驱动全向底盘所要移动到的目标点的位置值以及姿态角值，将接收到的指令传给上位机arm；fpga将计数电路和信息采集电路融合，由FPGA对码盘计数模块的数据进行采集与处理，然后通过通用I/O接口传输到ARM，同时FPGA也作为接口电路，将ARM的控制指令传输到直流电机控制模块，完成对直流电机的转速以及方向的控制；ARM是整个控制装置的核心控制模块，用以对接收到的数据进行融合，实现对四轮驱动全向底盘的位姿进行实时结算，完成对四个直流电机的实时控制。
【专利说明】-种四轮驱动全向底盘的控制装置及控制方法
【技术领域】
[0001] 本发明是一种四轮驱动全向底盘的控制装置及控制方法，主要适用于机器人底盘 移动控制领域，可用于家用机器人、工业用机器人、军用机器人的底盘移动控制。可使机器 人始终沿直线行走，同时可以改变机体姿态，大大缩短机器人的行走距离，提高了机器人的 灵活性和机动性。采用这种方法，可以使机器人在狭小空间内较好地完成任务。
【背景技术】
[0002] 随着1962年美国研制出了第一台工业机器人，经过了近50年的发展，各国在机 器人技术上投入了 了大量的人力、物力、财力，机器人技术的发展已经经历了翻天覆地的变 化。从最初的一代工业机器人，到如今第三代智能机器人，机器人技术水平已有了大大提 升，其中几个科技强国美、日、德、英、法等正走在世界前列，其中美、日两个科技超级大国更 是在各个方面均遥遥领先。目前，我国的机器人技术也在稳步发展，正在缩小与发达国家的 差距。
[0003] 目前，机器人行走移动控制已经成为机器人领域中的一个重要分支，得到了人们 越来越多的关注。如何提高机器人移动时的灵活性、机动性，是机器人领域中人们研究的热 点。
[0004] 当前，地面机器人的移动方式主要分为腿足式、履带式、轮式等几种。腿足式主要 依托于仿生学，其模仿人、动物、昆虫等生物的行走方式，目前发展迅猛，其机体的自由度 高，可以适应复杂地形，甚至可以攀爬楼梯，缺点是速度较慢；履带式适用于较复杂地形，如 山地等，移动速度比腿足式快；轮式机器人移动速度最快，但是通常只能应用于较为平坦的 地形。
[0005] 对于轮式机器人，其大部分底盘采用的均为两轮驱动，即两个主动轮，其余的均为 从动轮，但是两轮驱动的底盘不能直接进行横向的平移，只能进行纵向平移、自身旋转，即 两轮底盘只拥有两个自由度，这种驱动方式大大地限制了机器人的灵活性。因而，为提高机 器人的灵活性，必须在此基础上进行改进。
【发明内容】
[0006] 本发明是一种四轮驱动全向底盘的控制装置及控制方法，采用本控制装置及控制 方法可以使装有四轮全向底盘的机器人沿任意方向直线行进，并且同时自身旋转，即在行 驶过程中可以随意调节自身的姿态，即有三个自由度，这种驱动方式较两轮驱动有着更强 的灵活性。
[0007] 四轮驱动相比两轮驱动的优点是灵活性高，自由度更多，但对于控制上要求也更 高，对于四轮运动的协调控制是控制上的一大难点。本发明的核心技术就是解决四轮运动 协调控制的这一难题。
[0008] 一种四轮驱动全向底盘的控制装置，包括主控模块（ARM和FPGA)，无线串口通信 模块，码盘计数模块，直流电机控制模块。无线串口通信模块用来接收来自与计算机连接的 无线串口通信模块的指令，指令信息包括四轮驱动全向底盘所要移动到的目标点的位置值 以及姿态角值，将接收到的指令传给上位机ARM ;FPGA将计数电路和信息采集电路融合，由 FPGA对码盘计数模块的数据进行采集与处理，然后通过通用I/O接口传输到ARM，同时FPGA 也作为接口电路，将ARM的控制指令传输到直流电机控制模块，完成对直流电机的转速以 及方向的控制;ARM是整个控制装置的核心控制模块，用以对接收到的数据进行融合，实现 对四轮驱动全向底盘的位姿进行实时结算，并完成对四个直流电机的实时控制。
[0009] 一种四轮驱动全向底盘的控制方法，包括如上所用的控制装置，以及用于该装置 的核心的控制算法。具体步骤如下：
[0010] 设定四轮驱动全向底盘所要移动到的目标点的位置值，即X、Y坐标，以及机体自 身所要转动的角度Θ，启动开始按钮，通过无线串口通信方式，四轮驱动全向底盘上安装的 主控模块接收到指令后，按照预先设定的四轮驱动全向底盘运动的控制算法，通过直流电 机控制模块，控制四个直流电机协调工作，采用的导航方式为四码盘导航方式，通过码盘计 数模块实时计数，可以记录下各轮行走的距离，将此数据传给主控模块，主控模块再对四个 直流电机进行实时的PID控制，实时计算当前运动轨迹与预设轨迹之间的偏差，不断的进 行偏差纠正，使整个机体沿预设的轨迹进行运动，实现在自动方向沿直线行走并且实时改 变自身姿态角。
【专利附图】
【附图说明】
[0011]为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现 有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本 发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可 以根据这些附图获得其他的附图。
[0012] 图1为本发明实施例提供的四轮全向底盘的结构示意图；
[0013] 图2为本发明实施例提供的四轮全向底盘运动方式图；
[0014] 图3为本发明实施例提供的四轮全向底盘运动学分析图；
[0015] 图4为本发明实施例提供的全向码盘轮安装示意图；
[0016] 图5为本发明实施例提供的控制装置原理图；
[0017]图6为本发明实施例提供的四轮全向底盘运动轨迹示意图；
[0018]图7、图8为本发明实施例提供的底盘运动偏差示意图；
[0019]图9为本发明实施例提供的PID控制原理图；
[0020] 图10为本发明实施例提供的四轮全向底盘程序控制流程图；
【具体实施方式】
[0021] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完 整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于 本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他 实施例，都属于本发明保护的范围。
[0022]图1为本发明实施例提供的四轮全向底盘的结构示意图。与两轮底盘相比，四轮 全向底盘有灵活性高的特点。两轮驱动的底盘不能直接进行横向的平移，只能进行纵向平 移、自身旋转，即两轮底盘只拥有两个自由度。四轮驱动底盘则可沿任意方向行走，并且也 可自身旋转，即四轮驱动底盘具有三个自由度，这相比两轮驱动来说灵活性更高。
[0023] 图2为本发明实施例提供的四轮全向底盘运动方式图。如图2所示，四轮全向底 盘沿任意方向直线移动，并且在此期间机体姿态改变，这是四轮底盘最为突出的特点，也是 控制上的绝对难点。这种移动的方式的优点在于，在执行复杂任务(即要求机体质心由A点 移动到B点，同时要求车体转动某个角度）时极大地缩短了机体的运动距离，在机体速度确 定的情况下可以缩短到达目的地的时间，而这项任务是两轮驱动的底盘无法完成的。
[0024] 图3为本发明实施例提供的四轮全向底盘运动学分析图。四轮全向底盘工作空间 为一平面，建立如图3所示的绝对坐标系xa-ya和四轮全向底盘的坐标系Xy-y Y，其中，四轮 全向底盘坐标系原点与四轮全向底盘中心重合。a为四轮全向底盘中心到四个轮间的距离， 中心从A点运动到B点距离为L，Θ为质心从A点至B点运动时期望偏离绝对坐标系横轴 \的角度，Y为运动结束时χ γ与\的期望夹角（即四轮全向底盘在运动结束时期望自转 的角度)，为运动过程中实时的自转的角度，为期望运动的速度及自转角速 度，L为所要求的轮子i提供沿驱动方向的速度。
[0025] 通过运动学分析，可得如下运动学方程：
[0026] - ~xa sin γ' + j>a sips+ αγ
[0027] v2 = -xa siny' + yg cos f -af
[0028] v3 = xa cos γ' + ya sin γ' -af
[0029] \!4 = xu cos γ' + yu sin γ' -\- &f
[0030] 上述四个方程，则是4个直流电机分别对应的转速方程，将尨、p期望运动 的速度及自转角速度代入上述四个方程中，我们即可得到4个直流电机所要控制的理论转 速Vi。
[0031] 因此，上述四个方程是本发明的核心算法。
[0032] 图4为本发明实施例提供的全向码盘轮安装示意图。如图所示，全向码盘轮上连 接一个增量型码盘计数模块，实时记录下码盘转动过的圈数，将此数据传到主控模块，对全 向轮走过的距离进行结算。
[0033] 图5为本发明实施例提供的控制装置原理图。如图5中所示,无线串口通信模块与 ARM相连，用来接收来自与计算机连接的无线串口通信模块的指令，指令信息包括四轮驱动 全向底盘所要移动到的目标点的位置值以及姿态角值，将接收到的指令传给上位机ARM; FPGA与ARM相连，将计数电路和信息采集电路融合，由FPGA对码盘计数模块的数据进行采 集与处理，然后通过通用I/O接口传输到ARM，同时FPGA也作为接口电路，将ARM的控制指 令传输到直流电机控制模块，完成对直流电机的转速以及方向的控制;ARM是整个控制装 置的核心控制模块，用以对接收到的数据进行融合，实现对四轮驱动全向底盘的位姿进行 实时结算，并完成对四个直流电机的实时控制；码盘计数模块与FPGA相连,当码盘轮转动1 圈时，码盘计数模块会记录1200个数，因此可通过码盘计数模块实现对码盘轮行走的距离 进行记录，再通过码盘轮与驱动轮的半径之比，则可得到驱动轮的行走距离；电机控制模块 与FPGA相连，其作用是对直流电机的电流环进行闭环控制。
[0034] 图6为本发明实施例提供的四轮全向底盘运动轨迹示意图。如图9所示，按一定 周期对车的轨迹微分，本周期初始时刻，车的位姿为（xl，yl，thetal)(已知)，结束时车的位 姿为（x2，y2，theta2)(未知)。在这个周期内左轮的位移为sl，右轮的位移为 sr，前轮的位 移为sh，后轮的位移为sb，由于四轮全向运动时各轮的侧滑严重，码盘记的位移只沿着各 个轮的正方向，不会记录各轮侧滑而产生的位移，所以码盘记的各轮位移（dl，dr，dh，db)是 各轮实际运动方向的分量(轮子前进方向的分量)，根据实验结果，发现按以下公式解算车 的质心的位移较为精确：
[0035]
【权利要求】
1· 一种四轮驱动全向底盘的控制装置，其特征在于包括主控模块（ARM和FPGA)，无线 串口通信模块，码盘计数模块，直流电机控制模块；无线串口通信模块用来接收来自与计 算机连接的无线串口通信模块的指令，指令信息包括四轮驱动全向底盘所要移动到的目标 点的位置值以及姿态角值，将接收到的指令传给上位机ARM ;FPGA将计数电路和信息采集 电路融合，由FPGA对码盘计数模块的数据进行采集与处理，然后通过通用I/O接口传输到 ARM，同时FPGA也作为接口电路，将ARM的控制指令传输到直流电机控制模块，完成对直流 电机的转速以及方向的控制；ARM是整个控制装置的核心控制模块，用以对接收到的数据 进行融合，实现对四轮驱动全向底盘的位姿进行实时结算，并完成对四个直流电机的实时 控制。
2. -种四轮驱动全向底盘的控制方法，其特征在于具体步骤如下： 设定四轮驱动全向底盘所要移动到的目标点的位置值，即X、Y坐标，以及机体自身所 要转动的角度Θ，启动开始按钮，通过无线串口通信方式，四轮驱动全向底盘上安装的主控 模块接收到指令后，按照预先设定的四轮驱动全向底盘运动的控制算法，通过直流电机控 制模块，控制四个直流电机协调工作，采用的导航方式为四码盘导航方式，通过码盘计数模 块实时计数，可以记录下各轮行走的距离，将此数据传给主控模块，主控模块再对四个直流 电机进行实时的PID控制，实时计算当前运动轨迹与预设轨迹之间的偏差，不断的进行偏 差纠正，使整个机体沿预设的轨迹进行运动，实现在自动方向沿直线行走并且实时改变自 身姿态角。
3. 根据权利要求1所述的一种四轮驱动全向底盘的控制方法，其特征在于无线串口通 信模块与ARM相连,用来接收来自与计算机连接的无线串口通信模块的指令，指令信息包 括四轮驱动全向底盘所要移动到的目标点的位置值以及姿态角值，将接收到的指令传给上 位机ARM ;FPGA与ARM相连，将计数电路和信息采集电路融合，由FPGA对码盘计数模块的数 据进行采集与处理，然后通过通用I/O接口传输到ARM，同时FPGA也作为接口电路，将ARM 的控制指令传输到直流电机控制模块，完成对直流电机的转速以及方向的控制；ARM是整 个控制装置的核心控制模块，用以对接收到的数据进行融合，实现对四轮驱动全向底盘的 位姿进行实时结算，并完成对四个直流电机的实时控制；码盘计数模块与FPGA相连，当码 盘轮转动1圈时，码盘计数模块会记录1200个数，因此可通过码盘计数模块实现对码盘轮 行走的距离进行记录，再通过码盘轮与驱动轮的半径之比，则可得到驱动轮的行走距离；电 机控制模块与FPGA相连，其作用是对直流电机的电流环进行闭环控制； 按一定周期对车的轨迹微分，本周期初始时刻，车的位姿为（xl，yl，thetal)(已知)， 结束时车的位姿为（x2, y2, theta2)(未知）；在这个周期内左轮的位移为sl，右轮的位移 为sr，前轮的位移为sh，后轮的位移为sb，由于四轮全向运动时各轮的侧滑严重，码盘记 的位移只沿着各个轮的正方向，不会记录各轮侧滑而产生的位移，所以码盘记的各轮位移 (dl，dr, dh, db)是各轮实际运动方向的分量(轮子前进方向的分量)，按以下公式解算车的 质心的位移： A_ idz+m2 车转过的角度为： ι(Φ~0 dh-db'' ~~+~?~ 式（4. 27)中，k为车左右轮的码盘间距，Lbh为车前后轮的码盘间距； 由Λ Θ可以求得本周期结束时车的角度为： theta2=thetal+Δ Θ 当这一段位移足够小时，轨迹可以近似成一条与水平方向夹角为VelTheta的直线；其 中： Veltheta=thetal+arctan((db+dh)/(dr+dl)) 则有： x2=x1+d>l<cos (Veltheta) y2=yi+d*sin(Veltheta) 至此，由（xl，yl，thetal)推出了（x2, y2, theta2)，在平面运动中，知道了机体的初始 位姿，并记录了机体的各轮的位移，则四轮全向底盘在任意时刻的位姿都可以得到； 在四轮全向底盘进行直线运动时，需要给定目标的坐标点和直线与场地坐标系X轴正 方向的夹角，这样就可以将此条直线确定下来；机器人根据这条目标直线和当前自身的坐 标值和航向角，判断与目标直线的位置和角度偏差，同时控制四轮差速修正行进轨迹； 在四轮全向底盘轨迹的修正过程中，需要调节的两个偏移量为横向偏差和航向偏差， 如果想同时对这两个变量进行调节，则情况会比较复杂，需要根据机体在不同的情况给调 节参数赋予不同的符号控制电机差速调节； 为了同时调节四轮全向底盘行进的横向偏差和航向偏差，提高直线行走收敛的快速 性，减小超调，引入一个前置探测点，即在机体行进方向的前方某一位置上，放置一个虚拟 点，计算此点与目标直线的偏差，并根据这个偏差设置合适的反馈增益来计算修正的四个 轮的差速值，修正机体行进的直线轨迹；前置探测点长短的选择和反馈增益系数需要根据 机器人实际行走的调试效果来整定，通过加入前置探测点，可以将横向偏差和航向偏差整 合在了一起，同时相当于引入超前校正环节,可以提高行走精度；对于机体运动时姿态角的 控制，只要在这个基础上加入姿态角的偏差修正即可； 位置型PID控制算法的基本算式为： n{k) = Kr \e(k) + - φ-m 增量型PID控制算法的基本算式为： Δ u (k) =KP {[e (k) -e (k-1) ] +Κχθ (k) +KD [e (k) -2e (k-1) +e (k-2) ]} 其中，u(k)--第k次采样时计算机输出值；e(k)--第η次采样时的偏差值； Au(k)--第k次采样与第k-1次采样的计算机的差值； 通过观察上式可知，位置式PID算法输出与整个过去状态有关，计算式中要用到过去 偏差的累加值，容易产生较大的积累误差，容易产生积分饱和；而增量式只需计算增量，当 存在计算误差或精度不足时，对控制量计算的影响较小； 当执行部件不带积分部件，其位置与计算机输出的数字量是一一对应(如电液伺服 阀)，采用位置式PID控制算法；若执行部件带积分部件(如步进电机、步进电机带动阀门或 带动多圈电位器）时，通常选用增量式PID控制算法； 底盘的行进过程共有三个阶段，分别为加速阶段、匀速阶段和减速阶段；在每个阶段中 都要实时计算与目标点的距离，若达到目标点就停止；在加速段中，可采用匀加速的控制方 式，但首先需设定好一个最大速度，当速度达到最大速度时，则进入匀速区，进入匀速区后， 需检测是否到达减速区，当进入减速区时，为了避免由于突然减速带来的电机振动问题，减 速区减速的方式采用的是离目标距离与减速区长度之比乘以当前速度，得到一个平滑的衰 减过程，最终的速度会缓慢的到达零；这种减速的效果不错，减速也很快；实际过程中，考 虑到时间的要求以及防止电机低速转动，当速度减到10%的最大速度即以此勻速速度行 走，直到到达目标点停止或者直接在程序上进入下一段路径。
【文档编号】G05D1/02GK104216406SQ201310219764
【公开日】2014年12月17日 申请日期:2013年6月5日 优先权日:2013年6月5日
【发明者】滕鑫淼 申请人:中国石油天然气集团公司, 中国石油集团钻井工程技术研究院