AGV运动学模型

第二章自动导引小车运动学模型2.1自动导引小车的结构特点自动导引小车的研究涉及多种技术领域，是一个典型的机电一体化多技术多 学科的集成系统，其机构示意图如图 2-1自动导引小车的机械机构部分主要包括如下几个方面：(1) 车体车体由车架、相应的机械电气机构、外观件等部分组成，它是自动导引小车的基础部分。车架的设计需要考虑刚性强度、整车的平稳性等重要的 机械性能，重心的位置是又一关键因素，重心越低越有利于抗倾翻。在满足车载 机械电气机构、外观件以及其它装置装配空间，和整车刚性要求的条件下，尽量 考虑整车的外形造型美观和轻便小巧。(2) 驱动装置 驱动装置是自动导引小车行走的执行机构，它主要由车轮、 减速器、制动器、电机及速度控制器等部分组成。通常情况下，驱动装置和转向 装置集成在一起使用。(3) 蓄电池和充电装置 蓄电池和充电装置是自动导引小车的动力源。自动导引小车一般采用24V或48V直流工业蓄电池电能为动力，对于传统的铅酸蓄电池， 一般需保证8小时以上的安培小时值。随着电池科技的发展，快速充电蓄电池问 世，这类蓄电池的安培小时值根据实际生产需求而定，而且与之配套的采用一种先进智能快速充电技术，充五分钟电可以使用一个多小时。从而提高自动导引小 车的有效使用率。充电装置有多种，目前最常用有地靴式和测挂式等。一般地， 充电装置需要安全保护。(4) 位姿传感装置 位姿传感装置主要是为了从自动导引小车的当前环境中，获得小车的位姿(位置与转角)和其它相关的信息，如运行前方有无障碍等。 位姿传感装置会因为采用的导引技术的不同而不同，如采用电磁感应技术的位姿传感装置主要是安装在小车上的一对探头(即感应线圈)和比较/放大电路等，而 采用光学检测技术的位姿传感装置则主要是光学检测器和辅助装置等。(5) 导向控制装置导向控制装置是整个导引小车运动控制的核心，主要色含有硬件部分和软件部分。一般来说，尽管采用的导引技术千差万别，但是，导向控制装置的结构大体相同。硬件部分主要是数字电路部分，主要是位置环、垢差控制 器等，多采用单片机实现，从而可以通过程序方便的控制自动导引小车跳加速、 减速和匀速运动，需要的话也可以切换偏差控制器实现直道、弯道的多棒型控制 同时由于自动导引小车行走过程中对实时性要求较高，对包括速度环、电流环及驱动部分的控制器及脉冲宽度调制(Pulse Width Modulation)信号发生器而言，采用模拟电路控制具有实时快速、 不占用CPU寸间的优点，特别适合电流，速度 环快速PID运算。(6) 专用功能执行机构自动导引小车的形式种类很多，每种实用型自动导引小车，它都不光是装备了自动导引行走功能， 还装备了专用功能执行机构，以满足 自动导引小车的实际功能需求，如叉车式自动导引小车的自动叉体机构， 转道式 自动导引小车的自动辊道机构等。专用功能执行机构是根据各种自动导引小车需 要完成的功能专门设计而成，具有特定专用性。(7) 安全保护装置在实际生产中，人与机械处于同一环境，自动导引小车作为一种运动中的动作功能机械，其安全保障功能尤为重要。安全装置的作用包括防止 设备在运行中出错，也预防运行出错对人员及其运行环境设施产生的影响，直接地，安全装置的功能就是保护自动导引小车自身，以及维护自动导引小车攻用的顺利完成，同时在最大可能的范围内保护人员和运行环境设施的安全。自戴导引小车的安全装置主要是障碍检测传感装置以及避障控制装置。本论文研究对象是车体采用三轮式结构的自动导引小车，车体结构示意图如图2-2所示。两个后轮作为驱动轮，利用两轮的速度差实现 转向；前轮为随动轮，仅起到支撑车体的作用， 而没有导向的作用。采用这种结构的自动导引 小车只有直线运动和圆弧运动两种运动形式， 较适合于固定路径导引控制和运动路径不是 很复杂的导引控制中。此车体正向运行和反向运行的动力学模 型是不同的，可以只按车体正向运行情形求取 其动力学模型。如果要求所求取的动力学模型 对于车体正向运行与反向运行均适合，那么该11-Mfe A左枪 A右絵4-车探图卜2车体结构示宜图车体结构必须前后对称，即前轮为操舵轮，中间两轮是驱动轮，后轮也为操舵轮 的变四轮结构(如图1-2 )。实际运行中，如果需要反向运行，只需要将自动导 引小车原地旋转180度以后，就相当于正向运行了。2.2.自动导引小车运动学模型的建立2.2.1自动导引小车运动学分析运动学模型主要根据运动学方面的理论直接求得伺服电机端电压与车速的 关系，在这种模型中，自动导引小车的速度只与电机端电压及电机电枢回路的时 间常数有关，而忽略了车体质量、摩擦阻力对车速的影响。建立如图2-3所示的坐标系。图中XOY为世界坐标系，v, , v，分别为左/ 右驱动轮轮心相对于地面的线速度 (m/s), L为两驱动轮之间的距离，R为 自动导引小车绕瞬心口的转弯半径，必为自动导引小车绕瞬心口转动的角速 度(rad/s )自动导引小车的运动学模型是基 于以下几点假设：自动导引小车是刚 性的；自动导引小车运动在水平面上；自动导引小车的左、右轮受 丫力相等，且车轮与地面之间没有相对滑动;自动导引小车的车体质量、摩擦阻力、负载等的变化对车速的影响忽略不计。取，为距离偏差（m ）, 为角度偏差（rad ），在此设定角度以逆时针为正,(2-1)(2-2)(2-3)顺时针为负。根据自动导引小车的车体结构和刚体平动原理可知， 自动导引小车 在任意时刻都是作绕车体瞬心 0的转动。由图2-3可知，在t时刻自动导引小车 绕瞬心口的转弯半径R为：L V, +v,自动导引小车在t时刻绕瞬心口转动的角速度必为M + v,a =-2R则两轮中七的速度#在斗y坐标方向的分旬分别为: 叫=vsintf化=vcos对式（2-2）、（2-3）积分可以得到;x =| vsin （2-4）尸儿 + vccsOdt式中0小工九为小车的初始方位角和初始位置.由式（2-4 ）可以看出，只要正确标定小车的初始位置，控制小车左右两驱动轮的速度，可以使两轮的中心跟踪任意给定的运动轨迹，也就是说通过分别控制小车两驱动轮的速度可以使小车实现路径跟踪控制。A p假设经过时间一后，自动导引小车产生的角度偏差量为.，距离偏差量为吐“。则自动导引小车运动方程为:2(2 - 5)* * * * * (2 - 6)如果自动导引小车在运动过程中，角度偏差-较小，由式（2-5）可以得到角度偏差的变化率为：df 2RL式中：V 跟踪路径曲率变化对角度偏差变化率的影响,（2 7）当跟踪路径为直线时,其值为零加ngg + Ac/同样的，由式（2-6）可以得到距离偏差一的变化率为:(2-8)式中： X -踪路径曲率变化对距离偏差变化率的影响，当跟踪路径为直 线时，其值为零。令了-冷十叮厂：一.注八由公式（2-7 ） , （2-8）即可以得到状态变量为-的自动导引小车的状态方程为（2-9）描述自动导引小车左右轮速差卞乍期蜃痕茶J 4臥离镶芍J上网的关系。它可以看成是以小车角度偏差，和距离偏差为状态变量，以车辆 左右轮速差；为输入变量的状态方程，即可以通过控制自动导引小车左右轮的 速度差来调节小车的角度偏差：。和距离偏差1 -，但由于左右轮速差 不是系统的直接输入量，系统的控制输入量为工业控制机送出的D/A值，即电压值,所以为了将控制电压作为输入加到状态方程中，需要将小车的运动学方程与反映 驱动系统动态特性的传递函数合并，得到描述整个被控过程动态特性的系统状态 空间数学模型。2.2.2驱动系统数学模型的建立驱动系统的数学模型描述着计算机D/A输出值（即电压值）和驱动轮转速之间的关系，设计自动导引控制器及进行仿真和试验研究都需要建立能够正确地反 映车辆驱动系统特性的数学模型。自动导引小车的驱动系统如图2-4所示。工业机计数器传感器图A4昭动系统示童图由于AGV驱动系统环节较多，例如，电机调速机构包括D/A转换、电机控制 器、减速器、控制电机等；而且各部件的有关性能参数难以确定，如机械传动机 构的刚度、阻尼、纵向运动惯量等机械性能参数都不便于测定，因此采用解析法建立车辆调速机构数学模型的困难很大。由于系统的输入输出信号一般总是可以 测量的，而系统的动态特性必然出现于这些输入输出数据中，故可以利用输入输出数据所提供的信息来建立系统的数学模型，这种建模方法就是系统辨识。随着 系统辨识理论与方法的发展，应用系统辨识的方法，通过实验研究来确定系统的 数学模型，是一种能满足上述要求的行之有效的途径。为了对驱动系统动态特性进行初步分析并便于以后的系统辨识，需要首先通过对其进行特定的输入信号(如阶跃输入)下的动态响应过程试验，从而可以根 据试验结果判断出驱动系统模型的阶次。 另外，由于输入输出信号均为离散数字 信号，因此宜采用差分方程描述该系统。用差分方程定量描述一个动态系统时， 必须确定出方程中的有关参数。所以，该种系统辨识的实质是一个参数估计问题， 可视为一种灰箱式部分辨识问题，同时，辨识过程中由于输入输出数据受到噪声 的影响，一般应看成是随机变量，因此也属于统计学范畴。在参数估计时，力求使某一个被适当定义的误差标准趋于最小，以便使寻求的数学模型与试验数据有最佳拟合。 在各种参数估计技术中，最小二乘法是从试 验数据进行参数估计的主要手段，其获得的估计在一定条件下具有最佳的统计特 性，因此该方法被广泛应用于系统辨识研究中。在系统辨识中，输入信号的类别和形式影响着所采用的辨识方法和辨识精 度。用于辨识输入信号的最低要求是具有持续激励特性，即在整个观测周期上， 过程的所有模态必须被输入信号持续激励。这意味着输入信号不能随意选择，否 则不但辨识精度不能保证，甚至可能造成不可辨识。目前常用的信号主要是随机 序列(如白噪声)和伪随机序列。理论分析表明，选用白噪声作为辨识输入信号可以保证获得较好的辨识效 果，但是白噪声在工程上不易实现，因此工程中一般选用最长线性移位寄存器序 列(简称M序列)作为辨识输入信号。M序列是二进制伪随机码序列(PRBS)的一种 形式，它的自相关函数接近脉冲函数，具有近似白噪声的性质，可保证有较好的 辨识精度。对驱动系统的模型辨识而言，M序列的物理意义是驱动电机电源控制器输入 电压信号，它在计算机内由D/A产生。M序列有3个参数，即一个电平的持续时 X.、咅期&裳叭农爲股耳选择M序列就是确定这几个参数。试验开始时，应先给系统以预激励(或预扰动)。由于M序列u(k)实质上是 周期信号，其施加于系统后，输出端响应 y(k)最终也必定是周期波形。不过在 t=0Ts的过渡过程初始阶段内，由于非零初始条件的作用，系统的输出在一段 时间内是非平稳的，为了保证辨识精度，要避开这段非平稳过程。由于已选定N心沁，当M序列的第一个周期己度过，y(k)已成为周期为巾讥心的周 期信号后，即可认为y(k)已具备平稳性，此时进行。(k)和y(k)的相关运算刁有 意义。因此在辨识试验中将第一个周期内的 M序列作为预激励，而将第1个周期 以后采集的试验数据留作系统辨识之用，这样做的目的是利用比较充足的数据， 以提高辨识精度。系统辨识中，取电压信号u(k)序列作为输入值，小车速度信号y(k)序列作为输出值进行辨识。通过研究电压值与驱动轮转速的相互关系建立描述驱动系 统动态特性的差分方程。驱动系统的数学模型选用 ARX模型，模型为：A(z)z(k) =+(2-10)其中：期)二1 + %十+ 口肿5(2 *) = 1 + ,2 1 4 +如严利用最小二乘方法进行系统辨识，可得描述驱动系统输入输出关系的差分模 型。而描述线性连续系统的数学模型是微分方程和传递函数，必须把差分方程转换成微分方程或者传递函数的形式。本文采用差分方程与微分方程系数的转换法 实现f7la n 阶微分方程_ dy(f) _ 小 r dn-lu(t)r“听-7 +务冶一】+&(0=妬由”十+占評(2_ii)与X阶差分方程叫，(女+町4吋(上+甘亠口 +十叫巩北)=jW(l + n -1) + 6rtu(i) (2 -12)之间的系数转换关系为（2 13）式中(2-14)D“和D*是一个下八%nt巾笋治，忑F际洛洛上t耳珂卡门务，五了D“和2,分别是口+1阶和m阶方阵，即转换矩阵三角矩阵，其对角线商的元素之绝对值及最后一行的元素的绝对值都是1;矩阵中每个元素1 决定；矩阵中绝对值不等于1的其它非零元素的 绝对值由下式计算：I叫卜必” |+|a” |(2 一利用Matlab编程实现最小二乘方法系统辨识和模型转换算法，得到驱动系 统频域形式的传递函数为：G（） =0.0099+0*5924$+ 0.557?+1.779 + 0,768为了将驱动系统的动态特性方程加入到系统状态方程中，需对驱动系统进行 降阶，即把三阶传递函数降为一阶形式，从而近似简化成惯性环节和比例环节， 具体计算采用Matlab工具软件编程实现，可得驱动系统传递函数的一阶形式为 模型为：(2-17)如=5+0.8025用微分方程表示输入输岀关系为：= -OJ025r(/) + 0.5791u(/)(218)令0 = 0.8025 = 0.5791,将上式写成增貴方式的微分方程：(2-19)虽然对模型降阶处理使模型与实际系统有差距，但是这样可以把驱动系统的微分方程加入到状态方程中，对建模是有利的。式(2-14)中,t可以理解成 左右轮的速度差，y可以理解成计算机给左右驱动电机电源控制器的输入电 压之差。2.2.3自动导引小车控制系统状态空间方程由于自动导引小车采用控制两驱动轮的转速差实现路径跟踪，即当自动导引小车在跟踪路径有偏差时，采用分别将两个驱动轮加、减一个纠偏的速度，而且已经把驱动系统的动态特性简化成一阶系统，因此可以把工业控制机的D/A输出作为控制器的输入， V作为模型中的状态变量，.分别为左、右电 机的控制电压(定义右轮的电压为正)，则自动导引小车的控制结构图如图 2-5。以自动导引小车的左右轮转速差角度偏差和距离偏差作为系统状态变量，工业控制机的控制信号 x作为系统输入，可以得到被控系统状态空间 方程为：仏V j击-a0hV0de/di1 / 00%+0(2 - 20 )0Vc0L 5 一0L Jxx = Ar % 为状态变量，加为系统谕入，则状态方程町以写成：x= Ax5Aw(2-21)式中* 1-Q00bA =ML001B 二00Vc00将车辆距离偏差町作为输出，聚统的输出方程为：y (Jx 4- ZAi/* (2 * 22)式中：C = o 0 j 、 D = o至此，己建立了系统被控过程的状态空间数学模型，也可记作(A, B, C, D)。因此自动导引小车的运行轨迹与距离偏差 e和角度偏差.，这两个参数 有关，它们之间存在着非常复杂的非线性关系。此外，考虑到自动导引小车的车身中线的实际偏转角度要比车轮的旋转角度要小。因此，系统在角度偏差较小， 且离控制线距离较近时，旋转灵敏度要高：而在角度偏差较小，且离控制线较远 时，旋转灵敏度要低。2.3自动导引小车导引方程231 AGV自动导引技术概述顾名思义，自动导引小车包含一个自动导引系统， AGV就是依靠它沿一定的 路线自动行驶的。不同类型的AGV系统中采用的自动导引技术各不相同， 而采用 哪一种导引技术将直接影响自动导引小车系统各方面的性能。根据AGV系统中AGV运行路线的性质，导引系统可分为固定路径导引、自由 路径导引、组合路径导引三种：(bU)图27峪V辱引系甌董型固定路径导引是指AGV运行路线是以某种具体的形式规定的，如图2-6 (a)。 具体的路线可以是电磁感应导引中的导引电缆、 磁导引中的磁条和光学导引中的 反光带等。自由路径导引是指AGV的运行路线是无任何具体形式的运行轨道，AGV沿虚 拟的路线运行，如图2-6 (b)。这种虚拟的路线由控制系统间接通过一些指示装 置来确定，计算机视觉等导引方式均属此类。组合路径导引是指AGV在多数工作区间内沿某种具体形式的固定路线运行，而在某些区域可沿控制系统指定的虚拟路线运行，如图2-6 (c)，两种路径分别用实线和虚线来表示。一般综合使用上述两类导引方式中的不同导引技术。一般而言，固定路径导引实施较容易，技术较为成熟，但运行路线的更改相 对较为困难。自由路径导引的成本较高，同时在实际应用中还有一些具体问题需 要解决，但AG运行路线的更改容易，柔性较高。组合路径导引可综合上两类导 引系统的优点。a 线路径导引示St图b園弧段齬径异弓I禾憊團图2T导引控制坐标示意图232 AGV自动导引方程自动导引小车的运动路径一般都是直线与圆弧组成。下面就分别讨论自动导 引小车在直线段与圆弧段的导引方程。2. 3. 2. 1直线路径导引方程AGV在直线路径上导引示意图如图2-7 (a)，其中，Y轴与路径重合。% % 蚀分别表示左右轮转速和车体转速，用为车体转角，L为驱动轮中心距,R为驱动轮半径，B为传感器中心距两驱动轮中心连线的距离。则车体转动角速度为：少=(甜一吋、R4(2-23)令车体初始角度为 匕，车体相对于参考坐标系丫轴的转动角度为：(2-24)且两驱动轮中心点C点处的瞬时线速度为】牛=(&r/t/2(2-25)该点处壮y方向速度分量分别为= vr sin 0 ,= vc * cos令C点初始塑标为兀儿厂则该点坐标为： 咚=% +卜鼻，儿=沙+乙也传感器中心点M点位置坐标为，xm =+ Ssin6? yu = y + B cos 6由于在直线段时，y轴可以取与路径重合*礼。就是AGV导引的初始偏差，而工八入的值分别是Q点和M点的导引偏差，因此其備差为：At =x皿=几=%+恥in(虬孑皿)曲)(2-29)式2-28和2-29即是AGV在直线路徨时的导引方程。23 3. 2圆弧段路径导引方程设弯曲段为一半径为；的圆弧，中心坐标为 ，两驱动轮中心点C点坐 标为|(打，儿)，取丫轴与路径直线段重合，直线段与圆弧切点伪坐标原点(0,0) 那么，圆弧中心坐标为(r,0) C点初始坐标为心AGV导引的初始偏差就是C点的横坐标O由图2-7 (b)的几何关系可以得到：由图2-7(b)的几何关系可以得到；兀二兀弄审夕+咕曲 怙曲-儿弄曲xM = xt + flsin T yn = yc + BcosJ式中各表达式的意义与上一节相同占因此，圆弧路段的导引偏差表达式是; Af = r- J(兀 7(/)2 中(人一片丫 -(xf -r)2 +(2-30)辱-J( -琮)十(儿-加 二尸“ JgF +元(2-31)2.4本章小结本章介绍了自动导引小车的结构特点，并根据AGV勺结构特点及与路径相对 运动的关系建立了小车的运动学模型。 通过分析了小车控制结构特点，将通过系 统辨识获得的自动导引小车驱动系统的传递函数合并，得到了描述整个被控过程的动态特性的系统状态空间数学模型。最后分析了自动导引小车在直线段与圆弧 段的导引方程。