CN1163005A - 制导车辆行驶的控制装置 - Google Patents

Abstract

本发明控制装置的目的在于，提高制导车辆沿预定行驶路线行驶时的制导行驶的精确度，也即是说，根据以选择手段选择的坐标位置数据以及行驶方向数据和以检测位置手段检测的现在坐标位置，运算沿车辆行驶方向上的垂直方向车辆位置的现在偏差值。随后，根据以偏差值运算手段运算的偏差值和以速度检测手段检测的现在的行驶速度，使偏差值为零，检测行驶速度越大，运算车辆所操的舵角越小。然后，根据以所操的舵角运算手段运算的车辆所操的舵角，控制车辆所操的舵角，高精度地制导车辆沿预定行驶路线行驶。还有，用积分运算以偏差手段运算的偏差值。然后，根据以运算偏差值手段运算的偏差值、以积分演运手段运算的积分值和以速度检测手段检测的现在的行驶速度，使偏差值为零，检测行驶速度越大，演算的车辆所操的舵角越小。

Description

制导车辆行驶的控制装置

本发明涉及制导车辆行驶的控制装置，该装置存储预定行驶路线的位置数据，根据该位置数据控制车辆所操的舵角，制导车辆沿所述预定行驶路线行驶。

过去，对于制导车辆沿预定行驶路线行驶的装置已进行种种开发，并申请了专利等。

特公平5-22925号乃至22926号公报中公开了这样一种制导行驶的技术，即使多条直线结合起来形成近似的行驶路线，制导车辆沿该直线行驶。

根据上述公报所记载的技术，车辆只是直线前进即可，所以可简易地控制车辆的所操的舵角运算等，这是有利之点，但是，行驶路线是曲线的，其所经路线是近似的直线，所以车辆不可能正确沿预定行驶路线行驶，这是一个缺点。

还有，根据上述公报记载的技术，车辆所操的舵角的设定与车辆现在的速度无关。

因此，车辆的速度高时，所操的舵角过大，导致过度转向，脱离曲线的行驶路线。同样，车辆的速度低时，所操的舵角过小，导致操纵不灵，脱离曲线的行驶路线。

还有，根据上述公报记载的技术，在所操的舵角运算处理上，在不脱离直线路线行驶时，所操的舵角设定为零。

然而，在自卸卡车等车辆的情况下，由于装载的货物或装载的状态不同，车辆四轮的调整发生变化。这样，调整一旦发生变化，而仍使所操的舵角保持为零继续行驶时，车辆会脱离直线路线，在实用上成为很大的问题。

还有，特公平5-503775号公报中公开了所操的舵角运算，求出了由5次多项式所表示的、可从现在位置顺利地沿行驶路线行驶的路线，车辆沿着这条路线行驶。

根据该公报记载的技术，考虑的是沿曲线的行驶路线行驶，其运算繁杂，且费时，这是个缺点。

还有，根据上述公报记载的技术，仍像所述特公平5-22925号乃至22926号公报的技术那样，车辆所操的舵角的设定与车辆现在的速度无关。因此，也导致脱离曲线的行驶路线的问题。

还有，同所述特公平5-22925号乃至22926号公报的技术那样，在不偏离直线行驶路线行驶的情况下，因所操的舵角设定为零，所以若车辆前后轮的调整不完善，则要不偏离直线路线行驶是极端困难的。

如上所述，根据已有技术，存在着不能通过简易处理正确地在曲线行驶路线上行驶的问题。

还有，存在着由于什么样的车辆速度而导致脱离曲线行驶路线的问题。

还有，由于车辆的调整变化，例如即使是直线行驶路线，也会有脱离预定路线的问题。

本发明的目的在于，提供一种可解决这些问题的装置。

因此，本发明的第一发明目的是，通过简易处理使车辆正确地在曲线行驶路线上行驶，同时不管车辆的速度如何，不会使其脱离曲线行驶路线。

这一目的，可通过下列方式达到，即这种制导车辆行驶的控制装置存储预定行驶路线的位置数据，根据该位置数据控制车辆所操的舵角，制导车辆沿所述预定行驶路线行驶。这种装置具有：

存储手段，存储表示所述预定行驶路线各点上的各该点坐标位置和所述车辆应行驶方向的数据；

位置检测手段，检测所述车辆现在的坐标位置；

速度检测手段，检测所述车辆现在的行驶速度；

选择手段，从所述存储手段的存储内容中，选择最接近于以所述位置检测手段检测的现在坐标位置的、预定行驶路线上点的坐标位置数据，以及该最接近点的行驶方向数据；

偏差值运算手段，根据以所述选择手段选择的坐标位置数据以及行驶方向数据和以所述位置检测手段检测的现在的坐标位置，运算沿车辆行驶方向的垂直方向上车辆位置现在的偏差值；

所操的舵角运算手段，根据以所述偏差值运算手段运算的偏差值和以所述速度检测手段检测的现在行驶速度，为使所速偏差值为零，所述检测行驶速度越大，运算车辆所操的舵角越小；

控制手段，根据以所述所操的舵角运算手段运算的车辆所操的舵角，控制所述车辆所操的舵角。

还有，本发明的第二发明目的，除加进上述第一发明的目的外，可吸收消除车辆调整的变化，制导车辆正确地沿预定行驶路线行驶。

在这个目的中加进了上述第一发明的构成，并具有积分运算手段，以积分法运算用所述偏差值运算手段运算的偏差值，同时还具有所操的舵角运算手段。

根据以所述偏差值运算手段运算的偏差值和以所述积分运算手段运算的积分值，和以所述速度检测手段检测的现在的行驶速度，为使所述的偏差值为零，所述检测行驶速度越大，则运算车辆所操的舵角越小。

还有，本发明的第三发明目的，除加进上述第一发明目的外，可制导车辆沿着曲率半径复杂变化的路线正确地行驶。

在这个目的中，加进了上述第一发明的构成，并具有积分运算手段，运算以所述偏差值运算手段运算的偏差值，同时不采用所操的舵角运算手段，而具有第一所操的舵角运算手段，该手段根据以所述偏差值运算手段运算的偏差值和以所述积分运算手段运算的积分值，和以所述速度检测手段检测的现在的行驶速度，为使所述偏差值为零，所述检测速度越大，则运算第一车辆所操的舵角越小；

还有，第二所操的舵角运算手段，根据所述最近点的所述预定行驶路线的曲率半径，运算第二车辆所操的舵角；

第三所操的舵角运算手段，根据以所述第一所操的舵角运算手段运算的第一车辆所操的舵角，和所述第二所操的舵角运算手段运算的第二车辆所操的舵角，运算第三车辆所操的舵角；

控制手段，根据以所述第三所操的舵角运算手段运算的第三车辆所操的舵角，控制所述车辆所操的舵角。

根据上述第一发明的构成，根据以选择手段选择的坐标位置数据以及行驶方向数据和以位置检测手段检测的现在坐标位置，运算在车辆行驶方向的垂直方向上车辆位置的现在偏差值，即可简易地运算与曲线行驶路线间的偏差值。

而且，根据以偏差值运算手段运算的偏差值和以速度检测手段检测的现在行驶速度，使偏差值为零，检测行驶速度越大，简易运算车辆所操的舵角越小，即，简易而正确地运算与车辆现在速度相应的车辆所操的舵角。

而且，根据以所操的舵角运算手段运算的车辆所操的舵角，控制车辆所操的舵角，高精度地制导车辆在预定行驶路线上行驶。

还有，根据上述第二发明的构成，以偏差值运算手段运算的偏差值，由积分运算。而且，根据以偏差值运算手段运算的偏差值和以积分运算手段运算的积分值，和以速度检测手段检测的现在行驶速度，为使偏差值为零、检测行驶速度越大，运算车辆所操的舵角越小。即，考虑偏差值的积分，运算所操的舵角，因此，可正确地求出能够吸收并消除车辆的调整变化，高精度地制导车辆在预定行驶路线上行驶。

还有，根据上述第三发明的构成，根据最接近车辆现在位置的预定行驶路线的曲率半径，运算第二车辆所操的舵角。而且，根据基于上述偏差值的第一车辆所操的舵角和第二车辆所操舵角，运算第三车辆所操的舵角，并根据该第三车辆所操的舵角，控制车辆所操的舵角。其结果，通过迅速地保持基于路线曲率的第三所操的舵角，可大致地制导车辆沿路线行驶，同时，通过基于偏差值的第一所操的舵角来修正离开路线的偏差，即可高精度地制导车辆沿曲率半径复杂变化的路线行驶。

以下，对附图进行简要说明。

图1为表示有关本发明制导车辆行驶的控制装置实施例构成的框图。

图2为说明通过模拟取得的预定行驶路线数据处理的框图。

图3为用于说明运算车辆所操的舵角几何学关系图。

图4为表示自动制导车辆行驶处理程序的程序图。

下面，参照附图，说明有关本发明制导车辆行驶的控制装置的实施例。

图1为表示实施例的制导行驶控制装置的框图。图2为说明在实际制导行驶前所进行的模拟内容的框图。

预定行驶路线的模拟

本实施例假定以图2所示的无人驾驶的自卸卡车(以下称“车辆”)14作为应行驶的车辆。

例如，车辆14是在预定行驶路线S上无人驾驶行驶的。表示该预定行驶路线S的数据，是通过模拟，即通过有人驾驶在应行驶路线上行驶一周的模拟作业而设定并存储在车辆14上设置的存储器5。

车辆14上装有检测行驶方向的陀螺仪1、检测行驶距离的距离编码器2、和控制陀螺仪1与距离编码器2的位置计测控制器3。

在教学行驶中，车辆14依次的坐标位置、车辆14依次的行驶方向、车辆14依次的速度，依据由陀螺仪1、距离编码器2输出的检测信号，通过位置计测控制器进行运算，并输出，作为全部路线数据4。

但是，全部路线数据4以一秒钟输出数次至数十次，数据规模大，所以为使该数据尺寸成为适于储存器5的存储容量的尺寸，而以给定的时间间隔取样。取样的间隔τ定为不能过稀、过密的最佳间隔。

这样取样的车辆14依次的坐标位置P1(X1、Y1)、P2(X2Y2)...Pi(Xi、Yi)...、而车辆14依次的行驶方向(方位角)ψ1、ψ2...ψi...、而车辆14依次的速度V1、V2...Vi...，均作为路线数据6被存储在存储器5中。

这样，在存储器5中，在预定行驶路线S各点的每个P1、P2...Pi...上均记忆和存储着车辆14应处位置的坐标位置(Xi、Yj)、车辆14应行驶的行驶方向ψ1、车辆14应行驶的速度Vi。

还有，在本实施例中，是通过模拟作业求出路线数据6，其求出的方法是任意的，也可以通过测量等方法求出。

自动制导行驶

下面，参照图4的程序图，说明自动制导车辆14行驶的处理程序。

模拟行驶后，开始自动制导行驶(重复)时，则由陀螺仪1、距离编码器2输出检测信号，车辆14依次的现在位置Q(X、Y)通过位置计测控制器3运算，并输入制导控制器7(步骤101)。

然后，在制导控制器7中，运算如后所述的所操的目标舵角ζ，通过操舵控制器8由液压伺服系统11驱动车辆14未图示的操舵，以可得到上述所操的目标舵角ζ而使车辆14的方向发生变化(步骤102-106)。

还有，在制导控制器7中，运算如后所述的车辆14目标速度，通过加速控制器9和制动控制器10由机械伺服系统12、气动伺服系统13驱动车辆14图中未示出的加速器及制动器，以可得到上述目标速度而使车辆14的速度发生变化。

下面，将更详细地说明关于上述所操的目标舵角的运算。

即是说，在车辆14行驶方向的前方选择最接近车辆14的现在位置Q(X、Y)的行驶路线S上的点Pi(步骤102，参阅图3)。

于是，与该选择点Pi相对应的位置坐标数据(Xi、Yi)、方位角数据ψi，可由存储器5中读取。

还有，对于与上述最近点Pi的行驶路线S的曲率半径对应的所操的基本舵角ζP，进行如后所述的运算(步骤103)。

然后，根据这样得到的路线上位置坐标(Xi、Yi)、方位角ψi、所操的基本舵角ζp和车辆现在的位置Q(X、Y)，进行下列(1)、(2)、(3)式的运算，运算车辆14所操的目标舵角ζ。

yi＝-(X-Xi)sinφi+(Y-Yi)cosφi   …(1)

αs＝-(K4+K5·K6)yi-K4·K6 yidt-K5dyi/dt…(2)

ζ＝tan-1(αsL/V2)+Kp·ζp…(3)

式中，K4、K5、K6、Kp为控制常数；L为车辆14的前后轴距(前、后车轴之间的距离)。

V是车辆14的现在速度，通过位置计测控制器3而被输入制导控制器7。

若就上述(1)-(3)式加以说明，则首先可由下列(4)-(7)式求出(3)式所示的所操的基本舵角ζp。

也就是说，若使预定行驶路线S上的点Pi与下一点Pi+1相联结的路线上的圆弧长度为G，以车辆14的现在的速度为V，取样时间为τ，则可求下式：

            G＝V·τ...(4)

而且，若点Pi与下一点Pi+1之间的圆弧的曲率半径为Ri，则圆弧的长度G可由下式求得：

            G＝Ri·(ψi+1-ψi)...(5)

由上述(4)、(5)式，可通过下式(6)求曲率半径Ri：

             Ri＝V·τ/(ψi+1-ψi)...(6)

所以，利用这样求得的曲率半径Ri和轴距L，可得到所操的基本舵角ζp，下式(7)：

            ζp＝arctan(L/Ri)...(7)。

且说，上述(1)式中，X-Y固定坐标系中车辆14的Y轴方向坐标位置Y(现在位置)，可变换为局部坐标系xi-yi上的yi。

也就是说，如图3所示，以预定行驶路线S上的点Pi为原点，在Pi点中，以车辆14应行驶的速度Vi(矢量)的方向，即行驶方向ψ1作为xi轴，进而，在线路S的法线方向上取yi轴时的xi-yi坐标系，则可以局部坐标系的形式设定。因此，经坐标转换的坐标位置yi就意味着在预定行驶路线S的法线方向(沿行驶方向的垂直方向)上车辆14的偏差值。然后，通过(3)式即可求以该法线方向偏差值yi变为零的所操的目标舵角ζ。

还有，X-Y固定坐标系上车辆14的X轴方向坐标位置X(现在位置)，坐标转换为局部坐标系xi-yi上的xi，如下列(8)所示：

          xi＝(X-X₁)cosψi+(Y-Yi)sinψi...(8)

将上述(1)式所求出的法线方向偏差值yi代入上述(2)式中，求得车辆14的横(法线)方向加速度αs，通过将该横向加速度αs代入上述(3)式的方法，运算所操的目标舵角ζ。

这里，横方向加速度αs中含有法线方向偏差值yi的积分值-K4·K6yidt，该积分值接近所操的目标舵角ζ。

因此，若将该所操的目标舵角ζ成为零那样来控制车辆14的操舵，则车辆14的调整即使有毛病，也会通过上述积分值将其吸收、消除出去，从而有可能使车辆14不偏离直线路线行驶。

还有，由上述(3)式求的所操的目标舵角ζ与1/V2成比例。因此，车辆14的现在速度V越高，所操的舵角越小，速度越低，所操的舵角越大。其结果，可排除只与速度相适的适当角度的操舵，可防止车辆14由路线S上脱离。

还有，以上的运算不是求复杂多项式的复杂运算，而是简易的运算。因此，不需要繁杂的运算而以简易的处理，即可制导车辆14在曲线路线上正确地行驶(步骤103、104、105)。

但是，通过位置计测控制器3可运算车辆14现在的方位角θ，并将其输入制导控制器7中。然后，根据该现在方位角θ和现在速度V，和对应点Pi的方位角ψi，通过下列(9)、(10)、(11)式运算车辆14的目标速度Vxi、Vyi：

θi＝θ-ψi…(9)

Vxi＝Vcosθi…(10)

Vyi＝Vsinθi…(11)

但是，上述(9)式中，若θ和ψ1相同，也可以相似，因此，θi成为零，横(法线)方向速度Vyi成为零.结果，只要控制(10)式所示的纵(切线)方向速度Vxi就可以了，因此，可以目标速度的形式输入加速控制器9和制动控制器10，以控制加速器、制动器。

如上所述，根据本发明，通过简易运算可求车辆所操的舵角，因此，通过简易处理可制导车辆简易地在曲线行驶路线上行驶。

还有，由于考虑到了行驶路线法线方向偏差值的积分值，而运算所操的舵角，所以即使车辆调整出了毛病，也能制导车辆高精度地沿预定行驶路线行驶。

还有，根据基于行驶路线曲率的基本所舵角，预先给予所操的必需舵角，进而使基于编差值的所操的舵角接近，修正来自路线的偏差，所以即使对于曲率半径复杂变化的曲线路线，也能及时、高精度地制导行驶。

本发明对于不论是无人驾驶的车辆，还是有人驾驶的车辆均可适用。此外，本发明对于不论是室外行驶的自卸等车辆，还是室内行驶的无人驾驶的搬运车等车辆，也均可适用。

Claims (3)

1.一种制导车辆行驶的控制装置，存储预定行驶路线的位置数据，根据该位置数据控制车辆所操的舵角，制导车辆沿所述预定行驶路线行驶，其特征在于，该装置具有：

存储手段，存储表示所述预定行驶路线各点上的各该点的坐标位置和所述车辆应行驶方向的数据；

位置检测手段，检测所述车辆现在的坐标位置；

速度检测手段，检测所述车辆现在的行驶速度；

选择手段，从所述存储手段的存储内容中，选择最接近于以所述位置检测手段检测的现在坐标位置的、预定行驶路线上各坐标位置的数据，以及该最接近点的行驶方向数据；

偏差值运算手段，根据以所述选择手段选择的坐标位置数据以及行驶方向数据和以所述位置检测手段检测的现在的坐标位置，运算沿车辆行驶方向的垂直方向上车辆位置现在的偏差值；

所操的舵角运算手段，根据以所述偏差值运算手段运算的偏差值和以所述速度检测手段检测的现在的行驶速度，为使所述偏差值为零，所述检测行驶速度越大，则运算车辆所操的舵角越小；

控制手段，根据以所述所操的舵角运算手段运算的车辆所操的舵角，控制所述车辆所操的舵角。

2.一种制导车辆行驶的控制装置，存储预定行驶路线的位置数据，根据该位置数据控制车辆所操的舵角，制导车辆沿所述预定路线行驶，其特征在于，该装置具有：

存储手段，存储表示所述预定行驶路线的各点上的各该点的坐标位置和所述车辆应行驶方向的数据；

位置检测手段，检测所述车辆现在的坐标位置；

速度检测手段，检测所述车辆现在的行驶速度；

选择手段，从所述存储手段的存储内容中，选择最接近于以所述位置检测手段检测的现在坐标位置的、预定行驶路线上坐标位置的数据，以及该最近点的行驶方向数据；

偏差值运算手段，根据以所述选择手段选择的坐标位置数据以及行驶方向数据和以所述位置检测手段检测的现在坐标位置，运算沿车辆行驶方向的垂直方向上车辆位置现在的偏差值；

积分运算手段，以积分法运算以所述偏差值运算手段运算的偏差值；

所操的舵角运算手段，根据以所述偏差值运算手段运算的偏差值和以所述积分运算手段运算的积分值，和以所述速度检测手段检测的现在行驶速度，为使所述偏差值为零，所述检测行驶速度越大，则运算车辆所操的舵角越小；

控制手段，根据以所述所操的舵角运算手段运算的车辆所操的舵角，控制所述车辆所操的舵角。

3.一种制导车辆行驶的控制装置，存储预定行驶路线的位置数据，根据该位置数据控制车辆所操的舵角，制导车辆沿所述预定路线行驶，其特征在于，该装置具有：

存储手段，存储表示所述预定行驶路线的各点上的各该点的坐标位置和所述车辆应行驶方向的数据；

位置检测手段，检测所述车辆现在的坐标位置；

速度检测手段，检测所述车辆现在的行驶速度；

选择手段，从所述存储手段的存储内容中，选择最接近于以所述位置检测手段检测的现在坐标位置的、预定行驶路线上坐标位置的数据，以及该最近点的行驶方向数据；

偏差值运算手段，根据以所述选择手段选择的坐标位置数据以及行驶方向数据和以所述位置检测手段检测的现在坐标位置，运算沿车辆行驶方向上的垂直方向上车辆位置现在的偏差值；

积分运算手段，以积分法运算以所述偏差值运算手段运算的偏差值；

第一所操的舵角运算手段，根据以所述偏差值运算手段运算的偏差值和以所述积分运算手段运算的积分值，和以所述速度检测手段检测的现在的行驶速度，为使所述偏差值为零，所述检测行驶速度越大，则运算车辆所操的舵角越小；

第二所操的舵角运算手段，根据所述最近点的所述预定行驶路线的曲率半径，运算第二车辆所操的舵角；

第三所操的舵角运算手段，根据以所述第一所操的舵角运算手段运算的第一车辆所操的舵角，和以所述第二所操的舵角运算手段运算的第二的车辆所操的舵角，运算第三的车辆所操的舵角；

控制手段，根据以所述第三所操的舵角运算手段运算的第三车辆所操的舵角，控制所述车辆所操的舵角。

Images