CN113038821B - 误差修正装置 - Google Patents

Abstract

误差修正装置(34)，设于作业车辆，对基于传感器装置的测定值的误差进行修正，传感器装置具备GNSS传感器和陀螺仪传感器，误差修正装置包括：第1取得部(341)，取得由GNSS传感器测定的自身坐标系中的作业车辆的车辆位置、和由陀螺仪传感器测定的传感器坐标系中的角速度，传感器坐标系具有以传感器装置为基准的3个轴；坐标变换部(345)，基于所取得的角速度，将在传感器坐标系中以设定于与传感器装置不同位置的设定位置为起点的传感器装置所设置的测定位置的相对位置矢量坐标变换为自身坐标系中的相对位置矢量；车辆位置修正部(346)，设以自身坐标系的原点为起点的设定位置的位置矢量为P_a、测定位置的位置矢量为P_S、坐标变换后的相对位置矢量为L，利用P_a＝P_S－L的式子算出设定位置的位置矢量P_a并对由GNSS传感器测得的车辆位置的误差进行修正。

Description

误差修正装置

技术领域

本发明的实施方式涉及利用设于作业车辆的传感器装置进行的测定值的修正。

背景技术

以往，为了实现农业的高效率而进行如下的自动转向控制，使拖拉机、种植机等农用作业车辆沿着指定的行驶路径行驶。根据自动转向控制，基于作为目标的行驶路径上的位置与作业车辆的位置之间的分隔距离，对作业车辆的转向进行控制，以使作业车辆的位置定位于行驶路径上。

此外，作为与作业车辆的自动转向相关的技术，已知有如下的自动转向***，其具备：卫星定位模块，基于来自卫星的卫星信息而输出位置信息；前进基准位置算出部，基于位置信息算出前进行驶中的作业车的转向控制的作为基准位置的前进基准位置；后退基准位置算出部，基于位置信息算出后退行驶中的作业车的转向控制的作为基准位置的后退基准位置；转向控制部，在前进行驶时输出基于行驶路径与前进基准位置的偏差而算出的前进用转向控制信号，而且，在后退行驶时输出基于行驶路径与后退基准位置的偏差而算出的后退用转向控制信号；转向机构，基于前进用转向控制信号及后退用转向控制信号进行所述作业车的转向(例如，参照专利文献1)。

【专利文献1】日本特开2018－120364号公报

发明内容

在这样的自动转向控制中，测定作业车辆的车辆位置的GNSS(Global NavigationSatellite System，全球导航卫星***)传感器设置在适于从卫星接收GNSS信号的适当位置。作业车辆中的适于接收GNSS信号的适当位置是以从主体向上方延伸的方式设置的臂的上端部分、或通过该臂连接于其上端而被支承的车顶部。

此外，在作业车辆于起伏剧烈的农田上行驶的情况下，车辆的姿势会发生倾斜。此时，存在如下问题：由于GNSS传感器安装于上方，因此对于农田上的作业车辆的位置，由GNSS传感器测定的位置产生误差，进而由于该误差而导致自动转向控制的精度降低。

本发明是为了解决上述问题点而做出的，其目的在于提供能够降低利用GNSS传感器测定的作业车辆的车辆位置的误差的误差修正装置。

为了解决上述课题，本实施方式涉及的误差修正装置，其设于作业车辆，对基于传感器装置的测定值的误差进行修正，所述传感器装置具备GNSS传感器和陀螺仪传感器，所述误差修正装置包括：第1取得部，取得由所述GNSS传感器测定的自身坐标系中的所述作业车辆的车辆位置、和由所述陀螺仪传感器测定的传感器坐标系中的角速度，所述传感器坐标系具有以所述传感器装置为基准的3个轴；坐标变换部，基于所述取得的角速度，将在所述传感器坐标系中以设定于与所述传感器装置不同位置的设定位置为起点的所述传感器装置所设置的测定位置的相对位置矢量坐标变换为所述自身坐标系中的相对位置矢量；车辆位置修正部，设以所述自身坐标系的原点为起点的所述设定位置的位置矢量为P_a、所述测定位置的位置矢量为P_S、所述坐标变换后的相对位置矢量为L，利用P_a＝P_S－L的式子算出所述设定位置的位置矢量P_a并对由所述GNSS传感器测得的车辆位置的误差进行修正。

根据本发明，能够降低利用GNSS传感器测定的作业车辆的车辆位置的误差。

附图说明

图1是表示实施方式涉及的农用拖拉机的构成的概略侧视图。

图2是表示实施方式涉及的控制***的整体构成的框图。

图3是表示误差修正装置的硬件构成的框图。

图4是表示误差修正装置的功能构成的框图。

图5是表示误差修正装置的动作的流程图。

图6是表示相对位置矢量的概略主视图。

图7是表示相对位置矢量的概略俯视图。

图8是表示速度矢量的概略主视图。

具体实施方式

以下，参照附图说明本发明的实施方式。

(作业车辆的构成)

首先，对具有本实施方式涉及的误差修正装置的作业车辆进行说明。图1是表示实施方式涉及的农用拖拉机的构成的概略侧视图。

作为本实施方式涉及的自动转向***的转向对象的车辆是作业车辆，具体而言，为图1所示的农用拖拉机1。该拖拉机1是具备车体10、2个前轮11和2个后轮12的四轮车辆，但只要是可转向的车辆，则不限于作业车辆，可以是任意车辆。此外，拖拉机1具备：供驾驶者就座的座席13、转向柱15、方向盘16、转向驱动装置17、包含油门踏板、制动器等的踏板类18、4个支承框架19a～19d、及车顶部19r。

对前轮11施加偏转角来转向拖拉机1的转向***设于转向柱15内，并在转向柱15中内置有用于将由方向盘16或转向驱动装置17所致的方向盘角输入到转向***的输入轴151，基于该输入轴151的旋转的转向角被施加到前轮11。转向驱动装置17是为了对不具有用于进行自动转向控制的构成、以手动转向为前提的拖拉机1进行自动转向而后附加的装置，在转向驱动装置17的上方安装有方向盘16，在转向驱动装置17的下方嵌合有转向***的输入轴151的上端部。分别形成为长条状的4个支承框架19a～19d以立起的状态设置在车体10上的前后左右的不同位置，在所述4个支承框架19a～19d上载置有整体呈大致平板状的车顶部19r。

在车顶部19r的上表面上，以该车顶部19r作为架台而设有传感器装置30。如后所述该传感器装置30包含GNSS传感器，因此为了接收从人造卫星发送的GNSS信号，优选是该传感器装置30安装在车顶部19r这样的拖拉机1的最上部，在拖拉机1不具有车顶部19r的情况下，将支承车顶部19r的4个支承框架19a～19d中的任一个作为架台而设置传感器装置30。此外，在位于前方的支承框架19a、19b的任一个设置有自动转向控制装置21。该自动转向控制装置21对拖拉机1的自动转向进行控制。

此外，拖拉机1具备：作为在作业车辆上设置的作业机的旋耕机41；以能够使该旋耕机41升降的方式与拖拉机1连结的连杆机构42；检测旋耕机41与农田的距离作为耕深位置的接地传感器43。接地传感器43具备：接地部431，其以能够以上端部为支点上下摆动的方式设置，以便于下端部设置于农田；检测该接地部431的摆动位移的检测部432。

(控制***的构成)

图2是表示实施方式涉及的控制***的整体构成的框图。

拖拉机1所具备的控制***包括自动转向控制***和耕深控制***，如图2所示，自动转向控制***由对转向***中的输入轴151进行驱动的转向驱动装置17、传感器装置30、自动转向控制装置21和图1中未图示的方向盘角度控制装置22构成，耕深控制***由经由连杆机构42使旋耕机41向上下方向移动的升降驱动装置45、基于由接地传感器43检测出的旋耕机41的耕深位置而控制升降驱动装置45的升降控制装置46构成。

转向驱动装置17具备：向输入轴151传递驱动力的传递轴171、驱动传递轴171的马达172、检测传递轴171的旋转量及旋转位置的编码器173，所述马达172为步进马达，所述编码器173为旋转编码器。需要说明的是，马达172只要是能够输出可使输入轴151旋转的充分的扭矩的马达，则可以是任意种类的马达。

自动转向控制装置21基于传感器装置30的测定值而输出转向角，方向盘角度控制装置22基于由自动转向控制装置21指示的转向角对转向驱动装置17反馈控制。在此，转向驱动装置17控制马达172使传递轴171驱动，以使由编码器173检测出的旋转位置成为所希望的旋转位置。

如此，通过对拖拉机1后附加设置转向驱动装置17、传感器装置30、自动转向控制装置22及方向盘角度控制装置23，从而能够在以手动转向的方式构成的拖拉机1中实现自动转向。

传感器装置30具备陀螺仪传感器31、加速度传感器32、GNSS传感器33和误差修正装置34，这些部件收纳于同一壳体内。这意味着陀螺仪传感器31、加速度传感器32和GNSS传感器33在拖拉机1中设置于大致同一部位。陀螺仪传感器31检测绕3个轴的角速度来测定拖拉机1的方位角ξ、俯仰角θ(pitch angle)、侧倾角φ(滚转角，roll angle)。加速度传感器32测定拖拉机1的3个轴的加速度。GNSS传感器33测定拖拉机1的位置即车辆位置、和拖拉机1的速度矢量。误差修正装置34对由陀螺仪传感器31、加速度传感器32、GNSS传感器33测得的测定值的误差进行修正。

自动转向控制装置21基于由GNSS传感器33检测出的拖拉机1的位置即车辆位置、和所设定的目标行驶路径，运算使车辆位置与目标行驶路径一致的拖拉机1的行进方位即目标行进方位，并基于该目标行进方位与根据GNSS传感器33的速度矢量所得的拖拉机1的行进方位之间的偏差、即行进方位偏差，运算转向角并向方向盘角度控制装置22输出。

升降控制装置46基于由接地传感器43得到的耕深位置，运算用于旋耕机41维持恒定耕深位置的升降量并向升降驱动装置45输出。根据升降控制装置46，在因拖拉机1的自重而前轮11、后轮12沉入农田的情况下，输出使旋耕机41上升的升降量，在前轮11、后轮12脱离了下沉的情况下，输出使旋耕机41下降的升降量。此外，升降控制装置46将升降量向误差修正装置34输出。

(误差修正装置的构成)

对误差修正装置的硬件构成及功能构成进行说明。图3及图4分别是表示误差修正装置的硬件构成、功能构成的框图。

如图3所示，误差修正装置34中，作为硬件而具备：CPU(Central ProcessingUnit)91、RAM(Random Access Memory)92、存储装置93、外部I/F(Interface)94。

CPU91及RAM92协作而执行后述的各种功能，存储装置93存储由各种功能执行的处理所用的各种数据。外部I/F94与陀螺仪传感器31、加速度传感器32、GNSS传感器33、自动转向控制装置21进行数据的输入输出。需要说明的是，也可以自动转向控制装置21具备CPU、RAM、存储装置、外部I/F，取代误差修正装置而执行后述的各种功能。

此外，如图4所示，误差修正装置34中，作为功能而具备：第1取得部341、具有第1低通滤波器342a及第2低通滤波器342b的低通滤波器342、第2取得部343、设定位置修正部344、坐标变换部345、车辆位置修正部346、速度矢量修正部347。

第1取得部341取得由陀螺仪传感器31、加速度传感器32、GNSS传感器33各自测定的测定值。第2取得部343取得由升降控制装置46输出的升降量。

第1低通滤波器342a针对由第1取得部341取得的由陀螺仪传感器31测得的测定值的时序信号，使比预先设定的第1截止频率高的高频信号减小。第2低通滤波器342b针对由第1取得部341取得的由加速度传感器32测得的测定值的时序信号，使比预先设定的第2截止频率高的高频信号减小。

如上所述，传感器装置30中包含GNSS传感器33，因此要接收GNSS信号就得优选在4个支承框架19a～19d中的任一个或支承于这些框架上的车顶部19r上安装传感器装置30。在拖拉机1等作业车辆中，相当于本实施方式中的4个支承框架19a～19d这样的对车顶进行支承的部件的刚性不充分，尤其是作业车辆在凹凸剧烈的路面上行驶时等，会对传感器装置30施加高频振幅。这样的高频振幅尤其使陀螺仪传感器31、加速度传感器32测得的测定值产生误差，但利用低通滤波器342能够减少这样的误差。

坐标变换部345将传感器坐标系中的相对位置矢量及角速度矢量各自坐标变换为自身坐标系中的矢量。在此，传感器坐标系是具有以传感器装置30所安装的位置、且传感器装置30进行测定的位置即测定位置作为原点而相互正交的x、y、z这3个轴的坐标系，设为x轴朝向拖拉机1的直进方向的前侧、y轴朝向拖拉机1的宽度方向的右侧、z轴朝向拖拉机1的上下方向的下侧。自身坐标系是具有向北、向东及向下的3个轴的坐标系。需要说明的是，关于相对位置矢量、角速度矢量将在后详述。

车辆位置修正部346将在拖拉机1中的上方、在本实施方式中为安装于车顶部19r的传感器装置30的测定位置处由GNSS传感器33测定的自身坐标系中的拖拉机1的车辆位置修正为在传感器坐标系中预先设定的设定位置处的拖拉机1的车辆位置。

设定位置设定为将传感器装置30的测定位置投影于拖拉机1的行驶面、即农田表面所得的位置。在本实施方式中，设定位置被设定为：在y轴方向上位于拖拉机1的宽度方向中心、在z轴方向上位于前轮11、后轮12的下端或比该下端靠上方的下端近旁部。

速度矢量修正部347将在传感器装置30的测定位置处由GNSS传感器33测得的自身坐标系中的拖拉机1的速度矢量修正为设定位置处的速度矢量。

(误差修正装置的动作)

对误差修正装置的动作进行说明。图5是表示误差修正装置的动作的流程图。图6及图7分别是表示相对位置矢量的概略主视图、概略俯视图。图8是表示速度矢量的概略主视图。需要说明的是，图5所示的动作按规定的周期执行。

如图5所示，首先，第1取得部341取得由GNSS传感器33测定的自身坐标系中的车辆位置和速度矢量(S101)，并取得由陀螺仪传感器31测定的拖拉机1的角速度和由加速度传感器32测定的拖拉机1的加速度(S102)

接着，低通滤波器342对于由第1取得部341取得的角速度及加速度分别使比预先设定的第1截止频率、第2截止频率高的高频信号减小(S103)。

接着，第2取得部343取得由升降控制装置46输出的升降量(S104)，设定位置修正部344基于由第2取得部343取得的升降量而修正设定位置处的z轴上的位置(S105)。

在此，对由设定位置修正部344进行的设定位置的修正进行说明。如上所述，设定位置作为将传感器装置30的测定位置投影于农田表面所得的位置，在传感器坐标系的z轴上位于预先设定在拖拉机1的前轮11、后轮12的下端近旁的位置，在前轮11、后轮12的至少任一个大幅沉入农田的情况下，测定位置与作为拖拉机1的行驶面的农田表面之间的z轴上的相对距离小于测定位置与设定位置之间的z轴上的相对距离。此外，即使是同一农田，其状态也未必是整体上均匀，根据位置不同而状态不同，因此在拖拉机1的行驶中，测定位置与农田表面之间的z轴上的相对距离会发生变动。

设定位置修正部344基于升降量算出被升降控制为与农田表面保持恒定距离的旋耕机41的上下方向位置，以该上下方向位置越高则测定位置与设定位置之间的相对距离变小的方式，修正设定位置在z轴上的位置。由此，即使前轮11、后轮12沉入于农田的情况下，也能将设定位置设为投影于农田表面的位置。

坐标变换部345基于由陀螺仪传感器31测定的拖拉机1的方位角、俯仰角、侧倾角，通过L＝CL_S的式子将图5及图6所示的在传感器坐标系中以设定位置a为起点的测定位置s的相对位置矢量L_S坐标变换为自身坐标系中的相对位置矢量L(S106)。在此，C表示用于从传感器坐标系向自身坐标系进行坐标变换的坐标变换矩阵。

【数学式1】

在此，关于坐标变换矩阵C的各要素，设方位角为ξ、俯仰角为θ、侧倾角为φ，如下述这样定义。

【数学式2】

C11＝cosξ·cosθ

C12＝sinξ·cosθ

C13＝-sinθ

C21＝-sinξ·cosφ+cosξ·sinθ·sinφ

C22＝cosξ·cosφ+sinξ·sinθ·sinφ

C23＝cosθ·sinφ

C31＝sinξ·sinφ+cosξ·sinθ·cosφ

C32＝-cosξ·sinφ+sinξ·sinθ·cosφ

C33＝cosθ·cosφ

此外，在坐标变换之前，通常进行称为对准的初始化处理。由该初始化处理确定拖拉机1的初始方位(方位角)和初始姿势(俯仰角度及侧倾角)，在利用坐标变换部345进行的坐标变换中，利用由陀螺仪传感器31测量的角速度将该初始方位及初始姿势更新后而得的方位角ξ、俯仰角θ、侧倾角φ被赋予给坐标变换矩阵C。

初始姿势下的俯仰角度θ和侧倾角φ是通过将在拖拉机1静止的状态下由加速度传感器32测定的重力加速度设为g、将3个轴的加速度分别设为a_x、a_y、a_z而由下式求出。

【数学式3】

a_x＝g·sinθ

a_y＝-g·cosθ·sinφ

a_z＝-g·cosθ·cosφ

此外，作为初始方位的方位角ξ是基于在拖拉机1的行驶中由GNSS传感器33测定的速度矢量而设定。需要说明的是，关于初始姿势及初始方位，可以使用预先设定的值。

坐标变换后，车辆位置修正部346设以其自身坐标系的原点O为起点的设定位置a的位置矢量为P_a、测定位置s的位置矢量为P_S，利用P_a＝P_S－L的式子算出位置矢量P_a，由此将在测定位置s处测定的车辆位置修正为设定位置a的车辆位置(S107)。

接着，坐标变换部345使用坐标变换矩阵C，利用ω＝Cω_S的式子将由陀螺仪传感器31测定的在传感器坐标系中的拖拉机1的旋转角速度矢量ω_S坐标变换为自身坐标系中的旋转角速度矢量ω(S108)。

坐标变换后，速度矢量修正部347设由GNSS传感器33测定的测定位置s处的拖拉机1的速度矢量为V_S、设定位置a处的速度矢量为V_a，利用V_a＝V_S－ω×L的式子算出速度矢量V_a，由此将在测定位置s测得的速度矢量V_S修正为设定位置a处的速度矢量V_a(S109)。

如此，误差修正装置34将在测定位置s测得的车辆位置、速度矢量各自修正为设定位置a处的车辆位置、速度矢量。根据修正后的车辆位置及速度矢量，自动转向控制装置21能够获得降低了特别是拖拉机1在具有凹凸的农田行驶时产生的传感器装置30的振动所引起的测定值的误差的、更准确的车辆位置、行进方位及行进速度。由此，可利用自动转向控制装置21实现准确的拖拉机1的自动转向。

需要说明的是，作为设有由误差修正装置34修正测定值误差的传感器装置30的作业车辆，除了拖拉机1以外，可举出种植机、联合收割机、蔬菜移载机、蔬菜收割机、割草机等设有以与农田接触的方式被升降控制的作业机的全部作业车辆。

本发明的实施方式是作为示例而提示，并不意图限定发明范围。上述新的实施方式可以以其他的各种方式来实施，在不脱离发明要旨的范围内可以进行各种省略、置换、变更。上述实施方式及其变形包含于发明范围、要旨，并且也包含于权利要求书记载的方案及其等同范围。

附图标记的说明

34误差修正装置

341第1取得部

345坐标变换部

346车辆位置修正部

Claims (7)

1.一种误差修正装置，设于作业车辆，对基于传感器装置的测定值的误差进行修正，所述传感器装置具备GNSS传感器和陀螺仪传感器，所述误差修正装置包括：

第1取得部，取得由所述GNSS传感器测定的自身坐标系中的所述作业车辆的车辆位置、和由所述陀螺仪传感器测定的传感器坐标系中的角速度，所述传感器坐标系具有以所述传感器装置为基准的3个轴；

坐标变换部，基于所述取得的角速度，将在所述传感器坐标系中以设定于与所述传感器装置不同位置的设定位置为起点的所述传感器装置所设置的测定位置的相对位置矢量坐标变换为所述自身坐标系中的相对位置矢量；以及

车辆位置修正部，设以所述自身坐标系的原点为起点的所述设定位置的位置矢量为P_a、所述测定位置的位置矢量为P_S、所述坐标变换后的相对位置矢量为L，利用P_a＝P_S－L的式子算出所述设定位置的位置矢量P_a并对由所述GNSS传感器测得的车辆位置的误差进行修正。

2.根据权利要求1所述的误差修正装置，其特征在于，

所述作业车辆具备以与农田接触的方式被升降控制的作业机，

所述传感器坐标系中的1个轴为所述作业车辆的上下方向，

所述设定位置预先设定为将所述测定位置投影于所述农田的表面的位置，

所述误差修正装置还包括：

第2取得部，取得所述作业机的升降控制涉及的升降量；以及

设定位置修正部，基于所述取得的升降量来修正所述设定位置，所述坐标变换部对以修正后的设定位置为起点的测定位置的相对位置矢量进行坐标变换。

3.根据权利要求1所述的误差修正装置，其特征在于，

所述第1取得部还取得由所述GNSS传感器测定的所述自身坐标系中的所述作业车辆的速度矢量，

所述坐标变换部还基于所述取得的角速度将基于所述取得的角速度的所述传感器坐标系中的所述作业车辆的旋转角速度矢量坐标变换为所述自身坐标系中的旋转角速度矢量，

所述误差修正装置还包括：速度矢量修正部，设所述测定位置处的所述作业车辆的速度矢量为V_S、所述设定位置处的所述作业车辆的速度矢量为V_a、所述坐标变换后的旋转角速度矢量为ω，利用V_a＝V_S－ω×L的式子算出所述设定位置处的所述作业车辆的速度矢量，对由所述GNSS传感器测定的所述作业车辆的速度矢量进行修正。

4.根据权利要求2所述的误差修正装置，其特征在于，

所述第1取得部还取得由所述GNSS传感器测定的所述自身坐标系中的所述作业车辆的速度矢量，

所述坐标变换部还基于所述取得的角速度将基于所述取得的角速度的所述传感器坐标系中的所述作业车辆的旋转角速度矢量坐标变换为所述自身坐标系中的旋转角速度矢量，

所述误差修正装置还包括：速度矢量修正部，设所述测定位置处的所述作业车辆的速度矢量为V_S、所述设定位置处的所述作业车辆的速度矢量为V_a、所述坐标变换后的旋转角速度矢量为ω，利用V_a＝V_S－ω×L的式子算出所述设定位置处的所述作业车辆的速度矢量，对由所述GNSS传感器测定的所述作业车辆的速度矢量进行修正。

5.根据权利要求1～4中任一项所述的误差修正装置，其特征在于，还包括：

第1低通滤波器，对于由所述陀螺仪传感器测定的角速度的时序信号，使比预先设定的第1截止频率高的高频信号减小，

其中，所述坐标变换部基于减小了所述高频信号的角速度对所述相对位置矢量进行坐标变换。

6.根据权利要求1～4中任一项所述的误差修正装置，其特征在于，

所述传感器装置还具备加速度传感器，

所述误差修正装置还包括：第2低通滤波器，对于由所述加速度传感器测定的加速度的时序信号，使比预先设定的第2截止频率高的高频信号减小。

7.根据权利要求5所述的误差修正装置，其特征在于，

所述传感器装置还具备加速度传感器，

所述误差修正装置还包括：第2低通滤波器，对于由所述加速度传感器测定的加速度的时序信号，使比预先设定的第2截止频率高的高频信号减小。

Images