无人自走车的导引控制方法与流程

1.本发明提供一种路径导引方法，特别是指一种单舵轮无人自走车的导引控制方法。


背景技术：

2.现今全球少子化浪潮所导致的劳动力资源短缺与人力成本逐年提升，并逐渐由劳力密集转型成技术密集的产业，基于各项营运成本不断升高，要如何降低各项的成本，已成为企业是否能获利的关键，而随着自动化科技的导入、物联网和人工智能的快速发展，使智能制造与智能工厂已逐渐应用于工业生产端与制造端，也有越来越多的任务被工业机器人所取代，以解决劳动力资源短缺的问题。
3.然而，自动导引车(automatic guided vehicle；agv)或称无人搬运车，指的是配置有电磁式或光学式等自动导引装置，并集合了环境感知、路径规划决策及无人自动操控等功能的运输车，属于轮式移动机器人(wmr-wheeled mobile robot)的范畴，主要功能表现为在计算机或车载系统的监控下，按照路径规划和作业要求，自动行走并停靠到指定地点或工作站，并完成一系列作业功能，一般自动导引车可通过车载系统或计算机控制其行进的路线，或是可利用墙壁、支柱或地面上沿着其行进的路线设立的指示标记(如电磁道道、具有反光特性的反光片、涂漆或色带等定位标志)作为导引，并在自动导引车上装设电磁式或光学式传感器(如电磁传感器、视觉传感器、超声波传感器或激光传感器等)，以侦测指示标记作为车辆运行的定位及位置修正，除了可使车体沿着预定规划的导引路径自动行驶之外，且因活动区域内无需铺设轨道、支架等固定装置，不会受到场地、道路和空间的限制，故自动导引车被广泛地应用于物料的自动运输、仓库的监视巡逻与有害场所的作业等。
4.传统的自动导引车大多数的路径规划为网格式点对点间相连接的线所组成的路径，并利用上述的导引方式使自动导引车能沿着预定的路径前进，不过上述的导引方式需使用大量复杂的运算来撷取环境中的实体标记或特征物，才能确定自动导引车应走的方向与速度，运算处理周期较长，反而降低了整体的导航效率，且该路径规划也并非平滑曲线，导致自动导引车在行进的过程中会有较不平顺的转弯，其实际行进的路径会与预定的路径产生偏离，便需要不断地进行位置及航向的比对与修正，即为此行业所亟欲研究改善的方向所在。


技术实现要素：

5.本发明的主要目的在于无人自走车的车体包含一个用于驱动与控制转向的舵轮及至少两个辅助转轮，并由自动导引装置对车体进行位置与姿态的定位，以及生成预定规划的目标路径，当自动导引装置取得车体中心的位置(如坐标与姿态角等)建立车辆坐标系，并进行坐标系转换建立本地坐标系，以计算车体中心至预定规划的目标路径其中一目标点的最短距离，车体中心与目标点间的夹角，以及车体中心至目标点的旋转半径后，再计算出舵轮转向至目标点所需的旋转半径及转角，使转向驱动系统可根据该转角来控制舵轮
转向至对应的位置，从而实现对无人自走车依循预定规划的目标路径运行的导引控制，且不需大量复杂的运算或较长的处理周期，可有效提升整体的导航效率。
6.本发明的次要目的在于当转向驱动系统完成舵轮转角的控制转向，可由自动导引装置计算出车体的当前位置、旋转半径与目标路径间的误差量，并根据该误差量采用pid控制可得到车体修正后的速度及旋转半径，再采用逆运动学以反推的方式计算车体移动至目标路径所需的速度或加速度，使转向驱动系统可控制舵轮来修正调整车体的当前位置与旋转半径，直到完成该车体的路径导引控制。
附图说明
7.图1为本发明无人自走车系统的示意图。
8.图2为本发明较佳实施例的步骤流程图。
9.图3为本发明车体的位置与姿态进行坐标系转换的示意图。
10.图4为本发明车体控制舵轮转向驱动的算法示意图。
11.图5为本发明目标路径闭回路导引控制的方块图。
12.图6为本发明修正车体转弯的旋转半径的示意图(一)。
13.图7为本发明修正车体转弯的旋转半径的示意图(二)。
14.图8为本发明车体进行直线控制的示意图。
15.图9为本发明车体相对于目标路径的姿态方向的示意图。
16.图10为本发明车体导引至下一个目标点的示意图(一)。
17.图11为本发明车体导引至下一个目标点的示意图(二)。
18.图12为本发明车体切换目标点的长度的示意图。
19.附图标记说明：1-无人自走车；11-车体；111-舵轮；112-转轮；12-自动导引装置；121-传感器模块；122-路径规划单元；13-转向驱动系统。
具体实施方式
20.为达成上述的目的及其功效，本发明所采用的技术手段及详细构造，兹绘图就本发明的较佳实施例来详加说明其构造与功能如下，以利完全了解。
21.请参阅图1～图4所示，分别为本发明无人自走车系统的示意图、较佳实施例的步骤流程图、车体的位置与姿态进行坐标系转换的示意图及车体控制舵轮转向驱动的算法示意图，由图中可清楚看出，本发明的无人自走车1包括有车体11、自动导引装置12及转向驱动系统13，并于车体11下方车轮模块包含一个舵轮111(即驱动与控制转向的主动轮)及至少两个转轮112(即承载或辅助转向的从动轮)，且自动导引装置12以通讯接口接收到控制管理中心下达的任务指令后，可通过车载系统或车载控制器操控转向驱动系统13驱动车轮模块，使车体11依循预定规划的目标路径运行，以构成一自动导引车(agv)、自主移动机器人(automated mobile robot，amr)或移动载具等。
22.在本实施例中，无人自走车1的车体11是以前方一个舵轮111用于驱动及控制转向，并配合车体11后方两个转轮(如货叉轮)112用于承载或辅助转向，为了使车体11具有更好的稳定性，亦可安装有两个以上的转轮112来提供承载的货叉起到支撑的作用，以构成一适用于重载货物搬运或移载的叉车式自动导引车，但并不以此为限，亦可为拖板式或堆高
机式的自动导引车。
23.此外，无人自走车1的车体11使用的舵轮111可为卧式舵轮或立式舵轮，并包含驱动轮、驱动单元(如驱动电机、齿轮箱等)及转向机构(如转向电机、编码器等)组成，同时带有驱动及控制转向，可以两个自由度来实现车体11的直线移动与转向功能，而自动导引装置12包含传感器模块121及路径规划单元122，其中传感器模块121包含装载在车体11上的内部传感器[如编码器、惯性测量单元(inertial measurement unit，imu)等]及外部传感器[如激光传感器、光学雷达(light detection and ranging，lidar)扫描仪、超声波(sonar)传感器或3d视觉传感器(3d camera)等]，并由内部传感器对车体11进行位置与姿态的定位，使自动导引装置12可在此一定位的基础上利用外部传感器获取的环境信息进行位置或姿态的修正，又路径规划单元122为采用算法依预先设定规划车体移动的路径并进行导航/导引，且该路径导航/导引控制的方式可为固定路径或虚拟路径，以供转向驱动系统13可依循预定规划的目标路径来驱动舵轮111，从而实现对车体11的定位及位置控制。
[0024]
具体而言，无人自走车1固定路径导航/导引控制是利用移动路径上设立的实体标记(如电磁道道、磁带、反光片等)作为导引，并由自动导引装置12的传感器模块121侦测标记对车体11进行位置与姿态的定位，以沿着路径规划单元122预定规划的目标路径运行，包含但不限于直接坐标导引(即笛卡尔坐标导引，cartesian guidance)、电磁导引(wire guidance)、磁带导引(magnetic tape guidance)或光学导引(optical guidance)，而无人自走车1虚拟路径导航/导引控制则没有存在实体标记，为将车体11移动路径的配置图数据存放在数据库或自动导引装置12内的地图库路线数据，并由传感器模块121对车体11进行位置与姿态侦测，使路径规划单元122自行决定预定规划的目标路径，包含但不限于惯性导航(inertial navigation)、激光导航(laser navigation)或超声波导航、视觉导航(visual navigation)或地理导航[如全球定位系统导航(global position system)]，该无人自走车1路径导航/导引控制的方式很多，在此则不作一赘述。
[0025]
如图2所示，本发明上述无人自走车系统所采用的路径导引方法，包括下列的实施步骤：
[0026]
(s101)无人自走车1的自动导引装置12先取得车体11中心的位置在全局坐标系中建立车辆坐标系，并进行坐标系转换以建立本地坐标系。
[0027]
(s102)计算车体11中心至预定规划的目标路径其中一目标点的最短距离，并根据几何关系计算车体11中心与目标点间的夹角，以及车体11中心至目标点的旋转半径。
[0028]
(s103)计算车体11的舵轮111转向至目标点所需的旋转半径及转角。
[0029]
(s104)转向驱动系统13根据计算得到的转角控制车体11的舵轮111转向至对应的位置。
[0030]
(s105)自动导引装置12计算车体11的当前位置、旋转半径与目标路径间的误差量，并采用pid控制得到修正后的速度及旋转半径，再采用逆运动学反推计算车体11的速度，使转向驱动系统13可控制舵轮111来修正调整该车体11的当前位置与旋转半径。
[0031]
由图中及上述的实施步骤可清楚得知，本发明以下说明书内容的无人自走车1较佳实施为以叉车式自动导引车为例，并利用自动导引装置12的传感器模块121对车体11进行位置与姿态的定位，以及路径规划单元122产生预定规划的目标路径，由于车体11的驱动机构主要是利用前方一个舵轮111(即主动轮)带有转向的功能，并配合后方两个转轮112
(即从动轮)运行，其实际移动的路径轨迹只和前方舵轮111转角或航向角有关，因此只要对舵轮111的转角或航向角进行控制，即可实现无人自走车1的路径导引控制。
[0032]
在本实施例中，为利用自动导引装置12先在无人自走车1所处的环境中建立一个全局坐标系(global coordinate system)(如图3中的xgyg坐标平面)，并取得车体11中心(即舵轮111的几何中心)在全局坐标系中的坐标(xc，yc)作为中心点c，以及目标点p作为预定规划的目标路径其中一目标点，且该预定规划的目标路径包含直线路径及弯曲路径，以建立车辆坐标系(如x
gmygm
坐标平面)后，再利用旋转矩阵进行坐标系转换建立一个本地坐标系(local coordinate system)(如x
lyl
坐标平面)可以得到：
[0033][0034]
其中θ为车体11当前姿态角，可表示为车辆坐标系的x
gm
或y
gm
轴旋转到本地坐标系的x
l
或y
l
轴的角度；xc为全局坐标系的yg轴与车辆坐标系的y
gm
轴的间距；yc为全局坐标系的xg轴与车辆坐标系的x
gm
轴的间距；xg，yg为预定规划的目标路径其中一目标点p在全局坐标系中的坐标(x，y)；x
l
，y
l
为目标点p在本地坐标系中的坐标(x，y)，以定位出车体11当前位置与姿态。
[0035]
根据直角三角形的几何关系可以得到：
[0036][0037][0038][0039]
其中d为车体11在本地坐标系中的中心点c(xc，yc)至目标点p(x
l
，y
l
)的最短路径距离；θs为本地坐标系的y
l
轴顺时针旋转到目标点p上的角度，可表示为车体11中心或中心点c与目标点p间的夹角，即车体11的舵轮111转向至目标点p的转角或航向角；由于车体11的舵轮111当前航向角与车体11在本地坐标系的y
l
轴保持一致，在d、θs已知的情况下，根据几何关系可以得到r为车体11中心(即舵轮111的几何中心)至目标点p的旋转半径。
[0040]
如图4所示，当自动导引装置12取得车体11中心或舵轮111的几何中心当前位置(即中心点c)、距离d及速度v，便可计算得到当前位置与目标路径的偏差量，并在v、d已知的情况下，根据直角三角形的几何关系则可以得到：
[0041][0042][0043][0044]
其中r为车体11中心(即舵轮111的几何中心)转向至目标点p的旋转半径；l为车体11前方舵轮111几何中心至后方两个转轮112中心联线的中点m之间固定的距离；w为两个转轮112中心联线的中点m至车体11中心的旋转半径的原点o之间可变的距离；θs为车体11的舵轮111转向至目标点p的转角或航向角，使转向驱动系统13可根据θs的偏差量来控制车体
11的舵轮111转向至对应的位置，从而实现对无人自走车1依循预定规划的目标路径运行的导引控制，并且车载控制器或自动导引装置12内建处理器采用的转向驱动算法，不需经由大量复杂的运算或较长的运算处理周期，对于车载控制器或处理器的运算性能要求相对降低，也可有效地提升整体的导航效率。
[0045]
请搭配参阅图5～图7所示，分别为本发明目标路径闭回路导引控制的方块图、修正车体转弯的旋转半径的示意图(一)及修正车体转弯的旋转半径的示意图(二)，由图中可清楚看出，本发明的无人自走车1可根据车体11移动状态与自动导引装置12所生成预定规划的目标路径进行pid(比例、积分与微分)控制，以形成闭回路的控制流程，从而实现对车体11周期性循环的控制调整。
[0046]
当车体11依循目标路径进行移动时，自动导引装置12可将车体11的当前位置与旋转半径进行坐标转换，并计算出车体11的当前位置、旋转半径与目标路径间的误差量(errord及errorr)，再根据该误差量进行pid控制得到车体11修正后的速度v*及旋转半径r*，便可采用逆运动学(inverse kinematics)以反推的方式计算车体11移动至目标路径上所需的速度v或加速度a，使转向驱动系统13可控制舵轮111来修正调整车体11的当前位置与旋转半径，如此反复修正使车体11移动状态符合期望的目标路径，直到完成车体11的路径导引控制。
[0047]
在本实施例中，自动导引装置12所生成预定规划的目标路径可导引车体11依循直线路径或弯曲路径运行，并给予车体11的当前位置与旋转半径，且可不断地侦测车体11的当前位置、旋转半径与目标路径间的误差量，其中errord为车体11当前位置与目标路径的最终位置直线距离的误差量，而errorr＝r-r
false
为车体11中心的旋转半径与车体11偏移产生的旋转半径的误差量，再进行pid控制不同的算法可计算得到车体11修正后的速度v*＝k
pr
＊(errord)，以及车体11修正后的旋转半径r*＝k
pr
＊(r-errorr)，其中k
pr
为增益量。
[0048]
当车体11行进于弯曲路径时，可利用调整旋转半径来改变车体11的转弯幅度，例如车体11中心的旋转半径变大时，表示车体11已偏移到弯曲路径外侧，则车体11调整改变的转弯幅度要变小；换言之，当车体11的旋转半径变小时，表示车体11已偏移到弯曲路径内侧，则车体11调整改变的转弯幅度要变大，至于车体11中心转弯的方向则可在算法中作判断，因此可利用车体11中心的旋转半径当作变化量来修正车体11的转向，使车体11在偏移弯曲路径时，能快速且准确将偏离修正，并稳定的保持在预定规划的目标路径上。
[0049]
请同时参阅图8～图12所示，分别为本发明车体进行直线控制的示意图、车体相对于目标路径的姿态方向的示意图、车体导引至下一个目标点的示意图(一)、车体导引至下一个目标点的示意图(二)及车体切换目标点的长度的示意图，由图中可清楚看出，上述的自动导引装置12所生成预定规划的目标路径为利用多个目标点p0～p9来划分成多个线段，并将多个线段连接形成直线路径轨迹，其中目标点p0可表示为直线路径的起始点，目标点p9可表示为直线路径的最终点，并根据直线方程式y＝ax+b依预定长度划分，当车体11行进时，依车辆坐标系来看第一个目标点p0在车体11的左边(如图9所示)，并计算得到车体11中心至目标点p0间的夹角[arctan(x/y)]可作为车体11转向的控制，也可以此判断车体11转弯的方向为逆时针或顺时针，其中逆时针转弯的方向为正，顺时针转弯的方向为负。
[0050]
在本实施例中，当车体11于追踪直线路径上划分的多个目标点p0～p9时，若是车体11中心超过了目标点就会追踪下一个目标点(如图10～图11所示)，至于判断是否超过了
该目标点，则可根据车辆坐标本身的目标点对于车体11的y轴方向向量是否小于0，若是判断目标点对于车体11的y轴方向向量为小于0时，就会从下一个目标点开始追踪，举例来说，若是车体11中心投影到直线路径上的定位点n为位于目标点p2与目标点p3之间，直线路径上的目标点p2对于车体11的y轴方向向量为2-n小于0，也就是说y小于0，而直线路径上的目标点p3对于车体11的y轴方向向量为3-n大于0，则可表示为y大于0，并结合程序代码进行函数运算，其中index为直线路径上的目标点，y＝index-n，若是计算得到y小于0，则预设的index便需要加1，直到y大于0，也就是最终的index计算结果为3，车体11便会从第三个目标点p3开始追踪。
[0051]
此外，为了避免车体11中心过于靠近直线路径上的目标点而x轴方向仍有过大差距，很容易造成车体11转向的角度[arctan(x/y)]过大，导致车体11产生偏移或强烈的抖动，因此可以通过参数设定切换目标点的长度(如图12所示)，以此确保车体11能够稳定的依循目标路径前进。
[0052]
上述详细说明为针对本发明一种较佳的可行实施例说明而已，该实施例并非用以限定本发明的范围，凡其他未脱离本发明所揭示的技艺精神下所完成的均等变化与修饰变更，均应包含于本发明所涵盖的范围中。
[0053]
综上所述，本发明的无人自走车的导引控制方法使用时为确实能达到其功效及目的。