CN113038821B - 误差修正装置 - Google Patents
误差修正装置 Download PDFInfo
- Publication number
- CN113038821B CN113038821B CN201980071147.0A CN201980071147A CN113038821B CN 113038821 B CN113038821 B CN 113038821B CN 201980071147 A CN201980071147 A CN 201980071147A CN 113038821 B CN113038821 B CN 113038821B
- Authority
- CN
- China
- Prior art keywords
- sensor
- vector
- coordinate system
- error correction
- work vehicle
- 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.)
- Active
Links
- 238000012937 correction Methods 0.000 title claims abstract description 53
- 238000005259 measurement Methods 0.000 claims abstract description 37
- 230000009466 transformation Effects 0.000 claims abstract description 19
- 238000006243 chemical reaction Methods 0.000 claims abstract description 9
- 230000001133 acceleration Effects 0.000 claims description 21
- 238000010586 diagram Methods 0.000 description 5
- 230000005540 biological transmission Effects 0.000 description 4
- 239000011159 matrix material Substances 0.000 description 4
- 230000007246 mechanism Effects 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 238000000034 method Methods 0.000 description 2
- 235000013311 vegetables Nutrition 0.000 description 2
- 238000010276 construction Methods 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 102220298895 rs1025502215 Human genes 0.000 description 1
- 102220279244 rs1555053901 Human genes 0.000 description 1
- 102220012869 rs35689081 Human genes 0.000 description 1
- 102220162169 rs575633576 Human genes 0.000 description 1
- 102220055684 rs727504015 Human genes 0.000 description 1
- 102220096718 rs865838543 Human genes 0.000 description 1
- 238000000926 separation method Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
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
- 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
- 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/40—Correcting position, velocity or attitude
-
- 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60Y—INDEXING SCHEME RELATING TO ASPECTS CROSS-CUTTING VEHICLE TECHNOLOGY
- B60Y2200/00—Type of vehicle
- B60Y2200/20—Off-Road Vehicles
- B60Y2200/22—Agricultural vehicles
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Life Sciences & Earth Sciences (AREA)
- Mechanical Engineering (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Soil Sciences (AREA)
- Environmental Sciences (AREA)
- Chemical & Material Sciences (AREA)
- Combustion & Propulsion (AREA)
- Transportation (AREA)
- Guiding Agricultural Machines (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Steering Control In Accordance With Driving Conditions (AREA)
- Aviation & Aerospace Engineering (AREA)
- Automation & Control Theory (AREA)
Abstract
误差修正装置(34),设于作业车辆,对基于传感器装置的测定值的误差进行修正,传感器装置具备GNSS传感器和陀螺仪传感器,误差修正装置包括:第1取得部(341),取得由GNSS传感器测定的自身坐标系中的作业车辆的车辆位置、和由陀螺仪传感器测定的传感器坐标系中的角速度,传感器坐标系具有以传感器装置为基准的3个轴;坐标变换部(345),基于所取得的角速度,将在传感器坐标系中以设定于与传感器装置不同位置的设定位置为起点的传感器装置所设置的测定位置的相对位置矢量坐标变换为自身坐标系中的相对位置矢量;车辆位置修正部(346),设以自身坐标系的原点为起点的设定位置的位置矢量为Pa、测定位置的位置矢量为PS、坐标变换后的相对位置矢量为L,利用Pa=PS-L的式子算出设定位置的位置矢量Pa并对由GNSS传感器测得的车辆位置的误差进行修正。
Description
技术领域
本发明的实施方式涉及利用设于作业车辆的传感器装置进行的测定值的修正。
背景技术
以往,为了实现农业的高效率而进行如下的自动转向控制,使拖拉机、种植机等农用作业车辆沿着指定的行驶路径行驶。根据自动转向控制,基于作为目标的行驶路径上的位置与作业车辆的位置之间的分隔距离,对作业车辆的转向进行控制,以使作业车辆的位置定位于行驶路径上。
此外,作为与作业车辆的自动转向相关的技术,已知有如下的自动转向***,其具备:卫星定位模块,基于来自卫星的卫星信息而输出位置信息;前进基准位置算出部,基于位置信息算出前进行驶中的作业车的转向控制的作为基准位置的前进基准位置;后退基准位置算出部,基于位置信息算出后退行驶中的作业车的转向控制的作为基准位置的后退基准位置;转向控制部,在前进行驶时输出基于行驶路径与前进基准位置的偏差而算出的前进用转向控制信号,而且,在后退行驶时输出基于行驶路径与后退基准位置的偏差而算出的后退用转向控制信号;转向机构,基于前进用转向控制信号及后退用转向控制信号进行所述作业车的转向(例如,参照专利文献1)。
【专利文献1】日本特开2018-120364号公报
发明内容
在这样的自动转向控制中,测定作业车辆的车辆位置的GNSS(Global NavigationSatellite System,全球导航卫星***)传感器设置在适于从卫星接收GNSS信号的适当位置。作业车辆中的适于接收GNSS信号的适当位置是以从主体向上方延伸的方式设置的臂的上端部分、或通过该臂连接于其上端而被支承的车顶部。
此外,在作业车辆于起伏剧烈的农田上行驶的情况下,车辆的姿势会发生倾斜。此时,存在如下问题:由于GNSS传感器安装于上方,因此对于农田上的作业车辆的位置,由GNSS传感器测定的位置产生误差,进而由于该误差而导致自动转向控制的精度降低。
本发明是为了解决上述问题点而做出的,其目的在于提供能够降低利用GNSS传感器测定的作业车辆的车辆位置的误差的误差修正装置。
为了解决上述课题,本实施方式涉及的误差修正装置,其设于作业车辆,对基于传感器装置的测定值的误差进行修正,所述传感器装置具备GNSS传感器和陀螺仪传感器,所述误差修正装置包括:第1取得部,取得由所述GNSS传感器测定的自身坐标系中的所述作业车辆的车辆位置、和由所述陀螺仪传感器测定的传感器坐标系中的角速度,所述传感器坐标系具有以所述传感器装置为基准的3个轴;坐标变换部,基于所述取得的角速度,将在所述传感器坐标系中以设定于与所述传感器装置不同位置的设定位置为起点的所述传感器装置所设置的测定位置的相对位置矢量坐标变换为所述自身坐标系中的相对位置矢量;车辆位置修正部,设以所述自身坐标系的原点为起点的所述设定位置的位置矢量为Pa、所述测定位置的位置矢量为PS、所述坐标变换后的相对位置矢量为L,利用Pa=PS-L的式子算出所述设定位置的位置矢量Pa并对由所述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=CLS的式子将图5及图6所示的在传感器坐标系中以设定位置a为起点的测定位置s的相对位置矢量LS坐标变换为自身坐标系中的相对位置矢量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个轴的加速度分别设为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的车辆位置(S107)。
接着,坐标变换部345使用坐标变换矩阵C,利用ω=CωS的式子将由陀螺仪传感器31测定的在传感器坐标系中的拖拉机1的旋转角速度矢量ωS坐标变换为自身坐标系中的旋转角速度矢量ω(S108)。
坐标变换后,速度矢量修正部347设由GNSS传感器33测定的测定位置s处的拖拉机1的速度矢量为VS、设定位置a处的速度矢量为Va,利用Va=VS-ω×L的式子算出速度矢量Va,由此将在测定位置s测得的速度矢量VS修正为设定位置a处的速度矢量Va(S109)。
如此,误差修正装置34将在测定位置s测得的车辆位置、速度矢量各自修正为设定位置a处的车辆位置、速度矢量。根据修正后的车辆位置及速度矢量,自动转向控制装置21能够获得降低了特别是拖拉机1在具有凹凸的农田行驶时产生的传感器装置30的振动所引起的测定值的误差的、更准确的车辆位置、行进方位及行进速度。由此,可利用自动转向控制装置21实现准确的拖拉机1的自动转向。
需要说明的是,作为设有由误差修正装置34修正测定值误差的传感器装置30的作业车辆,除了拖拉机1以外,可举出种植机、联合收割机、蔬菜移载机、蔬菜收割机、割草机等设有以与农田接触的方式被升降控制的作业机的全部作业车辆。
本发明的实施方式是作为示例而提示,并不意图限定发明范围。上述新的实施方式可以以其他的各种方式来实施,在不脱离发明要旨的范围内可以进行各种省略、置换、变更。上述实施方式及其变形包含于发明范围、要旨,并且也包含于权利要求书记载的方案及其等同范围。
附图标记的说明
34误差修正装置
341第1取得部
345坐标变换部
346车辆位置修正部
Claims (7)
1.一种误差修正装置,设于作业车辆,对基于传感器装置的测定值的误差进行修正,所述传感器装置具备GNSS传感器和陀螺仪传感器,所述误差修正装置包括:
第1取得部,取得由所述GNSS传感器测定的自身坐标系中的所述作业车辆的车辆位置、和由所述陀螺仪传感器测定的传感器坐标系中的角速度,所述传感器坐标系具有以所述传感器装置为基准的3个轴;
坐标变换部,基于所述取得的角速度,将在所述传感器坐标系中以设定于与所述传感器装置不同位置的设定位置为起点的所述传感器装置所设置的测定位置的相对位置矢量坐标变换为所述自身坐标系中的相对位置矢量;以及
车辆位置修正部,设以所述自身坐标系的原点为起点的所述设定位置的位置矢量为Pa、所述测定位置的位置矢量为PS、所述坐标变换后的相对位置矢量为L,利用Pa=PS-L的式子算出所述设定位置的位置矢量Pa并对由所述GNSS传感器测得的车辆位置的误差进行修正。
2.根据权利要求1所述的误差修正装置,其特征在于,
所述作业车辆具备以与农田接触的方式被升降控制的作业机,
所述传感器坐标系中的1个轴为所述作业车辆的上下方向,
所述设定位置预先设定为将所述测定位置投影于所述农田的表面的位置,
所述误差修正装置还包括:
第2取得部,取得所述作业机的升降控制涉及的升降量;以及
设定位置修正部,基于所述取得的升降量来修正所述设定位置,所述坐标变换部对以修正后的设定位置为起点的测定位置的相对位置矢量进行坐标变换。
3.根据权利要求1所述的误差修正装置,其特征在于,
所述第1取得部还取得由所述GNSS传感器测定的所述自身坐标系中的所述作业车辆的速度矢量,
所述坐标变换部还基于所述取得的角速度将基于所述取得的角速度的所述传感器坐标系中的所述作业车辆的旋转角速度矢量坐标变换为所述自身坐标系中的旋转角速度矢量,
所述误差修正装置还包括:速度矢量修正部,设所述测定位置处的所述作业车辆的速度矢量为VS、所述设定位置处的所述作业车辆的速度矢量为Va、所述坐标变换后的旋转角速度矢量为ω,利用Va=VS-ω×L的式子算出所述设定位置处的所述作业车辆的速度矢量,对由所述GNSS传感器测定的所述作业车辆的速度矢量进行修正。
4.根据权利要求2所述的误差修正装置,其特征在于,
所述第1取得部还取得由所述GNSS传感器测定的所述自身坐标系中的所述作业车辆的速度矢量,
所述坐标变换部还基于所述取得的角速度将基于所述取得的角速度的所述传感器坐标系中的所述作业车辆的旋转角速度矢量坐标变换为所述自身坐标系中的旋转角速度矢量,
所述误差修正装置还包括:速度矢量修正部,设所述测定位置处的所述作业车辆的速度矢量为VS、所述设定位置处的所述作业车辆的速度矢量为Va、所述坐标变换后的旋转角速度矢量为ω,利用Va=VS-ω×L的式子算出所述设定位置处的所述作业车辆的速度矢量,对由所述GNSS传感器测定的所述作业车辆的速度矢量进行修正。
5.根据权利要求1~4中任一项所述的误差修正装置,其特征在于,还包括:
第1低通滤波器,对于由所述陀螺仪传感器测定的角速度的时序信号,使比预先设定的第1截止频率高的高频信号减小,
其中,所述坐标变换部基于减小了所述高频信号的角速度对所述相对位置矢量进行坐标变换。
6.根据权利要求1~4中任一项所述的误差修正装置,其特征在于,
所述传感器装置还具备加速度传感器,
所述误差修正装置还包括:第2低通滤波器,对于由所述加速度传感器测定的加速度的时序信号,使比预先设定的第2截止频率高的高频信号减小。
7.根据权利要求5所述的误差修正装置,其特征在于,
所述传感器装置还具备加速度传感器,
所述误差修正装置还包括:第2低通滤波器,对于由所述加速度传感器测定的加速度的时序信号,使比预先设定的第2截止频率高的高频信号减小。
Applications Claiming Priority (3)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2018-203759 | 2018-10-30 | ||
JP2018203759 | 2018-10-30 | ||
PCT/JP2019/042504 WO2020090863A1 (ja) | 2018-10-30 | 2019-10-30 | 誤差補正装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113038821A CN113038821A (zh) | 2021-06-25 |
CN113038821B true CN113038821B (zh) | 2022-12-06 |
Family
ID=70462085
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201980071147.0A Active CN113038821B (zh) | 2018-10-30 | 2019-10-30 | 误差修正装置 |
Country Status (5)
Country | Link |
---|---|
EP (1) | EP3874926B1 (zh) |
JP (1) | JP7269255B2 (zh) |
KR (1) | KR20210083247A (zh) |
CN (1) | CN113038821B (zh) |
WO (1) | WO2020090863A1 (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114771656B (zh) * | 2022-05-11 | 2024-06-21 | 山东理工大学 | 一种拖拉机前轮转向角测量方法及*** |
CN116972854B (zh) * | 2023-09-20 | 2023-12-26 | 无锡新松机器人自动化有限公司 | 基于gps定位的农机导航路径规划方法及*** |
Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2003303021A (ja) * | 2002-04-10 | 2003-10-24 | Mitsubishi Agricult Mach Co Ltd | 作業用走行車 |
CN101326094A (zh) * | 2005-10-15 | 2008-12-17 | 天宝导航有限公司 | 基于转向组件角度及角速率传感器的车辆陀螺仪 |
JP2012154769A (ja) * | 2011-01-26 | 2012-08-16 | Seiko Epson Corp | 加速度検出方法、位置算出方法及び加速度検出装置 |
CN103206965A (zh) * | 2012-01-13 | 2013-07-17 | 株式会社电装 | 车辆用陀螺仪的角速度误差修正装置、修正方法 |
WO2014015583A1 (zh) * | 2012-07-26 | 2014-01-30 | 北京京东方光电科技有限公司 | 传感器的制造方法 |
CN104460361A (zh) * | 2013-09-16 | 2015-03-25 | 迪尔公司 | 车辆自动运动控制*** |
EP2980668A1 (en) * | 2013-03-29 | 2016-02-03 | Tokyo Keiki Inc. | Work vehicle automatic steering system |
CN107054361A (zh) * | 2015-09-25 | 2017-08-18 | 株式会社斯巴鲁 | 车辆的转向控制装置 |
WO2017208306A1 (ja) * | 2016-05-30 | 2017-12-07 | 株式会社クボタ | 自動走行作業車両 |
WO2018030154A1 (ja) * | 2016-08-09 | 2018-02-15 | ヤンマー株式会社 | 自律走行システム |
JP2018120364A (ja) * | 2017-01-24 | 2018-08-02 | 株式会社クボタ | 自動操舵システム |
Family Cites Families (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8140223B2 (en) * | 2003-03-20 | 2012-03-20 | Hemisphere Gps Llc | Multiple-antenna GNSS control system and method |
JP4876204B2 (ja) * | 2006-01-05 | 2012-02-15 | ヒロボー株式会社 | 小型姿勢センサ |
US7418328B2 (en) * | 2006-05-08 | 2008-08-26 | Deere & Company | Steering logic for self-propelled mower |
US7844378B2 (en) * | 2006-10-05 | 2010-11-30 | Trimble Navigation Limited | Farm apparatus having implement sidehill drift compensation |
US20080195268A1 (en) * | 2007-02-09 | 2008-08-14 | Novariant, Inc. | Implement control system and method of using same |
BR102015013228B1 (pt) * | 2014-06-13 | 2020-11-24 | Cnh Industrial America Llc | SISTEMA E METODO DE CONTROLE PARA UM VEfCULO AGRiCOLA |
JP6497546B2 (ja) | 2015-02-06 | 2019-04-10 | 国立研究開発法人農業・食品産業技術総合研究機構 | 走行制御装置 |
CN105203098B (zh) * | 2015-10-13 | 2018-10-02 | 上海华测导航技术股份有限公司 | 基于九轴mems传感器的农业机械全姿态角更新方法 |
JP6473684B2 (ja) * | 2015-11-11 | 2019-02-20 | 日立建機株式会社 | 車輪の滑り角推定装置及びその方法 |
US10143126B2 (en) | 2016-06-10 | 2018-12-04 | Cnh Industrial America Llc | Planning and control of autonomous agricultural operations |
-
2019
- 2019-10-30 CN CN201980071147.0A patent/CN113038821B/zh active Active
- 2019-10-30 WO PCT/JP2019/042504 patent/WO2020090863A1/ja unknown
- 2019-10-30 JP JP2020553966A patent/JP7269255B2/ja active Active
- 2019-10-30 EP EP19878452.2A patent/EP3874926B1/en active Active
- 2019-10-30 KR KR1020217006482A patent/KR20210083247A/ko unknown
Patent Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2003303021A (ja) * | 2002-04-10 | 2003-10-24 | Mitsubishi Agricult Mach Co Ltd | 作業用走行車 |
CN101326094A (zh) * | 2005-10-15 | 2008-12-17 | 天宝导航有限公司 | 基于转向组件角度及角速率传感器的车辆陀螺仪 |
JP2012154769A (ja) * | 2011-01-26 | 2012-08-16 | Seiko Epson Corp | 加速度検出方法、位置算出方法及び加速度検出装置 |
CN103206965A (zh) * | 2012-01-13 | 2013-07-17 | 株式会社电装 | 车辆用陀螺仪的角速度误差修正装置、修正方法 |
WO2014015583A1 (zh) * | 2012-07-26 | 2014-01-30 | 北京京东方光电科技有限公司 | 传感器的制造方法 |
EP2980668A1 (en) * | 2013-03-29 | 2016-02-03 | Tokyo Keiki Inc. | Work vehicle automatic steering system |
CN104460361A (zh) * | 2013-09-16 | 2015-03-25 | 迪尔公司 | 车辆自动运动控制*** |
CN107054361A (zh) * | 2015-09-25 | 2017-08-18 | 株式会社斯巴鲁 | 车辆的转向控制装置 |
WO2017208306A1 (ja) * | 2016-05-30 | 2017-12-07 | 株式会社クボタ | 自動走行作業車両 |
WO2018030154A1 (ja) * | 2016-08-09 | 2018-02-15 | ヤンマー株式会社 | 自律走行システム |
JP2018120364A (ja) * | 2017-01-24 | 2018-08-02 | 株式会社クボタ | 自動操舵システム |
Also Published As
Publication number | Publication date |
---|---|
KR20210083247A (ko) | 2021-07-06 |
EP3874926A1 (en) | 2021-09-08 |
WO2020090863A1 (ja) | 2020-05-07 |
JPWO2020090863A1 (ja) | 2021-09-30 |
EP3874926A4 (en) | 2021-12-29 |
EP3874926B1 (en) | 2022-12-21 |
JP7269255B2 (ja) | 2023-05-08 |
CN113038821A (zh) | 2021-06-25 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP6552420B2 (ja) | 作業車 | |
JP6525902B2 (ja) | 圃場作業車両 | |
JP6576237B2 (ja) | 作業車 | |
JP6860387B2 (ja) | 作業車 | |
US11780499B2 (en) | Working vehicle and tractor | |
KR20160140832A (ko) | 병주 작업 시스템 | |
CN113038821B (zh) | 误差修正装置 | |
JP6811655B2 (ja) | 作業車 | |
US20160208460A1 (en) | System and method for calibrating blade of motor grader | |
WO2013080040A1 (en) | Method of estimating mounting angle error of gyroscopes by using a turning device, and corresponding turning device | |
JP2019166981A (ja) | 自律操舵装置 | |
JP6850759B2 (ja) | 自律操舵装置 | |
JP2019168939A (ja) | 自律走行システム | |
JP6871831B2 (ja) | 作業車両用の自律走行システム | |
US20180024252A1 (en) | Guidance system with navigation point correction | |
JP2023009115A (ja) | 作業車 | |
CN115244486A (zh) | 误差修正装置 | |
JP3949078B2 (ja) | 農作業機のローリング制御装置 | |
JP6546362B2 (ja) | 作業車 | |
JP3838959B2 (ja) | 農作業機のローリング制御装置 | |
JP3669947B2 (ja) | 作業機のローリング制御装置 | |
JP2020080743A (ja) | 車両の姿勢推定装置 | |
WO2022024528A1 (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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |