CN115244486A - 误差修正装置 - Google Patents

误差修正装置 Download PDF

Info

Publication number
CN115244486A
CN115244486A CN202180019638.8A CN202180019638A CN115244486A CN 115244486 A CN115244486 A CN 115244486A CN 202180019638 A CN202180019638 A CN 202180019638A CN 115244486 A CN115244486 A CN 115244486A
Authority
CN
China
Prior art keywords
sensor
work vehicle
velocity vector
set position
vector
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202180019638.8A
Other languages
English (en)
Inventor
荒金宏臣
吉田丰织
船山正行
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tokyo Keiki Inc
Original Assignee
Tokyo Keiki Inc
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Tokyo Keiki Inc filed Critical Tokyo Keiki Inc
Publication of CN115244486A publication Critical patent/CN115244486A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • A—HUMAN NECESSITIES
    • A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
    • A01B—SOIL WORKING IN AGRICULTURE OR FORESTRY; PARTS, DETAILS, OR ACCESSORIES OF AGRICULTURAL MACHINES OR IMPLEMENTS, IN GENERAL
    • A01B69/00—Steering of agricultural machines or implements; Guiding agricultural machines or implements on a desired track
    • A—HUMAN NECESSITIES
    • A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
    • A01B—SOIL WORKING IN AGRICULTURE OR FORESTRY; PARTS, DETAILS, OR ACCESSORIES OF AGRICULTURAL MACHINES OR IMPLEMENTS, IN GENERAL
    • A01B69/00—Steering of agricultural machines or implements; Guiding agricultural machines or implements on a desired track
    • A01B69/007—Steering or guiding of agricultural vehicles, e.g. steering of the tractor to keep the plough in the furrow
    • A01B69/008—Steering or guiding of agricultural vehicles, e.g. steering of the tractor to keep the plough in the furrow automatic
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62D—MOTOR VEHICLES; TRAILERS
    • B62D6/00—Arrangements for automatically controlling steering depending on driving conditions sensed and responded to, e.g. control circuits
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G05D1/0278—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using satellite positioning signals, e.g. GPS
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62D—MOTOR VEHICLES; TRAILERS
    • B62D15/00—Steering not otherwise provided for
    • B62D15/02—Steering position indicators ; Steering position determination; Steering aids
    • B62D15/025—Active steering aids, e.g. helping the driver by actively influencing the steering system after environment evaluation
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62D—MOTOR VEHICLES; TRAILERS
    • B62D6/00—Arrangements for automatically controlling steering depending on driving conditions sensed and responded to, e.g. control circuits
    • B62D6/002—Arrangements for automatically controlling steering depending on driving conditions sensed and responded to, e.g. control circuits computing target steering angles for front or rear wheels
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Mechanical Engineering (AREA)
  • Environmental Sciences (AREA)
  • Soil Sciences (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Transportation (AREA)
  • Combustion & Propulsion (AREA)
  • Chemical & Material Sciences (AREA)
  • Guiding Agricultural Machines (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种误差修正装置(34)，设于作业车辆，对基于传感器装置的测定值的误差进行修正，传感器装置具备GNSS传感器和陀螺仪传感器，误差修正装置包括：第一取得部(341)，取得由GNSS传感器测定的自身坐标系中的作业车辆的速度矢量和由陀螺仪传感器测定的传感器坐标系中的角速度，传感器坐标系具有以传感器装置为基准的3个轴；第二取得部(343)，取得作业车辆的转向量；设定位置确定部(344)，基于取得的转向量确定设定位置，设定位置在传感器坐标系中被设定在不同于传感器装置的位置；以及速度矢量修正部(347)，基于测定位置处的作业车辆的速度矢量和取得的角速度，算出确定的设定位置处的作业车辆的速度矢量，对由GNSS传感器测定的作业车辆的速度矢量进行修正。

Description

误差修正装置
技术领域
本发明的实施例涉及误差修正装置，利用设于作业车辆的传感器装置进行的测定值的修正。
背景技术
以往，为了实现农业的高效率而进行如下的自动转向控制，使拖拉机、种植机等作业车辆沿着指定的行驶路径行驶。根据这样的自动转向控制，基于作为目标的行驶路径上的位置与作业车辆的位置之间的分隔距离，对作业车辆的转向进行控制，以使作业车辆的位置定位于行驶路径上。
此外，作为与作业车辆的自动转向相关的技术，已知有如下的自动转向系统，其具备：卫星定位模块，基于来自卫星的卫星信息而输出位置信息；前进基准位置算出部，基于位置信息算出前进行驶中的作业车的转向控制的作为基准位置的前进基准位置；后退基准位置算出部，基于位置信息算出后退行驶中的作业车的转向控制的作为基准位置的后退基准位置；转向控制部，在前进行驶时输出基于行驶路径与前进基准位置的偏差而算出的前进用转向控制信号，并且在后退行驶时输出基于行驶路径与后退基准位置的偏差而算出的后退用转向控制信号；转向机构，基于前进用转向控制信号及后退用转向控制信号进行所述作业车的转向(例如，参照专利文献1)。
专利文献1：日本特开2018－120364号公报
发明内容
在作业车辆的自动转向控制中，有时使用由传感器装置测定的速度矢量，但存在该速度矢量根据作业车辆的行驶状态而产生与作业车辆的车体方位大幅度错位的误差的问题。
本发明是为了解决上述问题而做出的，其目的在于提供能够降低速度矢量的误差的技术。
为了解决上述课题，本实施例涉及的误差修正装置，设于作业车辆，对基于传感器装置的测定值的误差进行修正，所述传感器装置具备GNSS传感器和陀螺仪传感器，所述误差修正装置包括：第一取得部，取得由所述GNSS传感器测定的自身坐标系中的所述作业车辆的速度矢量、和由所述陀螺仪传感器测定的传感器坐标系中的角速度，所述传感器坐标系具有以所述传感器装置为基准的3个轴；第二取得部，取得所述作业车辆的转向量；设定位置确定部，基于所述取得的转向量确定设定位置，所述设定位置在所述传感器坐标系中被设定在不同于所述传感器装置的位置；以及速度矢量修正部，基于所述测定位置处的所述作业车辆的速度矢量和所述取得的角速度，算出所述确定的设定位置处的所述作业车辆的速度矢量，并对由所述GNSS传感器测定的所述作业车辆的速度矢量进行修正。
根据本发明，能够降低速度矢量的误差。
附图说明
图1是表示第一实施例涉及的农用拖拉机的构成的概略侧视图。
图2是表示第一实施例涉及的自动转向控制系统的整体构成的框图。
图3是表示误差修正装置的硬件构成的框图。
图4是表示误差修正装置的功能构成的框图。
图5是表示误差修正装置的动作的流程图。
图6是表示相对位置矢量的概略主视图。
图7是表示相对位置矢量的概略俯视图。
图8是表示速度矢量的概略主视图。
图9是表示设定位置修正处理的动作的流程图。
图10是表示通过前轮转向进行前进路线变更中的拖拉机的概略俯视图。
图11是表示通过前轮转向进行前进回转中的拖拉机的概略俯视图。
图12是表示车体方向算出处理的动作的流程图。
图13是表示通过前轮转向进行后退回转中的拖拉机的概略俯视图。
图14是表示通过后轮转向进行前进路线变更中的拖拉机的概略俯视图。
图15是表示通过后轮转向进行前进回转中的拖拉机的概略俯视图。
具体实施方式
以下，参照附图说明本发明的实施例。
＜第一实施例＞
(作业车辆的构成)
首先，对具有本实施例涉及的误差修正装置的作业车辆进行说明。图1是表示实施例涉及的农用拖拉机的构成的概略侧视图。
作为本实施例涉及的自动转向系统的转向对象的车辆是作业车辆，具体而言，为图1所示的农用拖拉机1。该拖拉机1是具备车体10、2个前轮11和2个后轮12，由后轮驱动行驶的四轮车辆，但只要是可转向的车辆，则不限于作业车辆，可以是任意车辆。此外，拖拉机1具备：供驾驶者就座的座席13、转向柱15、方向盘16、转向驱动装置17、包含油门踏板、制动器等的踏板类18、4个支承框架19a～19d、及车顶部19r。此外，在拖拉机1的车体10上通过连杆机构连结有作为设于作业车辆上的作业机的旋耕机41。
对前轮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的自动转向进行控制。
(自动转向控制系统的构成)
图2是表示实施例涉及的自动转向控制系统的整体构成的框图。
如图2所示，拖拉机1所具备的自动转向控制系统由对转向系统中的输入轴151进行驱动的转向驱动装置17、传感器装置30、自动转向控制装置21和图1中未图示的方向盘角度控制装置22构成。
转向驱动装置17具备：向输入轴151传递驱动力的传递轴171、驱动传递轴171的马达172、检测传递轴171的旋转量及旋转位置的编码器173，所述马达172为电动马达，所述编码器173为旋转编码器。需要说明的是，马达172只要是能够输出可使输入轴151旋转的充分的扭矩的马达，则可以是任意种类的马达。
自动转向控制装置21基于传感器装置30的测定值而输出转向量，方向盘角度控制装置22基于由自动转向控制装置21指示的转向量对转向驱动装置17反馈控制。在此，转向驱动装置17控制马达172使传递轴171驱动，以使由编码器173检测出的旋转位置成为所希望的旋转位置。
如此，通过对拖拉机1后附加设置转向驱动装置17、传感器装置30、自动转向控制装置21及方向盘角度控制装置22，从而能够在以手动转向的方式构成的拖拉机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输出。
(误差修正装置的构成)
对误差修正装置的硬件构成及功能构成进行说明。图3及图4分别是表示误差修正装置的硬件构成、功能构成的框图。
如图3所示，误差修正装置34中，作为硬件而具备：CPU(Central ProcessingUnit)91、RAM(RandomAccess Memory)92、存储装置93、外部I/F(Interface)94。
CPU91及RAM92协作而执行后述的各种功能，存储装置93存储由各种功能执行的处理所用的各种数据。外部I/F94与陀螺仪传感器31、加速度传感器32、GNSS传感器33、自动转向控制装置21进行数据的输入输出。需要说明的是，也可以自动转向控制装置21具备CPU、RAM、存储装置、外部I/F，取代误差修正装置34而执行后述的各种功能。
此外，如图4所示，误差修正装置34中，作为功能而具备：第一取得部341、具有第一低通滤波器342a及第二低通滤波器342b的低通滤波器342、第二取得部343、设定位置确定部344、坐标变换部345、车辆位置修正部346、速度矢量修正部347、第三取得部348、车体方位算出部349。
第一取得部341取得由陀螺仪传感器31、加速度传感器32、GNSS传感器33各自测定的测定值。第一低通滤波器342a针对由第一取得部341取得的由陀螺仪传感器31测得的测定值的时序信号，使比预先设定的第一截止频率高的高频信号减小。第二低通滤波器342b针对由第一取得部341取得的由加速度传感器32测得的测定值的时序信号，使比预先设定的第2截止频率高的高频信号减小。
如上所述，传感器装置30中包含GNSS传感器33，因此要接收GNSS信号就得优选在4个支承框架19a～19d中的任一个或支承于这些框架上的车顶部19r上安装传感器装置30。在拖拉机1等作业车辆中，相当于本实施例中的4个支承框架19a～19d这样的对车顶进行支承的部件的刚性不充分，尤其是作业车辆在凹凸剧烈的路面上行驶时等，会对传感器装置30施加高频振幅。这样的高频振幅尤其使陀螺仪传感器31、加速度传感器32测得的测定值产生误差，但利用低通滤波器342能够减少这样的误差。
第二取得部343取得由自动转向控制装置21输出的转向量。设定位置确定部344基于由第二取得部343取得的转向量来确定设定位置。在此，设定位置设定为将传感器装置30的测定位置投影于拖拉机1的行驶面、即农田表面所得的位置，是拖拉机1的宽度方向上的中心位置，关于拖拉机1的前后方向的位置是基于转向量来确定。
坐标变换部345将传感器坐标系中的相对位置矢量及角速度矢量各自坐标变换为自身坐标系中的矢量。在此，传感器坐标系是具有以传感器装置30所安装的位置、且传感器装置30进行测定的位置即测定位置作为原点而相互正交的x、y、z这3个轴的坐标系，设为x轴朝向拖拉机1的直进方向的前侧、y轴朝向拖拉机1的宽度方向的右侧、z轴朝向拖拉机1的上下方向的下侧。自身坐标系是具有向北、向东及向下的3个轴的坐标系。需要说明的是，关于相对位置矢量、角速度矢量将在后详述。
车辆位置修正部346将在拖拉机1中的上方、在本实施例中为安装于车顶部19r的传感器装置30的测定位置处由GNSS传感器33测定的自身坐标系中的拖拉机1的车辆位置修正为在传感器坐标系中预先设定的设定位置处的拖拉机1的车辆位置。速度矢量修正部347将在传感器装置30的测定位置处由GNSS传感器33测得的自身坐标系中的拖拉机1的速度矢量修正为设定位置处的速度矢量。
第三取得部348取得拖拉机1是处于前进状态还是后退状态来作为行驶方向。车体方位算出部349基于由第三取得部348取得的行驶状态和由速度矢量修正部347修正后的速度矢量，算出拖拉机1的车体10的朝向作为方位。
(误差修正装置的动作)
对误差修正装置的动作进行说明。图5是表示误差修正装置的动作的流程图。图6及图7分别是表示相对位置矢量的概略主视图、概略俯视图。图8是表示速度矢量的概略主视图。需要说明的是，图5所示的动作按规定的周期执行。
如图5所示，首先，第一取得部341取得由GNSS传感器33测定的自身坐标系中的车辆位置和速度矢量(S101)，并取得由陀螺仪传感器31测定的拖拉机1的角速度和由加速度传感器32测定的拖拉机1的加速度(S102)
接着，低通滤波器342对于由第一取得部341取得的角速度及加速度分别使比预先设定的第一截止频率、第二截止频率高的高频信号减小(S103)。
接着，执行后述的设定位置确定处理(S104)。
在设定位置确定处理执行之后，坐标变换部345基于由陀螺仪传感器31测定的拖拉机1的方位角、俯仰角、侧倾角，通过L＝CLS的式子将图6及图7所示的在传感器坐标系中以设定位置a为起点的测定位置s的相对位置矢量LS坐标变换为自身坐标系中的相对位置矢量L(S105)。在此，C表示用于从传感器坐标系向自身坐标系进行坐标变换的坐标变换矩阵。
【数学式1】
Figure BDA0003836596990000081
在此，关于坐标变换矩阵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个轴的加速度分别设为ax、ay、az而由下式求出。
【数学式3】
ax＝g·sinθ
ay＝-g·cosθ·sinφ
az＝-g·cosθ·cosφ
此外，作为初始方位的方位角ξ是基于在拖拉机1的行驶中由GNSS传感器33测定的速度矢量而设定。需要说明的是，关于初始姿势及初始方位，可以使用预先设定的值。
坐标变换后，车辆位置修正部346设以其自身坐标系的原点O为起点的设定位置a的位置矢量为Pa、测定位置s的位置矢量为PS，利用Pa＝PS-L的式子算出位置矢量Pa，由此将在测定位置s处测定的车辆位置修正为通过设定位置修正处理确定的设定位置a的车辆位置(S106)。
接着，坐标变换部345使用坐标变换矩阵C，利用ω＝CωS的式子将由陀螺仪传感器31测定的在传感器坐标系中的拖拉机1的角速度矢量ωS坐标变换为自身坐标系中的角速度矢量ω(S107)。
坐标变换后，速度矢量修正部347设由GNSS传感器33测定的测定位置s处的拖拉机1的速度矢量为VS、设定位置a处的速度矢量为Va，利用Va＝VS－ω×L的式子算出速度矢量Va，由此修正速度矢量(S108)。
在速度矢量修正后，执行后述的车体方位算出处理(S109)。
如此，误差修正装置34将在测定位置s测得的车辆位置、速度矢量各自修正为设定位置a处的车辆位置、速度矢量。根据修正后的车辆位置及速度矢量，自动转向控制装置21能够获得降低了特别是拖拉机1在具有凹凸的农田行驶时产生的传感器装置30的振动所引起的测定值的误差的、更准确的车辆位置、行进方位及行进速度。由此，可利用自动转向控制装置21实现准确的拖拉机1的自动转向。
(设定位置修正处理)
对设定位置修正处理的动作进行说明。图9是表示本实施例涉及的设定位置修正处理的动作的流程图。图10和图11分别是表示通过前轮转向进行前进路线变更中、前进回转中的拖拉机的概略俯视图。
如图9所示，首先，第二取得部343取得由自动转向控制装置21输出的转向量(S201)，设定位置确定部344判定所取得的转向量是否是预先设定的阈值以上(S202)。在此，阈值设定为拖拉机1进行回转的程度的转向量。
在转向量不是阈值以上的情况下(S202，否)，如图10所示，设定位置确定部344将作为拖拉机1的车辆重心或车辆运动中心的第一设定位置a1确定为设定位置a(S203)。需要说明的是，第一设定位置a1只要是至少在拖拉机1的前后方向上定位于前轮11的车轴中心f与后轮12的车轴中心r之间的位置即可。
另一方面，在转向量为阈值以上的情况下(S202，是)，如图11所示，设定位置确定部344将位于后轮12的车轴中心r的第二设定位置a2确定为设定位置a(S204)。需要说明的是，第二设定位置a2只要是至少定位于在拖拉机1的前后方向上比第一设定位置a1更靠后轮12侧、换言之非转向轮侧的位置即可。
如此，通过基于转向量修正设定位置，从而尤其在拖拉机1进行回转时，能够使设定位置a处的速度矢量Va的朝向更接近拖拉机1的车体10的朝向。
(车体方位算出处理)
对车体方位算出处理进行说明。图12是表示车体方向算出处理的动作的流程图。图13是表示通过前轮转向进行后退回转中的拖拉机的概略俯视图。
如图12所示，首先，第三取得部348取得拖拉机1是处于前进状态还是处于后退状态来作为行驶方向(S301)。在此，第三取得部348取得切换前进/后退的档杆的状态、传递发动机动力的变速箱中的前进档/后退档的切换状态等来作为行驶方向。
接着，车体方位算出部349判定由第三取得部348取得的行驶方向是否是后退(S302)。
在行驶方向为后退的情况下(S302，是)，如图13所示，车体方位算出部349将速度矢量Va设为x方向，即将速度矢量Va在拖拉机1的前后方向上反转(S303)，算出反转后的速度矢量Va的朝向作为车体方位(S304)。
另一方面，在行驶方向不是后退而是前进的情况下(S302，否)，车体方位算出部349算出速度矢量Va的朝向作为车体方位(S304)。
如此，通过在拖拉机1的行驶方向为后退的情况下将反转后的速度矢量Va的朝向作为车体方位，由此能够根据速度矢量Va算出车体方位。
＜第二实施例＞
本实施例中，拖拉机为后轮驱动，与此相伴，通过误差修正装置的设定位置确定处理确定的设定位置与第一实施例不同。图14和图15分别是表示通过后轮转向进行前进路线变更中、前进回转中的拖拉机的概略俯视图。
如图14和图15所示，本实施例涉及的拖拉机1a构成为具备作为非转向轮的前轮11a和作为转向轮的后轮12a的所谓后轮驱动车。
如图14所示，关于转向量小于阈值的情况下的设定位置、即第一设定位置，与第一实施例相同，处于在拖拉机1a的前后方向上前轮11a的车轴中心f与后轮12a的车轴中心r之间，优选是车辆重心或车辆运动中心。
如图15所示，关于转向量为阈值以上的情况下的设定位置、即第二设定位置a2，成为位于在拖拉机1a的前后方向上比第一设定位置a1更靠非转向轮侧的前轮11a的车轴中心f。
如此，通过将第二设定位置a2定位于比第一设定位置a1更靠非转向轮侧，由此不仅能够应对前轮转向的车辆，还能够应对后轮转向的车辆。
在上述的实施例中，将测定位置s处的车辆位置、速度矢量修正为设定位置a处的车辆位置、速度矢量，但也可以使车辆位置、速度矢量各自的设定位置a不同。例如可以是，将车辆位置总是修正为第一设定位置a1处的车辆位置，仅将速度矢量根据转向量修正为第一设定位置a1或第二设定位置a2中任一处的速度矢量。
本发明的实施例是作为示例而提示，并不意图限定发明范围。上述新的实施例可以以其他的各种方式来实施，在不脱离发明要旨的范围内可以进行各种省略、置换、变更。上述实施例及其变形包含于发明范围、要旨，并且也包含于权利要求书记载的方案及其等同范围。
附图标记的说明
34 误差修正装置
341 第一取得部
343 第二取得部
344 设定位置确定部
347 速度矢量修正部

Claims (8)

1.一种误差修正装置，设于作业车辆，对基于传感器装置的测定值的误差进行修正，所述传感器装置具备GNSS传感器和陀螺仪传感器，所述误差修正装置包括：
第一取得部，取得由所述GNSS传感器测定的自身坐标系中的所述作业车辆的速度矢量、和由所述陀螺仪传感器测定的传感器坐标系中的角速度，所述传感器坐标系具有以所述传感器装置为基准的3个轴；
第二取得部，取得所述作业车辆的转向量；
设定位置确定部，基于所述取得的转向量确定设定位置，所述设定位置在所述传感器坐标系中被设定在不同于所述传感器装置的位置；以及
速度矢量修正部，基于测定位置处的所述作业车辆的速度矢量和所述取得的角速度，算出所述确定的设定位置处的所述作业车辆的速度矢量，并对由所述GNSS传感器测定的所述作业车辆的速度矢量进行修正。
2.根据权利要求1所述的误差修正装置，其特征在于，
所述设定位置确定部在所述取得的转向量小于预先设定的阈值的情况下将所述设定位置确定为第一设定位置，在所述取得的转向量为所述阈值以上的情况下将所述设定位置确定为不同于所述第一设定位置的第二设定位置。
3.根据权利要求2所述的误差修正装置，其特征在于，
所述作业车辆中，仅前轮或后轮中的任一者为转向轮，
所述第一设定位置在所述作业车辆的前后方向上位于所述作业车辆的前轮与后轮之间，
所述第二设定位置位于在前后方向上相对于所述第一设定位置而言的非转向轮侧。
4.根据权利要求1～3中任一项所述的误差修正装置，其特征在于，
还包括车体方位算出部，其算出所述修正后的速度矢量的朝向作为所述作业车辆的车体方位。
5.根据权利要求4所述的误差修正装置，其特征在于，
还包括第三取得部，其取得表示所述作业车辆是处于前进状态还是后退状态的行驶方向，
所述车体方位算出部在所述取得的行驶方向为后退的情况下，算出使所述修正后的速度矢量反转而成的朝向作为所述作业车辆的车体方位。
6.根据权利要求1～5中任一项所述的误差修正装置，其特征在于，
还包括坐标变换部，其基于所述取得的角速度，将在所述传感器坐标系中以设定于与所述传感器装置不同位置的设定位置为起点的所述传感器装置所设置的测定位置的相对位置矢量坐标变换为所述自身坐标系中的相对位置矢量，所述坐标变换部还基于所述取得的角速度将基于所述取得的角速度的所述传感器坐标系中的所述作业车辆的角速度矢量坐标变换为所述自身坐标系中的角速度矢量，
所述速度矢量修正部基于所述测定位置处的所述作业车辆的速度矢量、所述坐标变换后的角速度矢量和所述坐标变换后的相对位置矢量，算出所述确定的设定位置处的所述作业车辆的速度矢量。
7.根据权利要求6所述的误差修正装置，其特征在于，
所述速度矢量修正部设所述测定位置处的所述作业车辆的速度矢量为VS、所述设定位置处的所述作业车辆的速度矢量为Va、所述坐标变换后的角速度矢量为ω、所述坐标变换后的相对位置矢量为L，利用Va＝VS－ω×L的式子算出所述确定的设定位置处的所述作业车辆的速度矢量。
8.根据权利要求6或7所述的误差修正装置，其特征在于，
还包括低通滤波器，其对于由所述陀螺仪传感器测定的角速度的时序信号，使比预先设定的第一截止频率高的高频信号减小，
所述坐标变换部基于减小了所述高频信号的角速度对所述相对位置矢量进行坐标变换。
CN202180019638.8A 2020-03-18 2021-03-18 误差修正装置 Pending CN115244486A (zh)

Applications Claiming Priority (3)

Application Number Priority Date Filing Date Title
JP2020048228 2020-03-18
JP2020-048228 2020-03-18
PCT/JP2021/011182 WO2021187594A1 (ja) 2020-03-18 2021-03-18 誤差補正装置

Publications (1)

Publication Number Publication Date
CN115244486A true CN115244486A (zh) 2022-10-25

Family

ID=77768460

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202180019638.8A Pending CN115244486A (zh) 2020-03-18 2021-03-18 误差修正装置

Country Status (5)

Country Link
EP (1) EP4123408A4 (zh)
JP (1) JPWO2021187594A1 (zh)
KR (1) KR20220155266A (zh)
CN (1) CN115244486A (zh)
WO (1) WO2021187594A1 (zh)

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8190337B2 (en) * 2003-03-20 2012-05-29 Hemisphere GPS, LLC Satellite based vehicle guidance control in straight and contour modes
JP5625293B2 (ja) * 2009-09-14 2014-11-19 ソニー株式会社 速度算出装置、速度算出方法及びナビゲーション装置
JP6386839B2 (ja) * 2014-09-04 2018-09-05 日立建機株式会社 位置算出装置及び運搬車両
CA2941233C (en) * 2015-11-30 2019-01-08 Komatsu Ltd. Work machine control system, work machine, work machine management system, and method for controlling work machine
JP6994831B2 (ja) 2017-01-24 2022-01-14 株式会社クボタ 自動操舵システム

Also Published As

Publication number Publication date
JPWO2021187594A1 (zh) 2021-09-23
WO2021187594A1 (ja) 2021-09-23
KR20220155266A (ko) 2022-11-22
EP4123408A1 (en) 2023-01-25
EP4123408A4 (en) 2023-09-27

Similar Documents

Publication Publication Date Title
JP6552420B2 (ja) 作業車
JP4816732B2 (ja) 運転支援システムおよび運転支援方法
JP6332167B2 (ja) 車線維持支援装置
JP6860387B2 (ja) 作業車
US11079237B2 (en) Method for determining a relative position of a motor vehicle, position determination system for a motor vehicle and motor vehicle
JP6811655B2 (ja) 作業車
JP4696671B2 (ja) 車両制御装置
JP2007296947A (ja) 車両の操舵装置
JP7072496B2 (ja) 自動走行作業車のための制御装置
US20210053617A1 (en) Control device and steering device
JP6850759B2 (ja) 自律操舵装置
JP5338968B2 (ja) 操舵制御装置
JP3707379B2 (ja) 車両の前後輪舵角制御装置
JP6928571B2 (ja) 自律走行システム
CN113038821B (zh) 误差修正装置
CN115244486A (zh) 误差修正装置
JP7012001B2 (ja) 車両の姿勢推定装置
JP3838959B2 (ja) 農作業機のローリング制御装置
JP2007186099A (ja) 車両の操舵装置
JP2024035299A (ja) ステアリングシステム
JP4544161B2 (ja) 車両の操舵装置
JP2010184509A (ja) 後輪操舵装置
CN116811857A (zh) 一种车辆控制方法、装置和计算机可读存储介质
JP2001191939A (ja) 車両用軌道制御装置
JP2004113028A (ja) 農作業機のローリング制御装置

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination