CN105358769B - 作业机械的姿态运算装置、作业机械及作业机械的姿态运算方法 - Google Patents
作业机械的姿态运算装置、作业机械及作业机械的姿态运算方法 Download PDFInfo
- Publication number
- CN105358769B CN105358769B CN201480000981.8A CN201480000981A CN105358769B CN 105358769 B CN105358769 B CN 105358769B CN 201480000981 A CN201480000981 A CN 201480000981A CN 105358769 B CN105358769 B CN 105358769B
- Authority
- CN
- China
- Prior art keywords
- attitude angle
- work machine
- acceleration
- target
- angle
- 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
Classifications
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F9/00—Component parts of dredgers or soil-shifting machines, not restricted to one of the kinds covered by groups E02F3/00 - E02F7/00
- E02F9/26—Indicating devices
- E02F9/264—Sensors and their calibration for indicating the position of the work tool
- E02F9/265—Sensors and their calibration for indicating the position of the work tool with follow-up actions (e.g. control signals sent to actuate the work tool)
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F3/00—Dredgers; Soil-shifting machines
- E02F3/04—Dredgers; Soil-shifting machines mechanically-driven
- E02F3/28—Dredgers; Soil-shifting machines mechanically-driven with digging tools mounted on a dipper- or bucket-arm, i.e. there is either one arm or a pair of arms, e.g. dippers, buckets
- E02F3/36—Component parts
- E02F3/42—Drives for dippers, buckets, dipper-arms or bucket-arms
- E02F3/43—Control of dipper or bucket position; Control of sequence of drive operations
- E02F3/435—Control of dipper or bucket position; Control of sequence of drive operations for dipper-arms, backhoes or the like
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F9/00—Component parts of dredgers or soil-shifting machines, not restricted to one of the kinds covered by groups E02F3/00 - E02F7/00
- E02F9/08—Superstructures; Supports for superstructures
- E02F9/10—Supports for movable superstructures mounted on travelling or walking gears or on other superstructures
- E02F9/12—Slewing or traversing gears
- E02F9/121—Turntables, i.e. structure rotatable about 360°
- E02F9/123—Drives or control devices specially adapted therefor
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F9/00—Component parts of dredgers or soil-shifting machines, not restricted to one of the kinds covered by groups E02F3/00 - E02F7/00
- E02F9/26—Indicating devices
- E02F9/261—Surveying the work-site to be treated
- E02F9/262—Surveying the work-site to be treated with follow-up actions to control the work tool, e.g. controller
-
- 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
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F3/00—Dredgers; Soil-shifting machines
- E02F3/04—Dredgers; Soil-shifting machines mechanically-driven
- E02F3/28—Dredgers; Soil-shifting machines mechanically-driven with digging tools mounted on a dipper- or bucket-arm, i.e. there is either one arm or a pair of arms, e.g. dippers, buckets
- E02F3/30—Dredgers; Soil-shifting machines mechanically-driven with digging tools mounted on a dipper- or bucket-arm, i.e. there is either one arm or a pair of arms, e.g. dippers, buckets with a dipper-arm pivoted on a cantilever beam, i.e. boom
- E02F3/32—Dredgers; Soil-shifting machines mechanically-driven with digging tools mounted on a dipper- or bucket-arm, i.e. there is either one arm or a pair of arms, e.g. dippers, buckets with a dipper-arm pivoted on a cantilever beam, i.e. boom working downwardly and towards the machine, e.g. with backhoes
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F9/00—Component parts of dredgers or soil-shifting machines, not restricted to one of the kinds covered by groups E02F3/00 - E02F7/00
- E02F9/20—Drives; Control devices
- E02F9/22—Hydraulic or pneumatic drives
- E02F9/2278—Hydraulic circuits
- E02F9/2285—Pilot-operated systems
Landscapes
- Engineering & Computer Science (AREA)
- Structural Engineering (AREA)
- Mining & Mineral Resources (AREA)
- Civil Engineering (AREA)
- General Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Operation Control Of Excavators (AREA)
- Component Parts Of Construction Machinery (AREA)
Abstract
作业机械的姿态运算装置包括:检测装置,其设于作业机械,并检测角速度及加速度;第一姿态角运算部,其设于所述检测装置,并根据由所述检测装置检测到的所述角速度及所述加速度来求出所述作业机械的姿态角;低通滤波器,其使所述第一姿态角运算部求出的姿态角通过并将其作为第一姿态角而输出;第二姿态角运算部,其将根据由所述检测装置检测到的所述角速度及所述加速度来求出的姿态角作为第二姿态角而输出;选择部,其基于所述作业机械的与角度变动相关的信息来切换所述第一姿态角与所述第二姿态角并将其输出。
Description
技术领域
本发明涉及作业机械的姿态运算装置、作业机械及作业机械的姿态运算方法。
背景技术
近年来,在液压挖掘机或推土机等作业机械中,存在如下技术:以避免超过挖掘对象的不可侵入区域的边界线而进一步挖掘的方式控制作业机,使其沿着边界线挖掘(例如,专利文献1)。
在先技术文献
专利文献
专利文献1:国际公开第1995/030059号公报
发明要解决的课题
在使作业机械沿着表示作业机的挖掘对象的目标形状的目标挖掘地形进行挖掘的情况下,需要求出作业机械所具备的作业机的位置,例如,在液压挖掘机中,需要求出铲斗的铲边的位置。这种情况下,需要准确地求出作业机械的与倾斜相关的信息。例如,将IMU(Inertial Measurement Unit:惯性计测装置)搭载于作业机械,根据IMU的检测值来得到倾滚角及俯仰角这样的倾斜度角作为作业机的与倾斜相关的信息。
在作业机械进行动作的情况下,需要求出与作业机械的动作对应的作业机的位置,使作业机械沿着目标挖掘地形进行挖掘,并以抑制超过边界线地挖入挖掘对象的方式控制作业机,因此,要求检测姿态角时的响应性高。然而,若提高检测姿态角时的响应性,则IMU检测到的姿态角有时会以短周期发生变动。因此,在作业机械静止时使作业机械沿着目标挖掘地形进行挖掘,以抑制超过边界线地挖入挖掘对象的方式控制作业机的情况下,作业机的位置的检测结果有时会出现振动。
发明内容
本发明目的在于,以无论作业机械的动作状态如何都能够抑制超过目标挖掘地形地挖入挖掘对象的方式控制作业机。
解决方案
本发明涉及一种作业机械的姿态运算装置,其用于求出具备作业机的作业机械的姿态角,其中,所述作业机械的姿态运算装置包括:检测装置,其设于所述作业机械,并用于检测角速度及加速度;第一姿态角运算部,其设于所述检测装置,并根据由所述检测装置检测到的所述角速度及所述加速度来求出所述作业机械的姿态角;低通滤波器,其使所述第一姿态角运算部求出的姿态角通过并将该姿态角作为第一姿态角而输出;第二姿态角运算部,其将根据由所述检测装置检测到的所述角速度及所述加速度来求出的姿态角作为第二姿态角而输出;选择部,其基于所述作业机械的与角度变动相关的信息来切换所述第一姿态角与所述第二姿态角并将其输出。
本发明涉及一种作业机械的姿态运算装置,其用于求出具备作业机的作业机械的姿态角,其中,所述作业机械的姿态运算装置包括:检测装置,其设于所述作业机械,并用于检测角速度及加速度;姿态角运算部,其设于所述检测装置,并根据由所述检测装置检测到的所述角速度及所述加速度来求出所述作业机械的姿态角;低通滤波器,其对由所述姿态角运算部求出的姿态角进行滤波处理,而作为第一姿态角求出;选择部,其在所述姿态角运算部中将所述姿态角作为第二姿态角,基于所述作业机械的与角度变动相关的信息来切换所述第一姿态角与所述第二姿态角并将其输出。
本发明优选的是,所述第二姿态角运算部包括:第一互补滤波器,其设定第一截止频率,减少根据由所述检测装置检测到的所述角速度及所述加速度来求出的所述姿态角中含有的杂音,并输出第三姿态角;第二互补滤波器,其设定与所述第一截止频率不同的第二截止频率,减少根据由所述检测装置检测到的所述角速度及所述加速度来求出的所述姿态角中含有的杂音,并输出第四姿态角;切换部,其根据所述作业机械的动作的状态,切换所述第三姿态角或所述第四姿态角并将其作为所述第二姿态角而输出。
优选的是,所述第二姿态角是所述作业机械的姿态角通过了截止频率比所述低通滤波器高的滤波器后的姿态角。
优选的是,所述与角度变动相关的信息是所述作业机械的与回旋相关的信息。
优选的是,所述作业机械具备行驶体和在所述行驶体的上部设置的回旋体,所述作业机械的与回旋相关的信息是所述回旋体的回旋速度,在所述回旋速度为规定的阈值以下时,所述选择部输出所述第一姿态角,在所述回旋速度超过了规定的阈值时,所述选择部输出所述第二姿态角。
优选的是,在所述第一姿态角与所述第二姿态角的差量超过了规定的阈值时,所述选择部输出所述第二姿态角。
优选的是,在所述回旋体的回旋速度为规定的阈值以下时,所述选择部输出所述第一姿态角,在所述回旋速度超过了规定的阈值时,所述选择部输出所述第二姿态角,在所述第一姿态角与所述第二姿态角的差量超过了规定的阈值时,所述选择部输出所述第二姿态角。
本发明涉及一种作业机械,其中,所述作业机械具备前述的作业机械的姿态运算装置,所述作业机械使用从所述作业机械的姿态运算装置输出的所述第一姿态角或所述第二姿态角来求出所述作业机械的至少一部分的位置信息。
优选的是,所述作业机械具有:位置检测装置,其用于检测所述作业机械的位置信息;目标挖掘地形生成装置,其基于从所述作业机械的姿态运算装置输出的所述作业机械的至少一部分的位置信息和由所述位置检测装置检测到的位置信息来求出所述作业机的位置,并根据表示目标形状的目标施工面的信息来生成与表示所述作业机的挖掘对象的目标形状的目标挖掘地形相关的信息。
优选的是,所述作业机械具有显示装置,该显示装置基于用于显示所述目标挖掘地形的显示用的信息来显示所述目标挖掘地形。
优选的是,所述作业机械具有作业机控制部,该作业机控制部基于从所述目标挖掘地形生成装置获取的与所述目标挖掘地形相关的信息,执行以使所述作业机接近挖掘对象的方向的速度成为限制速度以下的方式进行控制的挖掘控制。
本发明涉及一种作业机械的姿态运算方法,其用于求出具备作业机的作业机械的姿态角,其中,使所述作业机械的姿态角通过低通滤波器并将该姿态角作为第一姿态角而输出,并将所述作业机械的姿态角作为第二姿态角而输出,基于所述作业机械的与角度变动相关的信息来切换所述第一姿态角与所述第二姿态角并将其输出。
本发明能够以无论作业机械的动作状态如何都能够抑制超过目标挖掘地形地挖入挖掘对象的方式控制作业机。
附图说明
图1A是本实施方式的作业机械的立体图。
图1B是本实施方式的作业机械的侧视图。
图2是表示本实施方式的作业机械的控制***的图。
图3A是表示目标施工面的一例的示意图。
图3B是表示作业机控制装置及第二显示装置的框图。
图4是表示目标挖掘地形与铲斗的铲边之间的关系的一例的图。
图5是表示目标速度、垂直速度分量、水平速度分量之间的关系的示意图。
图6是表示垂直速度分量和水平速度分量的计算方法的图。
图7是表示垂直速度分量和水平速度分量的计算方法的图。
图8是表示铲边与目标挖掘地形之间的距离的示意图。
图9是表示限制速度信息的一例的坐标图。
图10是表示动臂的限制速度的垂直速度分量的计算方法的示意图。
图11是表示动臂的限制速度的垂直速度分量与动臂的限制速度之间的关系的示意图。
图12是表示铲边的移动引起的动臂的限制速度的变化的一例的图。
图13是表示本实施方式的控制***及液压***的一例的示意图。
图14是将图13的一部分放大的图。
图15是表示IMU的一例的框图。
图16是传感器控制装置的控制框图。
图17是用于说明上部回旋体的回旋速度的图。
图18是表示互补滤波器的特性的图。
图19是表示误差及误差的频率特性的图。
图20是表示第一互补滤波器的增益及第二互补滤波器的增益与频率之间的关系的图。
图21是表示第二姿态角运算部的切换部输出的第二姿态角、第三姿态角、第四姿态角的时间变化的一例的图。
图22是表示求出第二姿态角的处理的一例的流程图。
图23是表示本实施方式的变形例的第三姿态角与第四姿态角的切换所使用的表的一例的图。
图24是表示本实施方式的姿态角计算方法的第一例的处理次序的流程图。
图25是用于说明俯仰角的变化的图。
图26是表示本实施方式的第二姿态角计算处理方法的处理次序的流程图。
图27是具备消除离心力的功能的传感器控制装置的控制框图。
图28是用于说明IMU的安装位置的一例的图。
图29是用于说明液压挖掘机的局部坐标系和IMU的局部坐标系的图。
图30是第一变形例的传感器控制装置的控制框图。
图31是第二变形例的传感器控制装置的框图。
具体实施方式
参照附图,详细说明用于实施本发明的方式(实施方式)。
<作业机械的整体结构>
图1A是本实施方式的作业机械的立体图。图1B是本实施方式的作业机械的侧视图。图2是表示本实施方式的作业机械的控制***的图。作为作业机械的液压挖掘机100具有作为主体部的车辆主体1和作业机2。车辆主体1具有作为回旋体的上部回旋体3和作为行驶体的行驶装置5。上部回旋体3在发动机室3EG的内部收容有图2所示的作为动力产生装置的发动机36及液压泵37等装置。发动机室3EG配置在上部回旋体3的一端侧。
在本实施方式中,液压挖掘机100的作为动力产生装置的发动机36使用例如柴油发动机等内燃机,但动力产生装置并不限定于此。液压挖掘机100的动力产生装置也可以是例如将内燃机、发电电动机、蓄电装置组合而成的所谓混合动力方式的装置。
上部回旋体3具有驾驶室4。驾驶室4设置在上部回旋体3的另一端侧。即,驾驶室4设置在与配置发动机室3EG的一侧相反的一侧。在驾驶室4内,配置有图2所示的第一显示装置28及操作装置30。关于第一显示装置28及操作装置30在后文叙述。在上部回旋体3的上方安装有扶手19。
行驶装置5搭载上部回旋体3。行驶装置5具有履带5a、5b。行驶装置5的在左右设置的液压马达5c的一方或双方进行驱动,履带5a、5b旋转,由此使液压挖掘机100行驶。作业机2安装在上部回旋体3的驾驶室4的侧方侧。
液压挖掘机100可以是代替履带5a、5b而具备轮胎、且具备将图2所示的发动机36的驱动力经由变速器向轮胎传递而能够行驶的行驶装置的方式。作为这样的方式的液压挖掘机100,有例如轮式液压挖掘机。而且,液压挖掘机100具有如下结构:具备这样的具有轮胎的行驶装置,而且在车辆主体(主体部)上安装作业机,而不具备图1所示那样的上部回旋体3及其回旋机构,液压挖掘机100可以是例如反铲装载机。即,反铲装载机在车辆主体上安装作业机,且具备构成车辆主体的一部分的行驶装置。
上部回旋体3的配置作业机2及驾驶室4的一侧为前,配置发动机室3EG的一侧为后。朝向前方的左侧为上部回旋体3的左方,朝向前方的右侧为上部回旋体3的右方。而且,液压挖掘机100或车辆主体1以上部回旋体3为基准的行驶装置5侧为下,以行驶装置5为基准的上部回旋体3侧为上。在液压挖掘机100设置于水平面的情况下,下为铅垂方向即重力的作用方向侧,上为铅垂方向的相反侧。
作业机2具有动臂6、斗杆7、铲斗8、动臂油缸10、斗杆油缸11、铲斗油缸12。动臂6的基端部经由动臂销13以能够转动的方式安装在车辆主体1的前部。斗杆7的基端部经由斗杆销14以能够转动的方式安装在动臂6的前端部。在斗杆7的前端部经由铲斗销15安装有铲斗8。铲斗8以铲斗销15为中心进行转动。铲斗8在与铲斗销15相反的一侧安装有多个铲8B。铲边8T是铲8B的前端。
铲斗8也可以不具有多个铲8B。即,也可以不具有图1所示的铲8B而是铲边由钢板形成为直线形状的铲斗。作业机2可以具备例如具有一个铲的倾转铲斗。倾转铲斗是如下的铲斗:具备铲斗倾转油缸,通过铲斗向左右进行倾转倾斜,由此即使液压挖掘机处于倾斜地面也能够将斜面、平地进行成形、平整为自由的形状,且也能够进行基于底板的碾压作业。除此之外,作业机2也可以代替铲斗8而具备倾斜面铲斗或具备凿岩用的尖片的凿岩用的附件等。
图1A所示的动臂油缸10、斗杆油缸11、铲斗油缸12分别是由工作油的压力(以下,适当称为液压)来驱动的液压油缸。动臂油缸10对动臂6进行驱动而使动臂6升降。斗杆油缸11对斗杆7进行驱动而使斗杆7以斗杆销14为中心转动。铲斗油缸12对铲斗8进行驱动而使铲斗8以铲斗销15为中心转动。
在动臂油缸10、斗杆油缸11及铲斗油缸12等液压油缸与图2所示的液压泵37之间设有图2所示的液压控制阀38。液压控制阀38包括:用于驱动液压马达5c的行驶用控制阀;用于对动臂油缸10、斗杆油缸11及铲斗油缸12、以及使上部回旋体3回旋的回旋马达进行控制的作业机用控制阀。图2所示的作业机控制装置25通过控制液压控制阀38来控制向动臂油缸10、斗杆油缸11、铲斗油缸12、回旋马达或液压马达5c供给的工作油的流量。其结果是,动臂油缸10、斗杆油缸11及铲斗油缸12等的动作得以控制。
在上部回旋体3的上部安装有天线20、21。天线20、21为了检测液压挖掘机100的当前位置而被使用。天线20、21与图2所示的用于检测液压挖掘机100的当前位置的全局坐标运算部23电连接。全局坐标运算部23利用RTK-GNSS(Real Time Kinematic-GlobalNavigation Satellite Systems,GNSS称为全球导航卫星***)来检测液压挖掘机100的当前位置。在以下的说明中,将天线20、21适当称为GNSS天线20、21。
与GNSS天线20、21接收到的GNSS电波对应的信号向全局坐标运算部23输入。全局坐标运算部23检测GNSS天线20、21的设置位置。GNSS天线20、21的设置位置是液压挖掘机100的位置信息。
GNSS天线优选在上部回旋体3之上设置在沿着液压挖掘机100的左右方向分离的两端位置。在本实施方式中,GNSS天线20、21安装于在上部回旋体3的宽度方向两侧分别安装的扶手19上。GNSS天线20、21安装于上部回旋体3的位置并不限定于扶手19,但是GNSS天线20、21设置在尽可能分离的位置上的话,液压挖掘机100的当前位置的检测精度提高,因此优选。而且,GNSS天线20、21优选设置在尽量不妨碍操作员的视野的位置。
使用图1B,对全局坐标系及液压挖掘机100的局部坐标系进行说明。全局坐标系是成为液压挖掘机100的设置于作业区域GA的基准的、例如以基准桩80的基准位置PG为基准的由(X,Y,Z)表示的三维坐标系。如图3A所示,基准位置PG位于例如在作业区域GA设置的基准桩80的前端80T。在本实施方式中,全局坐标系例如是GNSS的坐标系。
液压挖掘机100的局部坐标系是以液压挖掘机100为基准的由(x,y,z)表示的三维坐标系。局部坐标系中,与z轴正交且与作业机2的动臂6及斗杆7转动的轴正交的轴为x轴,与x轴正交的轴为y轴。x轴是与上部回旋体3的前后方向平行的轴,y轴是与上部回旋体3的宽度方向(横向)平行的轴。在本实施方式中,局部坐标系的基准位置PL位于例如上部回旋体3回旋用的摆动圆上。
图1B所示的角度α1是动臂6的倾斜度角,角度α2是斗杆7的倾斜度角,角度α3是铲斗8的倾斜度角,角度θ5是车辆主体1的相对于前后方向的姿态角。倾斜角θ5是液压挖掘机100的俯仰角。倾斜角θ5即液压挖掘机100的俯仰角θ5是表示局部坐标相对于全局坐标的倾斜度的角度。
(液压挖掘机的控制***)
使用图2,对液压挖掘机100的控制***进行说明。液压挖掘机100包括作为作业机械的姿态运算装置的传感器控制装置24、作业机控制装置25、发动机控制装置26、泵控制装置27、第一显示装置28、检测角速度及加速度的IMU(Inertial Measurement Unit:惯性计测装置)29、第二显示装置39作为控制***。它们设置在上部回旋体3的内部。在本实施方式中,IMU29在驾驶室4下部,安装于上部回旋体3上部的高刚性的框架。除此以外的装置设置在驾驶室4内。如图1B所示,IMU29设置在从成为上部回旋体3的旋转中心的z轴分离的位置。
传感器控制装置24、作业机控制装置25、发动机控制装置26、泵控制装置27、第一显示装置28与设置在液压挖掘机100内的车内信号线41电连接。传感器控制装置24、作业机控制装置25、发动机控制装置26、泵控制装置27、第一显示装置28经由车内信号线41而能够相互通信。传感器控制装置24、IMU29、第二显示装置39与不同于车内信号线41的车内信号线42电连接。传感器控制装置24、IMU29、第二显示装置39经由车内信号线42而能够相互通信。全局坐标运算部23与第二显示装置39通过车内信号线43而电连接,且经由车内信号线43能够相互通信。IMU29也可以不与车内信号线42电连接,而是与车内信号线41电连接,从而与和车内信号线41电连接的其他电子设备能够相互通信。
传感器控制装置24电连接有对图1所示的动臂油缸10、斗杆油缸11及铲斗油缸12的行程进行检测的传感器、以及对上部回旋体3的回旋角度进行检测的传感器等这样的各种传感器类35。动臂6的角度及斗杆7的角度例如由对动臂油缸10等的行程的变化进行检测的传感器来检测。传感器控制装置24对各种传感器类35检测到的信号实施了滤波处理或A/D(Analog/Digital)转换等各种信号处理之后,向车内信号线41输出。
传感器控制装置24从车内信号线42获取IMU29输出的信号。IMU29输出的信号例如是加速度及角速度。在本实施方式中,IMU29根据自身检测到的加速度及角速度来求出姿态角并输出,因此该姿态角也是IMU29输出的信号。IMU29输出的姿态角是IMU29自身的姿态角,并且也是设置IMU29的作为作业机械的液压挖掘机100的姿态角。传感器控制装置24获取动臂油缸10、斗杆油缸11及铲斗油缸12各自具备的各行程传感器检测到的检测值,并将各检测值作为动臂6的倾斜度角α1、斗杆7的倾斜度角α2及铲斗8的倾斜度角α3而计算。
传感器控制装置24基于液压挖掘机100的与角度变化相关的信息,来切换通过低通滤波器的第一姿态角与未通过该低通滤波器的第二姿态角并将其输出。与角度变化相关的信息包括例如液压挖掘机100的与包括回旋角度的变化的回旋相关的信息及与俯仰角的变化相关的信息等。在本实施方式中,传感器控制装置24使由IMU29求出的姿态角通过低通滤波器后并将其作为第一姿态角输出,使用从IMU29获取的加速度及角速度来求出姿态角,对求出的姿态角实施滤波处理而除去噪声后,使其不通过前述的低通滤波器而作为第二姿态角输出。并且,传感器控制装置24根据液压挖掘机100的与回旋相关的信息例如图1所示的上部回旋体3的回旋速度的大小来切换第一姿态角与第二姿态角并将其输出。回旋速度是利用时间对回旋角度进行微分所得到的值,相当于回旋角度的变化。由IMU29求出的姿态角、使用IMU29检测到的加速度及角速度来求出的姿态角、第一姿态角以及第二姿态角都是液压挖掘机100的与倾斜相关的信息。关于传感器控制装置24的处理的详细情况在后文叙述。
作业机控制装置25基于来自操作装置30的输入,对图1所示的作业机2的动作进行控制。操作装置30具有作为操作部的作业机操作构件31L、31R及行驶操作构件33L、33R。在本实施方式中,作业机操作构件31L、31R及行驶操作构件33L、33R是先导压方式的杆,但并不限定于此。作业机操作构件31L、31R及行驶操作构件33L、33R也可以是例如电气方式的杆。
例如,操作装置30具有在操作员的左侧设置的左操作杆31L和在操作员的右侧配置的右操作杆31R。左操作杆31L及右操作杆31R的前后左右的动作对应于两轴的动作。右操作杆31R的前后方向的操作对应于动臂6的操作。当右操作杆31R被向前方操作时,动臂6下降,当右操作杆31R被向后方操作时,动臂6上升。对应于右操作杆31R的前后方向的操作来执行动臂6的下降上升的动作。右操作杆31R的左右方向的操作对应于铲斗8的操作。当右操作杆31R被向左侧操作时,铲斗8进行挖掘,当右操作杆31R被向右侧操作时,铲斗8进行倾卸。对应于右操作杆31R左右方向的操作来执行铲斗8的挖掘或释放动作。左操作杆31L的前后方向的操作对应于斗杆7的回旋。当左操作杆31L被向前方操作时,斗杆7进行倾卸,当左操作杆31L被向后方操作时,斗杆7进行挖掘。左操作杆31L的左右方向的操作对应于上部回旋体3的回旋。当左操作杆31L被向左侧操作时,上部回旋体3进行左回旋,当左操作杆31L被向右侧操作时,上部回旋体3进行右回旋。
在本实施方式中,动臂6的上升动作相当于倾卸动作。动臂6的下降动作相当于挖掘动作。斗杆7的挖掘动作相当于下降动作。斗杆7的倾卸动作相当于上升动作。铲斗8的挖掘动作相当于下降动作。铲斗8的倾卸动作相当于上升动作。需要说明的是,也可以将斗杆7的下降动作称为弯曲动作。也可以将斗杆7的上升动作称为伸长动作。
作业机操作构件31L、31R是液压挖掘机100的操作员用于操作作业机2的构件,例如是操纵杆那样的具备抓握部分和杆件的操作杆。这样的结构的作业机操作构件31L、31R可以由操作员握住抓握部而使其向前后左右倾倒。例如通过对设置在左方的作业机操作构件31L进行操作,能够使斗杆7及上部回旋体3动作,通过对设置在右方的作业机操作构件31R进行操作,能够使铲斗8及动臂6动作。
操作装置30根据对作业机操作构件31L、31R的输入即操作内容而产生先导压,并将产生的工作油的先导压向液压控制阀38所具备的作业用控制阀供给。此时,通过来自与各作业机的操作对应的操作装置的输入而产生先导压。作业机控制装置25检测产生的先导压,由此能够获知作业机操作构件31L、31R的输入的量即操作量。在本实施方式中,对应于动臂6被驱动时的作业机操作构件31R的操作而将基于检测到的先导压的操作量设为MB。同样,对应于斗杆7被驱动时的作业机操作构件31L的操作而将基于检测到的先导压的操作量设为MA,对应于铲斗8被驱动时的作业机操作构件31R的操作而将基于检测到的先导压的操作量设为MT。
行驶操作构件33L、33R是操作员用于操作液压挖掘机100的行驶的构件。行驶操作构件33L、33R例如是具备抓握部和杆件的操作杆(以下,适当称为行驶杆)。这样的行驶操作构件33L、33R可以由操作员握住抓握部而使其向前后倾倒。对于行驶操作构件33L、33R,若2个操作杆同时向前倾倒,则液压挖掘机100前进,若向后倾倒,则液压挖掘机100后退。
行驶操作构件33L、33R是操作员通过用脚踩踏而能够操作的未图示的踏板,例如是跷跷板式的踏板。通过踩踏踏板的前侧或后侧的任一方而与前述的操作杆同样地产生先导压,控制行驶用控制阀,液压马达5c进行驱动而能够使液压挖掘机100前进或后退。若将2个踏板同时且踩踏前侧,则液压挖掘机100前进,若踩踏后侧,则液压挖掘机100后退。若踩踏单方的踏板的前侧或后侧,则履带5a、5b的仅单侧旋转,能够使液压挖掘机100转弯。
如此,操作员在欲使液压挖掘机100行驶时,若执行用手使操作杆向前后倾倒、或用脚踩踏踏板的前侧或后侧中的任一方,则能够使行驶装置5的液压马达5c驱动。如图2所示,行驶操作构件33L、33R存在2组。通过对左侧的行驶操作构件33L进行操作,使左侧的液压马达5c驱动而能够使左侧的履带5b动作。通过对右侧的行驶操作构件33R进行操作,使右侧的液压马达5c驱动而能够使右侧的履带5a动作。
操作装置30根据对行驶操作构件33L、33R的输入即操作内容而产生先导压,并将产生的先导压向液压控制阀38所具备的行驶用控制阀供给。根据该先导压的大小而使行驶用控制阀动作,向行驶用的液压马达5c供给工作油。在行驶操作构件33L、33R为电气方式的杆时,使用例如电位计等来检测对行驶操作构件33L、33R的输入即操作内容,将输入转换成电信号(检测信号)而向作业机控制装置25输送。作业机控制装置25基于该检测信号来控制行驶用控制阀。
发动机控制装置26对发动机36进行控制。发动机36驱动液压泵37,向液压挖掘机100所具备的动臂油缸10、斗杆油缸11及铲斗油缸12等液压设备供给工作油。在发动机控制装置26上电连接有旋转速度检测传感器36R及燃料调整标度盘26D。发动机控制装置26基于旋转速度检测传感器36R检测到的发动机36的曲轴的旋转速度及燃料调整标度盘26D的设定等来控制向发动机36供给的燃料的量。如此,发动机控制装置26对发动机36进行控制。
泵控制装置27对液压挖掘机100所具备的液压泵37进行控制。液压泵37例如是通过变更斜盘的偏转角来变更工作油的喷出量等的斜盘式的液压泵。泵控制装置27例如经由车内信号线41从作业机控制装置25获取液压控制阀38的液压传感器38C检测到的先导压。泵控制装置27基于获取的先导压来控制液压泵37的斜盘的偏转角,由此控制从液压泵37喷出的工作油的流量。从液压泵37喷出的工作油经由液压控制阀38所具备的作业用控制阀或行驶用控制阀而向动臂油缸10、斗杆油缸11、铲斗油缸12及液压马达5c的至少1个供给,并对它们中的至少1个进行驱动。
第一显示装置28是显示图像的装置。第一显示装置28包括显示部28M和控制部28C。第一显示装置28在图1所示的液压挖掘机100的驾驶室4内,设置在驾驶席附近。在本实施方式中,第一显示装置28例如将液压挖掘机100的运转信息显示于显示部28M。运转信息例如是液压挖掘机100的累计运转时间、燃料的剩余量或发动机36的冷却水温度等。在液压挖掘机100具备周边监控用或后监控器用的相机等时,第一显示装置28可以显示该相机拍摄到的图像。
在本实施方式中,第一显示装置28除了将各种图像显示于显示部28M之外,还作为输入装置发挥功能。为此,第一显示装置28在显示部28M的下方具备输入装置28I。在本实施方式中,输入装置28I将多个按钮式的开关相对于显示部28M的横向平行地排列。通过操作输入装置28I,能够切换显示于显示部28M的图像、或执行与液压挖掘机100的动作相关的各种设定。需要说明的是,可以通过将输入装置28I装入到显示部28M的触摸面板来构成第一显示装置28。而且,输入装置28I可以与第一显示装置28分体地设于驾驶席附近的副仪表板。
第二显示装置39是显示图像的装置。第二显示装置39包括显示部39M和控制部39C。第二显示装置39设置在图1所示的液压挖掘机100的驾驶室4内的驾驶席附近。在本实施方式中,第二显示装置39例如将液压挖掘机100具备的铲斗8的铲边8T的与施工现场的地形对应的位置信息作为图像而显示于显示部39M。此时,第二显示装置39可以将铲边8T欲挖掘的施工现场的地形相关的信息与铲边8T的位置信息一起显示。
在本实施方式中,第二显示装置39的显示部39M例如是液晶显示装置,但并不限定于此。控制部39C控制显示部39M的动作、或求出铲边8T的位置信息。而且,控制部39C将表示铲边8T的位置与施工现场的地形的相对位置关系的指引图像显示于显示部39M。为此,控制部39C存储关于施工现场的地形的全局坐标位置信息。
在本实施方式中,第二显示装置39在显示部39M的下方具备输入装置39I。在本实施方式中,例如,在显示部39M等设置触摸面板,使用该触摸面板作为输入装置39I来切换显示于显示部39M的指引图像、或变更指引的内容、或输入各种设定。输入装置39I将多个按钮式的开关相对于显示部39M的横向平行地排列。通过操作输入装置39I,可以切换显示于显示部39M的指引图像、或变更指引的内容。在本实施方式中,第二显示装置39的功能可以由第一显示装置28实现。
IMU29检测液压挖掘机100的角速度及加速度。伴随着液压挖掘机100的动作,生成在行驶时产生的加速度、在转弯时产生的角加速度及重力加速度这样的各种加速度,但是IMU29检测至少包括重力加速度的加速度,并不区分各加速度的种类地将检测到的加速度输出。IMU29为了以更高的精度检测加速度,例如优选设置在液压挖掘机100的上部回旋体3的回旋中心轴上,但也可以如前述那样将IMU29设置在驾驶室4的下部,IMU29的详细情况在后文叙述。这种情况下,将从上部回旋体3的回旋中心轴的位置到IMU29的设置位置的距离作为回旋半径,求出根据离心力而求得的加速度(以下,适当称为离心加速度)和角加速度,只要从IMU29输出的加速度减去离心加速度及角加速度的分量,由此来修正与IMU29的设置位置相伴的加速度的影响即可。关于离心加速度及角加速度的分量的详细情况在后文叙述。
IMU29在图1A及图1B所示的局部坐标系(x,y,z)中检测x轴方向、y轴方向及z轴方向的加速度以及绕x轴、y轴及z轴的角速度(旋转角速度)。在图1所示的例子中,x轴是与液压挖掘机100的前后方向平行的轴,y轴是与液压挖掘机100的宽度方向平行的轴,z轴是与x轴及y轴这双方正交的轴。接着,说明作业机控制装置25执行的挖掘控制的一例。
(挖掘控制的一例)
图3A是表示目标施工面的一例的示意图。图3B是表示作业机控制装置25及第二显示装置39的框图。图4是表示目标挖掘地形73I与铲斗8的铲边8T之间的关系的一例的图。图5是表示目标速度、垂直速度分量、水平速度分量之间的关系的示意图。图6是表示垂直速度分量和水平速度分量的计算方法的图。图7是表示垂直速度分量和水平速度分量的计算方法的图。图8是表示铲边与目标挖掘地形73I之间的距离的示意图。图9是表示限制速度信息的一例的坐标图。图10是表示动臂的限制速度的垂直速度分量的计算方法的示意图。图11是表示动臂的限制速度的垂直速度分量与动臂的限制速度之间的关系的示意图。图12是表示铲边的移动引起的动臂的限制速度的变化的一例的图。
如图3B所示,第二显示装置39生成目标挖掘地形数据U而向作业机控制装置25输出。挖掘控制例如在液压挖掘机100的操作员使用图2所示的输入装置39I来选择执行挖掘控制的情况下被执行。在执行挖掘控制时,作业机控制装置25使用动臂操作量MB、斗杆操作量MA及铲斗操作量MT、以及从第二显示装置39获取的目标挖掘地形数据U、及从传感器控制装置24获取的倾斜度角α1、α2、α3,生成挖掘控制所需的动臂介入指令CBI、并根据需要来生成斗杆指令信号及铲斗指令信号,驱动控制阀及介入阀而控制作业机2。
首先,对第二显示装置39进行说明。第二显示装置39包括目标施工信息储存部39A、铲斗铲边位置数据生成部39B、目标挖掘地形数据生成部39D。目标施工信息储存部39A、铲斗铲边位置数据生成部39B及目标挖掘地形数据生成部39D的功能由控制部39C实现。
目标施工信息储存部39A是第二显示装置39的存储部的一部分,储存作为表示作业区域中的目标形状的信息的目标施工信息T。目标施工信息T包括为了生成作为表示挖掘对象的目标形状的信息的目标挖掘地形数据U所需的坐标数据及角度数据。目标施工信息T包括多个目标施工面71的位置信息。
作业机控制装置25控制作业机2或者使显示部39M显示目标挖掘地形数据Ua所需的目标施工信息T,例如通过无线通信从管理中心的管理服务器向目标施工信息储存部39A下载。而且,可以是将保存有目标施工信息T的终端装置与第二显示装置39连接,将目标施工信息T向目标施工信息储存部39A下载,也可以是可移动的存储装置与第二显示装置39连接而将目标施工信息T向目标施工信息储存部39A转送。
铲斗铲边位置数据生成部39B基于从全局坐标运算部23获取的基准位置数据P及回旋体方位数据Q,生成通过上部回旋体3的回旋轴z的表示液压挖掘机100的回旋中心的位置的回旋中心位置数据。回旋中心位置数据与局部坐标系的基准位置PL的xy坐标一致。
铲斗铲边位置数据生成部39B基于回旋中心位置数据和从传感器控制装置24获取的作业机2的倾斜度角α1、α2、α3,生成表示铲斗8的铲边8T的当前位置的铲斗铲边位置数据S。
如前述那样,铲斗铲边位置数据生成部39B例如以10Hz的频率从全局坐标运算部23获取基准位置数据P和回旋体方位数据Q。因此,铲斗铲边位置数据生成部39B能够以例如10Hz的频率对铲斗铲边位置数据S进行更新。铲斗铲边位置数据生成部39B将更新后的铲斗铲边位置数据S向目标挖掘地形数据生成部39D输出。
目标挖掘地形数据生成部39D获取储存于目标施工信息储存部39A的目标施工信息T和来自铲斗铲边位置数据生成部39B的铲斗铲边位置数据S。目标挖掘地形数据生成部39D在局部坐标系中将通过铲边8T的当前时刻的铲边位置P4的垂线与目标施工面71的交点设定为挖掘对象位置74。挖掘对象位置74是铲斗8的铲边位置P4的正下方的点。目标挖掘地形数据生成部39D基于目标施工信息T和铲斗铲边位置数据S,如图3A所示,获取被规定在上部回旋体3的前后方向上且通过挖掘对象位置74的作业机2的平面72与由多个目标施工面71表示的目标施工信息T的交线73作为目标挖掘地形73I的候补线。挖掘对象位置74是候补线上的一点。平面72是作业机2动作的平面(动作平面)。
在动臂6及斗杆7不绕着与液压挖掘机100的局部坐标系的z轴平行的轴转动时,作业机2的动作平面是与液压挖掘机100的xz平面平行的平面。在动臂6及斗杆7的至少一方绕着与液压挖掘机100的局部坐标系的z轴平行的轴转动时,作业机2的动作平面是与斗杆转动的轴即图1所示的斗杆销14的轴线正交的平面。以下,将作业机2的动作平面称为斗杆动作平面。
目标挖掘地形数据生成部39D将目标施工信息T的挖掘对象位置74的前后的一个或多个拐点及其前后的线确定为挖掘对象的目标挖掘地形73I。在图3A所示的例子中,将2个拐点Pv1、Pv2及其前后的线确定为目标挖掘地形73I。并且,目标挖掘地形数据生成部39D将挖掘对象位置74的前后的一个或多个拐点的位置信息及其前后的线的角度信息生成为表示挖掘对象的目标形状的信息即目标挖掘地形数据U。在本实施方式中,目标挖掘地形73I由线来规定,但也可以例如基于铲斗8的宽度等而规定为面。如此生成的目标挖掘地形数据U具有多个目标施工面71的一部分的信息。目标挖掘地形数据生成部39D将生成的目标挖掘地形数据U向作业机控制装置25输出。在本实施方式中,第二显示装置39和作业机控制装置直接进行信号的交接,但也可以例如经由CAN(Controller Area Network)那样的车内信号线来交接信号。
在本实施方式中,目标挖掘地形数据U是作业机2动作的作为动作平面的平面72与表示目标形状的至少1个目标施工面(第一目标施工面)71交叉的部分的信息。平面72是图1B所示的局部坐标系(x,y,z)中的xz平面。通过利用平面72切出多个目标施工面71而得到的目标挖掘地形数据U适当称为前后方向目标挖掘地形数据U。
第二显示装置39根据需要基于作为第一目标挖掘地形信息的前后方向目标挖掘地形数据U而在显示部39M上显示目标挖掘地形73I。作为显示用的信息,使用显示用的目标挖掘地形数据Ua。基于显示用的目标挖掘地形数据Ua,例如,将图2所示那样的表示作为铲斗8的挖掘对象而设定的目标挖掘地形73I与铲边8T之间的位置关系的图像显示在显示部39M上。第二显示装置39基于显示用的目标挖掘地形数据Ua而在显示部39M上显示目标挖掘地形(显示用的目标挖掘地形)73I。向作业机控制装置25输出的前后方向目标挖掘地形数据U使用于挖掘控制。将使用于挖掘控制的目标挖掘地形数据U适当称为作业用目标挖掘地形数据。
如前述那样,目标挖掘地形数据生成部39D以例如10Hz的频率从铲斗铲边位置数据生成部39B获取铲斗铲边位置数据S。因此,目标挖掘地形数据生成部39D能够以例如10Hz的频率对前后方向目标挖掘地形数据U进行更新,并向作业机控制装置25输出。接着,对作业机控制装置25进行说明。
作业机控制装置25具有目标速度确定部90、距离获取部91、限制速度确定部92、作业机控制部93。作业机控制装置25使用基于前述的前后方向目标挖掘地形数据U的目标挖掘地形73I来执行挖掘控制。如此,在本实施方式中,存在使用于显示的目标挖掘地形73I和使用于挖掘控制的目标挖掘地形73I。将前者称为显示用目标挖掘地形,将后者称为挖掘控制用目标挖掘地形。
在本实施方式中,目标速度确定部90、距离获取部91、限制速度确定部92及作业机控制部93的功能由图2所示的作业机用处理部25P来实现。接着,说明作业机控制装置25进行的挖掘控制。
目标速度确定部90确定动臂目标速度Vc_bm、斗杆目标速度Vc_am、铲斗目标速度Vc_bkt。动臂目标速度Vc_bm是仅动臂油缸10被驱动时的铲边8T的速度。斗杆目标速度Vc_am是仅斗杆油缸11被驱动时的铲边8T的速度。铲斗目标速度Vc_bkt是仅铲斗油缸12被驱动时的铲边8T的速度。动臂目标速度Vc_bm根据动臂操作量MB来计算。斗杆目标速度Vc_am根据斗杆操作量MA来计算。铲斗目标速度Vc_bkt根据铲斗操作量MT来计算。
作业机用存储部25M存储有对动臂操作量MB与动臂目标速度Vc_bm的关系进行规定的目标速度信息。目标速度确定部90通过参照目标速度信息来确定与动臂操作量MB对应的动臂目标速度Vc_bm。目标速度信息例如是记载有动臂目标速度Vc_bm相对于动臂操作量MB的大小的映射。目标速度信息可以是表或数学式等方式。目标速度信息包括对斗杆操作量MA与斗杆目标速度Vc_am之间的关系进行规定的信息。目标速度信息包括对铲斗操作量MT与铲斗目标速度Vc_bkt之间的关系进行规定的信息。目标速度确定部90通过参照目标速度信息来确定与斗杆操作量MA对应的斗杆目标速度Vc_am。目标速度确定部90通过参照目标速度信息来确定与铲斗操作量MT对应的铲斗目标速度Vc_bkt。如图7所示,目标速度确定部90将动臂目标速度Vc_bm转换成与目标挖掘地形73I(目标挖掘地形数据U)垂直的方向的速度分量(以下,适当称为垂直速度分量)Vcy_bm及与目标挖掘地形73I(目标挖掘地形数据U)平行的方向的速度分量(以下,适当称为水平速度分量)Vcx_bm。
例如,首先,目标速度确定部90获取IMU29检测到的倾斜度角θ5,求出与目标挖掘地形73I正交的方向的相对于全局坐标系的垂直轴的倾斜度。而后,目标速度确定部90根据上述的倾斜度来求出表示局部坐标系的垂直轴与正交于目标挖掘地形73I的方向的倾斜度的角度β2(参照图6)。
接着,如图6所示,目标速度确定部90根据局部坐标系的垂直轴与动臂目标速度Vc_bm的方向所成的角度β2,通过三角函数将动臂目标速度Vc_bm转换成局部坐标系的垂直轴方向的速度分量VL1_bm和水平轴方向的速度分量VL2_bm。并且,如图7所示,目标速度确定部90根据前述的局部坐标系的垂直轴与正交于目标挖掘地形73I的方向的倾斜度β1,通过三角函数将局部坐标系的垂直轴方向的速度分量VL1_bm和水平轴方向的速度分量VL2_bm转换成前述的相对于目标挖掘地形73I的垂直速度分量Vcy_bm及水平速度分量Vcx_bm。同样,目标速度确定部90将斗杆目标速度Vc_am转换成局部坐标系的垂直轴方向的垂直速度分量Vcy_am及水平速度分量Vcx_am。目标速度确定部90将铲斗目标速度Vc_bkt转换成局部坐标系的垂直轴方向的垂直速度分量Vcy_bkt及水平速度分量Vcx_bkt。
如图8所示,距离获取部91获取铲斗8的铲边8T与目标挖掘地形73I之间的距离d。详细而言,距离获取部91根据如前述那样获取的铲边8T的位置信息及表示目标挖掘地形73I的位置的目标挖掘地形数据U等,计算铲斗8的铲边8T与目标挖掘地形73I之间的最短的距离d。在本实施方式中,基于铲斗8的铲边8T与目标挖掘地形73I之间的最短的距离d来执行挖掘控制。
限制速度确定部92基于铲斗8的铲边8T与目标挖掘地形73I之间的距离d,来计算图1所示的作业机2整体的限制速度Vcy_lmt。作业机2整体的限制速度Vcy_lmt是在铲斗8的铲边8T接近目标挖掘地形73I的方向上能够允许的铲边8T的移动速度。图2所示的作业机用存储部25M存储有对距离d与限制速度Vcy_lmt之间的关系进行规定的限制速度信息。
图9示出限制速度信息的一例。图9中的横轴是距离d,纵轴是限制速度Vcy。在本实施方式中,铲边8T位于目标挖掘地形73I的外方即位于液压挖掘机100的作业机2侧时的距离d为正值,铲边8T位于目标挖掘地形73I的内方即位于比目标挖掘地形73I靠挖掘对象的内部侧的位置时的距离d为负值。这也可以是例如图8所图示那样,铲边8T位于目标挖掘地形73I的上方时的距离d为正值,铲边8T位于目标挖掘地形73I的下方时的距离d为负值。而且,铲边8T相对于目标挖掘地形73I而处于未侵蚀的位置时的距离d为正值,铲边8T相对于目标挖掘地形73I而处于侵蚀的位置时的距离d为负值。在铲边8T位于目标挖掘地形73I上时,即铲边8T与目标挖掘地形73I相接时的距离d为0。
在本实施方式中,铲边8T从目标挖掘地形73I的内方朝向外方时的速度为正值,铲边8T从目标挖掘地形73I的外方朝向内方时的速度为负值。即,铲边8T朝向目标挖掘地形73I的上方时的速度为正值,铲边8T朝向下方时的速度为负值。
在限制速度信息中,距离d为d1与d2之间时的限制速度Vcy_lmt的倾斜度小于距离d为d1以上或d2以下时的倾斜度。d1大于0。d2小于0。在目标挖掘地形73I附近的操作中,为了更详细地设定限制速度,使距离d为d1与d2之间时的倾斜度小于距离d为d1以上或d2以下时的倾斜度。在距离d为d1以上时,限制速度Vcy_lmt为负值,距离d越大而限制速度Vcy_lmt越小。即,在距离d为d1以上时,在比目标挖掘地形73I靠上方的位置,铲边8T越远离目标挖掘地形73I,朝向目标挖掘地形73I的下方的速度越大,限制速度Vcy_lmt的绝对值越大。在距离d为0以下时,限制速度Vcy_lmt为正值,距离d越小而限制速度Vcy_lmt越大。即,铲斗8的铲边8T远离目标挖掘地形73I的距离d为0以下时,在比目标挖掘地形73I靠下方的位置,铲边8T越远离目标挖掘地形73I,朝向目标挖掘地形73I的上方的速度越大,限制速度Vcy_lmt的绝对值越大。
距离d为第一规定值dth1以上的话,限制速度Vcy_lmt成为Vmin。第一规定值dth1为正值且大于d1。Vmin小于目标速度的最小值。即,距离d为第一规定值dth1以上的话,不进行作业机2的动作的限制。因此,铲边8T在目标挖掘地形73I的上方较大地远离目标挖掘地形73I时,不进行作业机2的动作的限制即挖掘控制。在距离d小于第一规定值dth1时,进行作业机2的动作的限制。详细而言,如后述那样,在距离d小于第一规定值dth1时,进行动臂6的动作的限制。
限制速度确定部92根据作业机2整体的限制速度Vcy_lmt、斗杆目标速度Vc_am、铲斗目标速度Vc_bkt来计算动臂6的限制速度的垂直速度分量(以下,适当称为动臂6的限制垂直速度分量)Vcy_bm_lmt。如图10所示,限制速度确定部92从作业机2整体的限制速度Vcy_lmt减去斗杆目标速度的垂直速度分量Vcy_am、铲斗目标速度的垂直速度分量Vcy_bkt,由此计算动臂6的限制垂直速度分量Vcy_bm_lmt。
如图11所示,限制速度确定部92将动臂6的限制垂直速度分量Vcy_bm_lmt转换成动臂6的限制速度(动臂限制速度)Vc_bm_lmt。限制速度确定部92根据前述的动臂6的倾斜度角α1、斗杆7的倾斜度角α2、铲斗8的倾斜度角α3、GNSS天线20、21的基准位置数据及目标挖掘地形数据U等,求出动臂限制速度Vc_bm_lmt的方向与垂直于目标挖掘地形73I的方向之间的关系,并将动臂6的限制垂直速度分量Vcy_bm_lmt转换成动臂限制速度Vc_bm_lmt。这种情况下的运算通过与前述的根据动臂目标速度Vc_bm来求出垂直于目标挖掘地形73I的方向的垂直速度分量Vcy_bm的运算相反的次序进行。
后述的梭式阀151选择基于动臂6的操作而生成的先导压力与基于动臂介入指令CBI而后述的介入阀127C生成的先导压力中的较大的一方,并将其向后述的方向控制阀164供给。在基于动臂介入指令CBI的先导压力比基于动臂6的操作而生成的先导压力大时,在基于动臂介入指令CBI的先导压力下,与动臂油缸10对应的后述的方向控制阀164动作。其结果是,能实现基于动臂限制速度Vc_bm_lmt的动臂6的驱动。
作业机控制部93对作业机2进行控制。作业机控制部93将斗杆指令信号、动臂指令信号、动臂介入指令CBI、铲斗指令信号向后述的控制阀127输出,由此控制动臂油缸10、斗杆油缸11、铲斗油缸12。斗杆指令信号、动臂指令信号、动臂介入指令CBI、铲斗指令信号分别具有与动臂指令速度、斗杆指令速度、铲斗指令速度对应的电流值。
在基于动臂6的上升操作而生成的先导压力比基于动臂介入指令CBI的先导压力大时,后述的梭式阀151选择基于杆操作的先导压。在基于动臂6的操作而通过梭式阀151选择的先导压力下,与动臂油缸10对应的方向控制阀164动作。即,动臂6基于动臂目标速度Vc_bm而被驱动,因此不会基于动臂限制速度Vc_bm_lmt而被驱动。
在基于动臂6的操作而生成的先导压力比基于动臂介入指令CBI的先导压力大时,作业机控制部93将动臂目标速度Vc_bm、斗杆目标速度Vc_am及铲斗目标速度Vc_bkt分别选择为动臂指令速度、斗杆指令速度及铲斗指令速度。作业机控制部93根据动臂目标速度Vc_bm、斗杆目标速度Vc_am及铲斗目标速度Vc_bkt来确定动臂油缸10、斗杆油缸11及铲斗油缸12的速度(油缸速度)。并且,作业机控制部93基于确定的油缸速度来控制图2所示的液压控制阀38,由此使动臂油缸10、斗杆油缸11及铲斗油缸12动作。
如此,在通常运转时,作业机控制部93根据动臂操作量MB、斗杆操作量MA、铲斗操作量MT而使动臂油缸10、斗杆油缸11、铲斗油缸12动作。因此,动臂油缸10以动臂目标速度Vc_bm动作,斗杆油缸11以斗杆目标速度Vc_am动作,铲斗油缸12以铲斗目标速度Vc_bkt动作。
在基于动臂介入指令CBI的先导压力比基于动臂6的操作而生成的先导压力大时,梭式阀151选择基于介入的指令的从介入阀127C输出的先导压。其结果是,动臂6以动臂限制速度Vc_bm_lmt动作,并且斗杆7以斗杆目标速度Vc_am动作。而且,铲斗8以铲斗目标速度Vc_bkt动作。
如前述那样,从作业机2整体的限制速度Vcy_lmt减去斗杆目标速度的垂直速度分量Vcy_am和铲斗目标速度的垂直速度分量Vcy_bkt,由此计算动臂6的限制垂直速度分量Vcy_bm_lmt。因此,在作业机2整体的限制速度Vcy_lmt小于斗杆目标速度的垂直速度分量Vcy_am与铲斗目标速度的垂直速度分量Vcy_bkt之和时,动臂6的限制垂直速度分量Vcy_bm_lmt成为动臂上升的负值。
因此,动臂限制速度Vc_bm_lmt成为负值。这种情况下,作业机控制部93使动臂6下降,但是比动臂目标速度Vc_bm减速。因此,能够将操作员的不适感抑制得较小并抑制铲斗8侵蚀目标挖掘地形73I的情况。
在作业机2整体的限制速度Vcy_lmt大于斗杆目标速度的垂直速度分量Vcy_am与铲斗目标速度的垂直速度分量Vcy_bkt之和时,动臂6的限制垂直速度分量Vcy_bm_lmt成为正值。因此,动臂限制速度Vc_bm_lmt成为正值。这种情况下,即使将操作装置30向使动臂6下降的方向操作,基于来自介入阀127C的指令信号而动臂6也会上升。因此,能够迅速地抑制目标挖掘地形73I的侵蚀的扩大。
在铲边8T位于比目标挖掘地形73I靠上方的位置时,铲边8T越接近目标挖掘地形73I,动臂6的限制垂直速度分量Vcy_bm_lmt的绝对值越减小,并且向与目标挖掘地形73I平行的方向的动臂6的限制速度的速度分量(以下,适当称为限制水平速度分量)Vcx_bm_lmt的绝对值也减小。因此,在铲边8T位于比目标挖掘地形73I靠上方的位置时,铲边8T越接近目标挖掘地形73I,动臂6的朝向与目标挖掘地形73I垂直的方向的速度和动臂6的朝向与目标挖掘地形73I平行的方向的速度都越减速。通过液压挖掘机100的操作员来同时操作左侧的作业机操作构件25L及右侧的作业机操作构件25R,由此动臂6、斗杆7、铲斗8同时动作。此时,动臂6、斗杆7、铲斗8的各目标速度Vc_bm、Vc_am、Vc_bkt被输入,当说明前述的控制时如下述那样。
图12示出目标挖掘地形73I与铲斗8的铲边8T之间的距离d小于第一规定值dth1、且铲斗8的铲边从位置Pn1向位置Pn2移动时的动臂6的限制速度的变化的一例。位置Pn2处的铲边8T与目标挖掘地形73I之间的距离小于位置Pn1处的铲边8T与目标挖掘地形73I之间的距离。因此,位置Pn2处的动臂6的限制垂直速度分量Vcy_bm_lmt2小于位置Pn1处的动臂6的限制垂直速度分量Vcy_bm_lmt1。因此,位置Pn2处的动臂限制速度Vc_bm_lmt2小于位置Pn1处的动臂限制速度Vc_bm_lmt1。而且,位置Pn2处的动臂6的限制水平速度分量Vcx_bm_lmt2小于位置Pn1处的动臂6的限制水平速度分量Vcx_bm_lmt1。但是,此时,对于斗杆目标速度Vc_am及铲斗目标速度Vc_bkt不进行限制。因此,对于斗杆目标速度的垂直速度分量Vcy_am及水平速度分量Vcx_am和铲斗目标速度的垂直速度分量Vcy_bkt及水平速度分量Vcx_bkt不进行限制。
如前述那样,对斗杆7不进行限制,由此与操作员的挖掘意图对应的斗杆操作量的变化反映作为铲斗8的铲边8T的速度变化。因此,本实施方式能够抑制目标挖掘地形73I的侵蚀的扩大并抑制操作员的挖掘时的操作的不适感。
铲边8T的铲边位置P4并不局限于通过GNSS进行测位,也可以通过其他的测位机构进行测位。因此,铲边8T与目标挖掘地形73I的距离d并不局限于通过GNSS进行测位,也可以通过其他的测位机构进行测位。铲斗限制速度的绝对值小于铲斗目标速度的绝对值。铲斗限制速度可以通过例如与前述的斗杆限制速度同样的手法来计算。需要说明的是,也可以与斗杆7的限制一起地进行铲斗8的限制。接着,说明液压挖掘机100具备的液压***的详细情况及挖掘控制时的液压***的动作。
图13是表示本实施方式的控制***200及液压***300的一例的示意图。图14是将图13的一部分放大的图。
如图13及图14所示,液压***300具备包括动臂油缸10、斗杆油缸11及铲斗油缸12在内的液压油缸160、和使上部回旋体3回旋的回旋马达163。液压油缸160通过从图2所示的液压泵37供给的工作油而工作。回旋马达163是液压马达,通过从液压泵37供给的工作油而工作。图2所示的液压控制阀38包括方向控制阀164及控制阀127,液压传感器38C包括压力传感器166及压力传感器167。
在本实施方式中,设有对工作油流动的方向进行控制的方向控制阀164。方向控制阀164在多个液压油缸160(动臂油缸10、斗杆油缸11及铲斗油缸12)上分别配置。方向控制阀164是使杆状的滑柱动作来切换工作油流动的方向的滑柱方式。方向控制阀164具有能够移动的杆状的滑柱。滑柱通过供给的先导油而移动。方向控制阀164通过滑柱的移动而向液压油缸160供给工作油来使液压油缸160动作。从液压泵37供给的工作油经由方向控制阀164而向液压油缸160供给。通过滑柱沿着轴向的移动来切换对盖侧油室的工作油的供给和对杆侧油室的工作油的供给。而且,通过滑柱沿轴向的移动来调整对液压油缸160的工作油的供给量(每单位时间内的供给量)。通过调整对液压油缸160的工作油的供给量,来调整液压油缸160的油缸速度。
方向控制阀164的驱动由操作装置30调整。从图2所示的液压泵37送出且由减压阀减压后的工作油作为先导油向操作装置30供给。需要说明的是,也可以将从与液压泵37不同的先导液压泵送出的先导油向操作装置30供给。如图2所示,操作装置30包括能够调整先导液压的压力调整阀250。基于操作装置30的操作量来调整先导液压。通过该先导液压来驱动方向控制阀164。通过利用操作装置30来调整先导液压,由此能调整轴向上的滑柱的移动量及移动速度。
方向控制阀164在动臂油缸10、斗杆油缸11、铲斗油缸12及回旋马达163上分别设置。在以下的说明中,将与动臂油缸10连接的方向控制阀164适当称为方向控制阀640。将与斗杆油缸11连接的方向控制阀164适当称为方向控制阀641。将与铲斗油缸12连接的方向控制阀164适当称为方向控制阀642。
操作装置30与方向控制阀164经由先导油路450而连接。用于使方向控制阀164的滑柱移动的先导油在先导油路450中流动。在本实施方式中,在先导油路450配置有控制阀127、压力传感器166及压力传感器167。
在以下的说明中,将先导油路450中的、操作装置30与控制阀127之间的先导油路450适当称为先导油路451,将控制阀127与方向控制阀164之间的先导油路450适当称为先导油路452。
在方向控制阀164上连接有先导油路452。经由先导油路452将先导油向方向控制阀164供给。方向控制阀164具有第一受压室及第二受压室。先导油路452包括与第一受压室连接的先导油路452A和与第二受压室连接的先导油路452B。
当经由先导油路452A向方向控制阀164的第一受压室供给先导油时,对应于该先导液压而使滑柱移动,经由方向控制阀164向液压油缸160的杆侧油室供给工作油。对杆侧液压室的工作油的供给量通过操作装置30的操作量(滑柱的移动量)来调整。
当经由先导油路452B向方向控制阀164的第二受压室供给先导油时,对应于该先导液压而滑柱移动,经由方向控制阀164向液压油缸160的盖侧油室供给工作油。工作油相对于盖侧液压室的供给量通过操作装置30的操作量(滑柱的移动量)来调整。
即,通过操作装置30调整了先导液压后的先导油向方向控制阀164供给,由此滑柱在轴向上向一侧移动。通过操作装置30调整了先导液压后的先导油向方向控制阀164供给,由此滑柱在轴向上向另一侧移动。其结果是,能调整轴向上的滑柱的位置。
先导油路451包括将先导油路452A与操作装置30连接的先导油路451A、将先导油路452B与操作装置30连接的先导油路451B。
在以下的说明中,将与对动臂油缸10进行工作油的供给的方向控制阀640连接的先导油路452A适当称为动臂调整用油路4520A,将与方向控制阀640连接的先导油路452B适当称为动臂调整用油路4520B。
在以下的说明中,将与对斗杆油缸11进行工作油的供给的方向控制阀641连接的先导油路452A适当称为斗杆调整用油路4521A,将与方向控制阀641连接的先导油路452B适当称为斗杆调整用油路4521B。
在以下的说明中,将与对铲斗油缸12进行工作油的供给的方向控制阀642连接的先导油路452A适当称为铲斗调整用油路4522A,将与方向控制阀642连接的先导油路452B适当称为铲斗调整用油路4522B。
在以下的说明中,将与动臂调整用油路4520A连接的先导油路451A适当称为动臂操作用油路4510A,将与动臂调整用油路4520B连接的先导油路451B适当称为动臂操作用油路4510B。
在以下的说明中,将与斗杆调整用油路4521A连接的先导油路451A适当称为斗杆操作用油路4511A,将与斗杆调整用油路4521B连接的先导油路451B适当称为斗杆操作用油路4511B。
在以下的说明中,将与铲斗调整用油路4522A连接的先导油路451A适当称为铲斗操作用油路4512A,将与铲斗调整用油路4522B连接的先导油路451B适当称为铲斗操作用油路4512B。
动臂操作用油路(4510A、4510B)及动臂调整用油路(4520A、4520B)与先导液压方式的操作装置30连接。对应于操作装置30的操作量而调整了压力后的先导油流向动臂操作用油路(4510A、4510B)。
斗杆操作用油路(4511A、4511B)及斗杆调整用油路(4521A、4521B)与先导液压方式的操作装置30连接。对应于操作装置30的操作量而调整了压力后的先导油流向斗杆操作用油路(4511A、4511B)。
铲斗操作用油路(4512A、4512B)及铲斗调整用油路(4522A、4522B)与先导液压方式的操作装置30连接。对应于操作装置30的操作量而调整了压力后的先导油流向铲斗操作用油路(4512A、4512B)。
动臂操作用油路4510A、动臂操作用油路4510B、动臂调整用油路4520A及动臂调整用油路4520B是用于使动臂6动作的先导油所流动的动臂用油路。
斗杆操作用油路4511A、斗杆操作用油路4511B、斗杆调整用油路4521A及斗杆调整用油路4521B是用于使斗杆7动作的先导油所流动的斗杆用油路。
铲斗操作用油路4512A、铲斗操作用油路4512B、铲斗调整用油路4522A及铲斗调整用油路4522B是用于使铲斗8动作的先导油所流动的铲斗用油路。
如前述那样,通过操作装置30的操作,动臂6执行下降动作及上升动作这2种动作。以执行动臂6的下降动作的方式对操作装置30进行操作,由此经由动臂操作用油路4510A及动臂调整用油路4520A向与动臂油缸10连接的方向控制阀640供给先导油。方向控制阀640基于先导液压而工作。由此,将来自液压泵37的工作油向动臂油缸10供给,执行动臂6的下降动作。
以执行动臂6的上升动作的方式对操作装置30进行操作,由此经由动臂操作用油路4510B及动臂调整用油路4520B将先导油向与动臂油缸10连接的方向控制阀640供给。方向控制阀640基于先导液压而工作。其结果是,来自液压泵37的工作油向动臂油缸10供给,执行动臂6的上升动作。
即,在本实施方式中,动臂操作用油路4510A及动臂调整用油路4520A是与方向控制阀640的第一受压室连接且用于使动臂6进行下降动作的先导油所流动的动臂下降用油路。动臂操作用油路4510B及动臂调整用油路4520B是与方向控制阀640的第二受压室连接且用于使动臂6进行上升动作的先导油所流动的动臂上升用油路。
另外,通过操作装置30的操作,斗杆7执行下降动作及上升动作这2种动作。以执行斗杆7的上升动作的方式对操作装置30进行操作,由此经由斗杆操作用油路4511A及斗杆调整用油路4521A将先导油向与斗杆油缸11连接的方向控制阀641供给。方向控制阀641基于先导液压而工作。其结果是,来自液压泵37的工作油向斗杆油缸11供给,执行斗杆7的上升动作。
以执行斗杆7的下降动作的方式对操作装置30进行操作,由此经由斗杆操作用油路4511B及斗杆调整用油路4521B将先导油向与斗杆油缸11连接的方向控制阀641供给。方向控制阀641基于先导液压而工作。其结果是,来自液压泵37的工作油向斗杆油缸11供给,执行斗杆7的下降动作。
即,在本实施方式中,斗杆操作用油路4511A及斗杆调整用油路4521A是与方向控制阀641的第一受压室连接且用于使斗杆7进行上升动作的先导油所流动的斗杆上升用油路。斗杆操作用油路4511B及斗杆调整用油路4521B是与方向控制阀641的第二受压室连接且用于使斗杆7进行下降动作的先导油所流动的斗杆下降用油路。
通过操作装置30的操作,铲斗8执行下降动作及上升动作这2种动作。以执行铲斗8的上升动作的方式对操作装置30进行操作,由此经由铲斗操作用油路4512A及铲斗调整用油路4522A将先导油向与铲斗油缸12连接的方向控制阀642供给。方向控制阀642基于先导液压而工作。其结果是,来自液压泵37的工作油向铲斗油缸12供给,执行铲斗8的上升动作。
以执行铲斗8的下降动作的方式对操作装置30进行操作,由此经由铲斗操作用油路4512B及铲斗调整用油路4522B将先导油向与铲斗油缸12连接的方向控制阀642供给。方向控制阀642基于先导液压而工作。其结果是,来自液压泵37的工作油向铲斗油缸12供给,执行铲斗8的下降动作。
即,在本实施方式中,铲斗操作用油路4512A及铲斗调整用油路4522A是与方向控制阀642的第一受压室连接且用于使铲斗8进行上升动作的先导油所流动的铲斗上升用油路。铲斗操作用油路4512B及铲斗调整用油路4522B是与方向控制阀642的第二受压室连接且用于使铲斗8进行下降动作的先导油所流动的铲斗下降用油路。
另外,通过操作装置30的操作,上部回旋体3执行右回旋动作及左回旋动作这2种动作。以执行上部回旋体3的右回旋动作的方式对操作装置30进行操作,由此将工作油向回旋马达163供给。以执行上部回旋体3的左回旋动作的方式对操作装置30进行操作,由此操作方向控制阀164,将工作油向回旋马达163供给。
控制阀127基于来自作业机控制装置25的控制信号(电流)来调整先导液压。控制阀127例如是电磁比例控制阀,基于来自作业机控制装置25的控制信号而被控制。控制阀127包括控制阀127A和控制阀127B。控制阀127A调整向方向控制阀164的第一受压室供给的先导油的先导液压,从而调整经由方向控制阀164向杆侧油室供给的工作油的供给量。控制阀127B调整向方向控制阀164的第二受压室供给的先导油的先导液压,从而调整经由方向控制阀164向盖侧油室供给的工作油的供给量。
在以下的说明中,将控制阀127A适当称为减压阀127A,将控制阀127B适当称为减压阀127B。在控制阀127的两侧设有检测先导液压的压力传感器166及压力传感器167。在本实施方式中,压力传感器166在先导油路451上配置于操作装置30与控制阀127之间。压力传感器167在先导油路452上配置于控制阀127与方向控制阀164之间。压力传感器166能够检测由控制阀127调整之前的先导液压。压力传感器167能够检测由控制阀127调整后的先导液压。压力传感器166能够检测通过操作装置30的操作而调整的先导液压。压力传感器166及压力传感器167的检测结果向作业机控制装置25输出。
在以下的说明中,将能够调整对方向控制阀640的先导液压的控制阀127适当称为动臂用减压阀270,该方向控制阀640对动臂油缸10进行工作油的供给。而且,动臂用减压阀270中,将一方的动臂用减压阀(相当于减压阀127A)适当称为动臂用减压阀270A,将另一方的动臂用减压阀(相当于减压阀127B)适当称为动臂用减压阀270B。动臂用减压阀270(270A、270B)配置于动臂操作用油路。
在以下的说明中,将能够调整对方向控制阀641的先导液压的控制阀127适当称为斗杆用减压阀271,该方向控制阀641对斗杆油缸11进行工作油的供给。而且,斗杆用减压阀271中,将一方的斗杆用减压阀(相当于减压阀127A)适当称为斗杆用减压阀271A,将另一方的斗杆用减压阀(相当于减压阀127B)适当称为斗杆用减压阀271B。斗杆用减压阀271(271A、271B)配置于斗杆操作用油路。
在以下的说明中,将能够调整对方向控制阀642的先导液压的控制阀127适当称为铲斗用减压阀272,该方向控制阀642对铲斗油缸12进行工作油的供给。而且,铲斗用减压阀272中,将一方的铲斗用减压阀(相当于减压阀127A)适当称为铲斗用减压阀272A,将另一方的铲斗用减压阀(相当于减压阀127B)适当称为铲斗用减压阀272B。铲斗用减压阀272(272A、272B)配置于铲斗操作用油路。
在对动臂油缸10进行工作油的供给的方向控制阀640上连接有先导油路451A、451B、452A、452B。在以下的说明中,将配置于动臂操作用油路4510A的动臂用压力传感器166适当称为动臂用压力传感器660A,将配置于动臂操作用油路4510B的动臂用压力传感器166适当称为动臂用压力传感器660B。而且,将配置于动臂调整用油路4520A的动臂用压力传感器167适当称为动臂用压力传感器670A,将配置于动臂调整用油路4520B的动臂用压力传感器167适当称为动臂用压力传感器670B。
在以下的说明中,在对斗杆油缸11进行工作油的供给的方向控制阀641上连接有先导油路451A、451B、452A、452B。在以下的说明中,将配置于斗杆操作用油路4511A的斗杆用压力传感器166适当称为斗杆用压力传感器661A,将配置于斗杆操作用油路4511B的斗杆用压力传感器166适当称为斗杆用压力传感器661B。而且,将配置于斗杆调整用油路4521A的斗杆用压力传感器167适当称为斗杆用压力传感器671A,将配置于斗杆调整用油路4521B的斗杆用压力传感器167适当称为斗杆用压力传感器671B。
在以下的说明中,在对铲斗油缸12进行工作油的供给的方向控制阀642上连接有先导油路451A、451B、452A、452B。在以下的说明中,将配置于铲斗操作用油路4512A的铲斗用压力传感器166适当称为铲斗用压力传感器662A,将配置于铲斗操作用油路4512B的铲斗用压力传感器166适当称为铲斗用压力传感器662B。而且,将配置于铲斗调整用油路4522A的铲斗用压力传感器167适当称为铲斗用压力传感器672A,将配置于铲斗调整用油路4522B的铲斗用压力传感器167适当称为铲斗用压力传感器672B。
在不执行挖掘控制的情况下,作业机控制装置25对控制阀127进行控制,将图13所示的先导油路450打开(设为全开)。通过将先导油路450打开,先导油路451的先导液压与先导油路452的先导液压变得相等。在通过控制阀127将先导油路450打开的状态下,先导液压基于操作装置30的操作量而被调整。
在通过控制阀127使先导油路450全开时,作用于压力传感器166的先导液压与作用于压力传感器167的先导液压相等。由于控制阀127的开度减小而作用于压力传感器166的先导液压与作用于压力传感器167的先导液压不同。
如挖掘控制等那样作业机2由作业机控制装置25控制的情况下,作业机控制装置25向控制阀127输出控制信号。先导油路451例如通过先导溢流阀的作用而具有规定的压力(先导液压)。当从作业机控制装置25向控制阀127输出控制信号时,控制阀127基于该控制信号而工作。先导油路451的先导油经由控制阀127向先导油路452供给。先导油路452的先导液压由控制阀127调整(减压)。先导油路452的先导液压作用于方向控制阀164。由此,方向控制阀164基于由控制阀127控制后的先导液压而工作。在本实施方式中,压力传感器166检测由控制阀127调整之前的先导液压。压力传感器167检测由控制阀127调整之后的先导液压。
由减压阀127A调整了压力后的先导油向方向控制阀164供给,由此滑柱在轴向上向一侧移动。由减压阀127B调整了压力后的先导油向方向控制阀164供给,由此滑柱在轴向上向另一侧移动。其结果是,调整轴向上的滑柱的位置。
例如,作业机控制装置25向动臂用减压阀270A及动臂用减压阀270B中的至少一方输出控制信号,能够调整对与动臂油缸10连接的方向控制阀640的先导液压。
另外,作业机控制装置25向斗杆用减压阀271A及斗杆用减压阀271B中的至少一方输出控制信号,能够调整对与斗杆油缸11连接的方向控制阀641的先导液压。
另外,作业机控制装置25向铲斗用减压阀272A及铲斗用减压阀272B中的至少一方输出控制信号,能够调整对与铲斗油缸12连接的方向控制阀642的先导液压。
作业机控制装置25在挖掘控制中,如前述那样,基于表示挖掘对象的目标形状即设计地形的目标挖掘地形73I(目标挖掘地形数据U)和表示铲斗8的位置的铲斗铲边位置数据S,根据目标挖掘地形73I与铲斗8的距离d,以使铲斗8接近目标挖掘地形73I的速度减小的方式限制动臂6的速度。
在本实施方式中,作业机控制装置25具有动臂限制部,该动臂限制部输出用于对动臂6的速度进行限制的控制信号。在本实施方式中,在基于操作装置30的操作而作业机2进行驱动的情况下,以避免铲斗8的铲边8T侵入目标挖掘地形73I的方式,基于从作业机控制装置25的动臂限制部输出的控制信号来控制动臂6的动作(动臂介入控制)。具体而言,在挖掘控制中,以避免铲边8T侵入目标挖掘地形73I的方式,动臂6通过作业机控制装置25来执行上升动作。
在本实施方式中,为了实现动臂介入控制,基于从作业机控制装置25输出的与动臂介入控制相关的控制信号而工作的控制阀127C设于先导油路150。在动臂介入控制中,调整了压力(先导液压)后的先导油流向先导油路150。控制阀127C配置于先导油路150,能够调整先导油路150的先导液压。
在以下的说明中,将在动臂介入控制中调整了压力后的先导油所流动的先导油路150适当称为介入用油路501、502,将与介入用油路501连接的控制阀127C适当称为介入阀127C。
向与动臂油缸10连接的方向控制阀640供给的先导油流向介入用油路502。介入用油路502经由梭式阀151而连接于与方向控制阀640连接的动臂操作用油路4510B及动臂调整用油路4520B。
梭式阀151具有2个入口和1个出口。一方的入口与介入用油路502连接。另一方的入口与动臂操作用油路4510B连接。出口与动臂调整用油路4520B连接。梭式阀151将介入用油路502及动臂操作用油路4510B中的先导液压较高的一方的油路与动臂调整用油路4520B连接。例如,在介入用油路502的先导液压高于动臂操作用油路4510B的先导液压时,梭式阀151以将介入用油路502与动臂调整用油路4520B连接、且不将动臂操作用油路4510B与动臂调整用油路4520B连接的方式进行工作。由此,介入用油路502的先导油经由梭式阀151向动臂调整用油路4520B供给。在动臂操作用油路4510B的先导液压高于介入用油路502的先导液压时,梭式阀151以将动臂操作用油路4510B与动臂调整用油路4520B连接、且不将介入用油路502与动臂调整用油路4520B连接的方式进行工作。由此,动臂操作用油路4510B的先导油经由梭式阀51向动臂调整用油路4520B供给。
在介入用油路501上设有对介入用油路501的先导油的先导液压进行检测的压力传感器168。介入用油路501包括:通过控制阀127C之前的先导油所流动的介入用油路501;通过介入阀127C之后的先导油所流动的介入用油路502。介入阀127C为了执行动臂介入控制,基于从作业机控制装置25输出的控制信号而被控制。
在不执行动臂介入控制时,基于通过操作装置30的操作而调整后的先导液压来驱动方向控制阀164。因此,作业机控制装置25对控制阀127不输出控制信号。例如,作业机控制装置25通过动臂用减压阀270B将动臂操作用油路4510B打开(设为全开)并通过介入阀127C将介入用油路501关闭,以便基于通过操作装置30的操作而调整后的先导液压来驱动方向控制阀640。
在执行动臂介入控制时,作业机控制装置25以基于由介入阀127C调整后的先导液压来驱动方向控制阀164的方式控制各控制阀127。例如,在挖掘控制中执行对动臂6的移动进行限制的动臂介入控制时,作业机控制装置25以使由介入阀127C调整后的介入用油路502的先导液压高于由操作装置30调整的动臂操作用油路4510B的先导液压的方式控制介入阀127C。如此,来自介入阀127C的先导油经由梭式阀151向方向控制阀640供给。
在以避免铲斗8侵入目标挖掘地形73I的方式通过操作装置30使动臂6以高速进行上升动作时,不执行动臂介入控制。以使动臂6以高速进行上升动作的方式对操作装置30进行操作,基于其操作量来调整先导液压,由此,通过操作装置30的操作而调整的动臂操作用油路4510B的先导液压高于由介入阀127C调整的介入用油路502的先导液压。由此,通过操作装置30的操作而调整了先导液压后的动臂操作用油路4510B的先导油经由梭式阀151向方向控制阀640供给。
在动臂介入控制中,作业机控制装置25判定是否满足限制条件。限制条件包括距离d比前述的第一规定值dth1小的情况及动臂限制速度Vc_bm_lmt比动臂目标速度Vc_bm大的情况。例如,在使动臂6下降的情况下,动臂6的向下方的动臂限制速度Vc_bm_lmt的大小小于向下方的动臂目标速度Vc_bm的大小时,作业机控制装置25判定为满足限制条件。而且,在使动臂6上升的情况下,动臂6的向上方的动臂限制速度Vc_bm_lmt的大小大于向上方的动臂目标速度Vc_bm的大小时,作业机控制装置25判定为满足限制条件。
在满足限制条件时,作业机控制装置25以使动臂以动臂限制速度Vc_bm_lmt上升的方式生成动臂介入指令CBI,对动臂油缸10的控制阀27进行控制。如此,动臂油缸10的方向控制阀640以使动臂以动臂限制速度Vc_bm_lmt上升的方式将工作油向动臂油缸10供给,因此动臂油缸10使动臂6以动臂限制速度Vc_bm_lmt上升。
在实施方式1中,斗杆限制速度Vc_am_lmt的绝对值比斗杆目标速度Vc_am的绝对值小的情况也可以包括于限制条件。限制条件还可以包括其他的条件。例如,限制条件还可以包括斗杆操作量为0的情况。限制条件也可以不包括距离d比第一规定值dth1小的情况。例如,限制条件可以仅为动臂6的限制速度比动臂目标速度大的情况。
第二规定值dth2只要比第一规定值dth1小即可,也可以大于0。这种情况下,在动臂6的铲边8T到达目标挖掘地形73I之前,进行动臂6的限制和斗杆7的限制这双方。因此,即使在动臂6的铲边8T到达目标挖掘地形73I之前,在动臂6的铲边8T欲超过目标挖掘地形73I时,也能够进行动臂6的限制和斗杆7的限制这双方。
(操作杆为电气方式时)
在左侧的作业机操作构件31L及右侧的作业机操作构件31R为电气方式时,作业机控制装置25获取与作业机操作构件31L、31R对应的电位计等的电信号。将该电信号称为操作指令电流值。作业机控制装置25将基于操作指令电流值的开闭指令向控制阀127输出。从控制阀127将与开闭指令对应的压力的工作油向方向控制阀的滑柱供给而使滑柱移动,因此经由方向控制阀向动臂油缸10、斗杆油缸11或铲斗油缸12供给工作油而使动臂油缸10、斗杆油缸11或铲斗油缸12伸缩。
在挖掘控制中,作业机控制装置25将基于挖掘控制的指令值及操作指令电流值的开闭指令向控制阀127输出。挖掘控制的指令值是在挖掘控制中用于执行动臂介入控制的指令值。被输入了开闭指令的控制阀127将与开闭指令对应的压力的工作油向方向控制阀的滑柱供给而使滑柱移动。向动臂油缸10的方向控制阀的滑柱供给与挖掘控制的指令值对应的压力的工作油,因此动臂油缸10伸长而使动臂6上升。
(指引的显示)
在指引中,图3B所示的第二显示装置39的铲斗铲边位置数据生成部39B基于从全局坐标运算部23获取的基准位置数据P及回旋体方位数据Q而生成回旋中心位置数据。并且,铲斗铲边位置数据生成部39B基于回旋中心位置数据和作业机2的倾斜度角α1、α2、α3而生成铲斗铲边位置数据S。而且,目标挖掘地形数据生成部39D根据目标施工信息T和铲斗铲边位置数据S,来生成表示用的目标挖掘地形数据Ua。显示部39M使用显示用的目标挖掘地形数据Ua来显示目标挖掘地形73I。
显示部39M根据目标挖掘地形73I和铲斗铲边位置数据S,依次(例如,100msec.周期)将铲斗8的正下方的目标挖掘地形73I的信息的一点确定为图3A所示的挖掘对象位置74。显示部39M从挖掘对象位置74沿着作业机2的前后方向延伸,对显示用的目标挖掘地形73I进行确定、显示。
目标挖掘地形数据生成部39D将液压挖掘机100的局部坐标的挖掘对象位置74、挖掘对象位置74的前后2点、挖掘对象位置74的前后2点以后的角度信息作为挖掘控制用的目标挖掘地形73I的信息即目标挖掘地形数据U,并将其向作业机控制装置25发送。第二显示装置39在指引及挖掘控制中,以从全局坐标运算部23获取的液压挖掘机100的位置信息及目标施工信息T为基础,例如以100msec.周期来生成目标挖掘地形数据U(目标挖掘地形73I),并将其向作业机控制装置25发送。
从第二显示装置39的目标挖掘地形数据生成部39D将目标挖掘地形数据U(目标挖掘地形73I)以例如100msec.的周期向作业机控制装置25输入。作业机控制装置25及第二显示装置39例如每10msec.将IMU29检测到的倾斜度角(以下,适当称为俯仰角)θ5输入。作业机控制装置25及第二显示装置39由IMU29检测,基于从传感器控制装置24输入的俯仰角θ5的上次值与本次值的增减量,对目标挖掘地形数据U(目标挖掘地形73I)的俯仰角θ5进行持续更新。作业机控制装置25使用该俯仰角θ5计算铲边位置P4而执行挖掘控制,第二显示装置39使用该俯仰角θ5计算铲斗铲边位置数据S而作为指引图像的铲边位置。在经过100msec.之后,从第二显示装置39将新的目标挖掘地形数据U(目标挖掘地形73I)向作业机控制装置25输入而进行更新。
图15是表示IMU29的一例的框图。IMU29包括回转仪29V、加速度传感器29A、AD转换部29AD、物理量转换部29PT。回转仪29V检测液压挖掘机100的角速度。加速度传感器29A检测液压挖掘机的加速度。由回转仪29V检测到的角速度及由加速度传感器29A检测到的加速度均是模拟量。AD转换部29AD将这些模拟量转换成数字量。物理量转换部29PT将AD转换部29AD的输出转换成物理量。具体而言,物理量转换部29PT将与回转仪29V的检测值对应的AD转换部29AD的输出转换成角速度ω,将与加速度传感器29A的检测值对应的AD转换部29AD的输出转换成加速度Ac。物理量转换部29PT将角速度ω及加速度Ac向车内信号线42输出。
AD转换部29AD将这些模拟量转换成数字量。物理量转换部29PT将AD转换部29AD的输出转换成物理量。具体而言,物理量转换部29PT将与回转仪29V的检测值对应的AD转换部29AD的输出转换成角速度ω,将与加速度传感器29A的检测值对应的AD转换部29AD的输出转换成加速度Ac。物理量转换部29PT将角速度ω及加速度Ac向车内信号线42输出。姿态角运算部29CP根据物理量转换部29PT求出的角速度ω及加速度Ac来运算姿态角θ,并将得到的姿态角θ向车内信号线42输出。以下,姿态角适当使用符号θ表示。如此,IMU29是检测液压挖掘机100的姿态角的装置。
液压挖掘机100的倾斜度可以由俯仰角、倾滚角及偏摆角表示。俯仰角是液压挖掘机100绕y轴倾斜时的角度,倾滚角是液压挖掘机100绕x轴倾斜时的角度,偏摆角是液压挖掘机100绕z轴倾斜时的角度。在本实施方式中,将俯仰角及倾滚角称为液压挖掘机100的姿态角。在本实施方式中,传感器控制装置24经由车内信号线42获取IMU29检测到的液压挖掘机100的角速度及加速度。传感器控制装置24根据获取的液压挖掘机100的角速度及加速度来求出姿态角。以下,姿态角适当使用符号θ表示。
图16是传感器控制装置24的控制框图。图17是用于说明上部回旋体3的回旋速度的图。在本实施方式中,图15所示的IMU29的姿态角运算部29CP作为第一姿态角运算部而发挥功能,根据通过作为检测装置的回转仪29V及加速度传感器29A检测到的角速度ω及加速度Ac来求出作业机械的姿态角θ向低通滤波器60输出。第二姿态角运算部50求出第二姿态角θ2并将其输出。第二姿态角运算部50输出的第二姿态角θ2不通过低通滤波器60而向选择部63输入。关于第二姿态角运算部50的详细情况在后文叙述。
IMU29的检测值经由车内信号线42向传感器控制装置24输入。传感器控制装置24被从IMU29输入角速度ω、加速度Ac及姿态角θ。传感器控制装置24包括第二姿态角运算部50、低通滤波器60、选择部63。此外,传感器控制装置24包括回旋状态判定部61、姿态角判定部62。
作为第一滤波器的低通滤波器60使从IMU29输入的姿态角θ通过,作为第一姿态角θ1而输出。在本实施方式中,作为姿态角θ而将俯仰角θp及倾滚角θr向低通滤波器60输入,作为第一姿态角θ1而输出第一俯仰角θ1p及第一倾滚角θ1r。低通滤波器60输出的第一姿态角θ1向选择部63输入。当姿态角θ通过低通滤波器60时,输出从姿态角θ除去了高频成分的第一姿态角θ1。
选择部63基于图1及图2所示的与液压挖掘机100的角度变动相关的信息而切换通过了低通滤波器60的第一姿态角θ1与未通过低通滤波器60的第二姿态角θ2,并将其作为液压挖掘机100的姿态角θo向车内信号线41输出。选择部63输出的姿态角θo是俯仰角θpo及倾滚角θro。
在本实施方式中,第二姿态角θ2未通过低通滤波器60是指第二姿态角θ2未通过第一姿态角θ1所通过的低通滤波器60。第二姿态角θ2可以通过第一姿态角θ1所通过的低通滤波器60以外的滤波器,例如,来自IMU29的姿态角θ可以直接向选择部63输入。
在本实施方式中,选择部63基于图1所示的与液压挖掘机100的回旋相关的信息、更具体而言基于上部回旋体3的角速度ωz,来切换输出第一姿态角θ1和第二姿态角θ2中的哪一个。例如,选择部63在角速度(以下,适当称为回旋速度)ωz为规定的阈值以下时,输出第一姿态角θ1,在回旋速度ωz超过了规定的阈值时,输出第二姿态角θ2。如图17所示,回旋速度ωz是上部回旋体3的绕着成为旋转中心的z轴(旋转中心轴)的角速度。z轴在液压挖掘机100的局部坐标系(x,y,z)中是上部回旋体3设为回旋的中心的轴。
选择部63例如可以基于液压挖掘机100的俯仰角的变化,将第一姿态角θ1与第二姿态角θ2切换并将其输出为与液压挖掘机100的角度变动相关的信息。例如,选择部63在液压挖掘机100的俯仰角的变化量为规定的阈值以下时,能够输出第一姿态角θ1,在超过了规定的阈值时,能够输出第二姿态角θ2。
回旋状态判定部61经由车内信号线42从IMU29获取回旋速度ωz。回旋状态判定部61将获取的回旋速度ωz与规定的阈值进行比较,在回旋速度ωz为规定的阈值以下时,将第一输出向选择部63输出,在回旋速度ωz超过了规定的阈值时,将第二输出向选择部63输出。选择部63在获取了第一输出时,输出第一姿态角θ1,在获取了第二输出时,输出第二姿态角θ2。
姿态角判定部62求出第一姿态角θ1与第二姿态角θ2的差量Δθ,并将其向选择部63输出。在差量超过了规定的阈值时,选择部63将第二姿态角θ2作为液压挖掘机100的姿态角θo向车内信号线41输出。
(第二姿态角运算部的例子)
第二姿态角运算部50包括角度运算部50C、相当于第二滤波器的滤波部50F、切换部55。角度运算部50C包括第三姿态角运算部51、第四姿态角运算部52,滤波部50F包括第一互补滤波器53、第二互补滤波器54。第三姿态角运算部51及第四姿态角运算部52根据液压挖掘机100的角速度ω及加速度Ac来求出液压挖掘机100的姿态角θ。在本实施方式中,第三姿态角运算部51根据IMU29检测到的液压挖掘机100的加速度Ac来求出姿态角θ。更具体而言,第三姿态角运算部51根据重力加速度的方向来求出姿态角θ。第四姿态角运算部52根据IMU29检测到的液压挖掘机100的角速度ω来求出姿态角θ。更具体而言,第四姿态角运算部52对角速度ω进行积分来求出姿态角θ。
第一互补滤波器53设定第一截止频率,减少由第三姿态角运算部51及第四姿态角运算部52求出的姿态角θ中含有的杂音,并输出第三姿态角θ3。第二互补滤波器54设定与第一截止频率不同的第二截止频率,减少由第三姿态角运算部51及第四姿态角运算部52求出的姿态角θ中含有的杂音,并输出第四姿态角θ4。第一互补滤波器53与第二互补滤波器54仅截止频率(cut-off频率)不同。
第一互补滤波器53具有滤波部53F和加法运算部53AD。滤波部53F具有第一LPF(Low Pass Filter)a和第一HPF(High Pass Filter)a。加法运算部53AD将第一LPFa的输出与第一HPFa的输出相加而输出。加法运算部53AD的输出是第一互补滤波器53的输出。将第一互补滤波器53的输出适当称为第三姿态角θ3。
第二互补滤波器54具有滤波部54F和加法运算部54AD。滤波部54F具有第二LPF(Low Pass Filter)b和第二HPF(High Pass Filter)b。加法运算部54AD将第二LPFb的输出与第二HPFb的输出相加而输出。加法运算部54AD的输出是第二互补滤波器54的输出。将第二互补滤波器54的输出称为第四姿态角θ4。
切换部55具有处理部55c和切换器55s。切换部55根据液压挖掘机100的状态,对第三姿态角θ3或第四姿态角θ4进行切换并将其输出。切换部55的处理部55c根据液压挖掘机100的状态、例如液压挖掘机100是动态还是静止,来判定输出第三姿态角θ3或第四姿态角θ4的哪一个。处理部55c的判定结果经由判定结果输出线55a向切换器55s输出。切换器55s根据处理部55c的判定结果,将第三姿态角θ3或第四姿态角θ4的任一方作为由第二姿态角运算部50求出的第二姿态角θ2,经由姿态角输出线55b向车内信号线41输出。
图18是表示互补滤波器的特性的图。图18的纵轴是增益GN,横轴是频率f。图18的曲线(LPF和HPF)表示互补滤波器的频率特性。互补滤波器具备LPF(Low Pass Filter)和HPF(High Pass Filter),从图18可知,是LPF的增益GN与HPF的增益GN之和成为1的滤波器。例如,当向互补滤波器输入姿态角θ时,LPF的输出LPF(θ)与HPF的输出HPF(θ)之和成为1。即,成为LPF(θ)+HPF(θ)=θ。将LPF的增益GN和HPF的增益GN均成为0.5时的频率称为截止频率fc。传感器控制装置24具有的第一互补滤波器53及第二互补滤波器54如前述那样仅截止频率fc不同。
图16所示的第三姿态角运算部51根据重力加速度的方向来求出的姿态角θ以真正的姿态角θtr与误差θan之和的方式求出。误差θan例如因冲击加速度等那样的重力加速度以外的加速度而产生。误差θan是高频成分为主体的杂音。图16所示的第四姿态角运算部52对角速度ω进行积分来求出的姿态角θ以真正的姿态角θtr与误差θwn之和的方式求出。误差θwn因通过积分所累积的偏差而产生。误差θwn是低频成分为主体的杂音。
如此,第三姿态角运算部51根据重力加速度的方向来求出的姿态角θ由于包括高频成分为主体的误差θan,因此向第一互补滤波器53的第一LPFa及第二互补滤波器54的第二LPFb输入。第四姿态角运算部52对角速度ω进行积分来求出的姿态角θ由于包括低频成分为主体的误差θwn,因此向第一互补滤波器53的第一HPFa及第二互补滤波器54的第二HPFb输入。
第一LPFa的输出成为LPFa(θtr+θan),第一HPFa的输出成为LPFa(θtr+θwn)。第二LPFb的输出成为LPFb(θtr+θan),第二HPFb的输出成为LPFb(θtr+θwn)。LPFa(θtr+θan)、LPFa(θtr+θwn)、LPFb(θtr+θan)及LPFb(θtr+θwn)均具有线形性。因此,式(1)至式(4)成立。
LPFa(θtr+θan)=LPFa(θtr)+LPFa(θan)··(1)
HPFa(θtr+θwn)=HPFa(θtr)+HPFa(θwn)··(2)
LPFb(θtr+θan)=LPFb(θtr)+LPFb(θan)··(3)
HPFb(θtr+θwn)=HPFb(θtr)+HPFb(θwn)··(4)
根据前述的互补滤波器的特性,LPFa(θ)+HPFa(θ)=θ及LPFb(θ)+HPFb(θ)=θ成立。在第一互补滤波器53中,滤波部53F的输出、即第一LPFa的输出和第一HPFa的输出通过加法运算部53AD而相加。加法运算部53AD的输出即第三姿态角θ3成为θtr+LPFa(θan)+HPFa(θwn)。在第二互补滤波器54中,滤波部54F的输出、即第二LPFb的输出和第二HPFb的输出通过加法运算部54AD而相加。加法运算部54AD的输出即第四姿态角θ4成为θtr+LPFb(θan)+HPFb(θwn)。
误差θan由于高频成分为主体,因此通过第一LPFa及第二LPFb而减小。因此,LPFa(θan)及LPFb(θan)的值减小。误差θwn由于低频成分为主体,因此通过第一HPFa及第二HPFb而减小。因此,LPFa(θan)及HPFa(θwn)以及LPFb(θan)及HPFb(θwn)的值减小,作为加法运算部53AD的输出的第三姿态角θ3及作为加法运算部54AD的输出的第四姿态角θ4成为接近于真正的姿态角θtr的值。
图19是表示误差θan及误差θwn的频率特性的图。图19的纵轴是误差θan及误差θwn的光谱,横轴是频率f。假设能够使用IMU29的性能高的结构时,IMU29检测的角速度ω及加速度Ac的精度也高,因此图16所示的传感器控制装置24具有的第一姿态角运算部51求出的姿态角θ的误差θan及第二姿态角运算部52求出的姿态角θ的误差θwn减小。在IMU29的性能低时,IMU29检测的角速度ω及加速度Ac的精度降低,因此图16所示的第二姿态角运算部50具有的第三姿态角运算部51求出的姿态角θ的误差θan及第四姿态角运算部52求出的姿态角θ的误差θwn增大。其结果是,如图19所示,误差θwn和误差θan即使分别超过互补滤波器的截止频率fc也存在,在包括截止频率fc的规定的频率f的范围内重合。误差θwn即使在比截止频率fc大的频率下也存在,误差θan即使在比截止频率fc小的频率下也存在。
因此,在IMU29的性能低时,在一个互补滤波器中,无法将作为杂音的误差θwn及误差θan充分除去,可能会导致姿态角θ的精度下降。这可能会给图2所示的第二显示装置39进行的铲边8T的位置信息的显示精度及液压挖掘机100的作业机控制的精度造成影响。高性能的IMU29由于价格也高,因此会导致液压挖掘机100的制造成本的上升。即,为了将性能低的IMU29适用于液压挖掘机100,需要考虑图19所示的特性。因此,第二姿态角运算部50使用截止频率fc不同的第一互补滤波器53和第二互补滤波器54,以便于即使在使用性能比较低的IMU29的情况下也能够抑制姿态角θ的精度下降。
图20是表示第一互补滤波器53的增益GN及第二互补滤波器54的增益GN与频率f之间的关系的图。图20的纵轴是增益GN,横轴是频率f。频率fch是第一互补滤波器53的第一截止频率,频率fcl是第二互补滤波器54的第二截止频率。在本实施方式中,第一截止频率fch比第二截止频率fcl高。即,第二截止频率fcl比第一截止频率fch低。
第一互补滤波器53的第一截止频率fch设定为能够使角速度ω的积分误差即误差θwn充分减小的频率。第二互补滤波器54的第二截止频率fcl设定为能够使重力加速度以外的加速度引起的误差θan充分减小的频率。
第一互补滤波器53虽然通过第一HPFa能够使角速度ω的积分产生的误差θwn有效地减小,但是难以使重力加速度以外的加速度引起的误差θan有效地减小。因此,第一互补滤波器53在液压挖掘机100为静止的状态或接近于静止状态的状态即看作静止的状态(适当称为准静止状态)时,能够高精度地求出姿态角θ,但是在液压挖掘机100为不是准静止状态的动态的状态时,姿态角θ的精度下降。在本实施方式中,动态的状态是看作液压挖掘机100动作的状态。
第二互补滤波器54虽然通过第二LPFa能够有效地减小重力加速度以外的加速度引起的误差θan,但是难以有效地减小角速度ω的积分引起的误差θwn。因此,第二互补滤波器54在液压挖掘机100为动态的状态时能够高精度地求出姿态角θ,但是在液压挖掘机100为准静止状态时,与第一互补滤波器53计算的姿态角θ相比,姿态角θ的精度下降。即,第二互补滤波器54虽然在短时间的动特性方面优异,但是在准静止状态下,与动态的状态同样地存在角速度ω的积分引起的误差θwn。
图16所示的第二姿态角运算部50具备的切换部55根据液压挖掘机100的状态是准静止状态还是动态的状态,对第三姿态角θ3或第四姿态角θ4进行切换并将其输出。例如,切换部55在液压挖掘机100为准静止状态时,将第一互补滤波器53输出的第三姿态角θ3作为第二姿态角θ2向车内信号线41输出。在液压挖掘机100为动态的状态时,切换部55将第二互补滤波器54输出的第四姿态角θ4作为第二姿态角θ2向车内信号线41输出。
如此,第二姿态角运算部50在液压挖掘机100为准静止状态时,将第一互补滤波器53的第三姿态角θ3作为第二姿态角θ2,因此在准静止状态下能够抑制第二姿态角θ2的精度下降。在液压挖掘机100为动态的状态时,第二姿态角运算部50将第二互补滤波器54的第四姿态角θ4作为第二姿态角θ2,因此即使在动态的状态下也能够抑制第二姿态角θ2的精度下降。其结果是,第二姿态角运算部50无论液压挖掘机100为准静止状态及动态的状态的哪一状态,都能够抑制第二姿态角θ2的精度下降。
在液压挖掘机100动作时,使用第二互补滤波器54输出的第四姿态角θ4,求出例如图1所示的铲斗8的铲边8T的位置。而且,在液压挖掘机100静止时,通过第一互补滤波器53输出的第三姿态角θ3,求出铲斗8的铲边8T的位置。因此,图2所示的第二显示装置39能抑制求出以铲斗8的铲边8T的位置为代表的作业机2的位置或液压挖掘机100的车辆主体1的位置等时的精度下降。
切换部55的处理部55c例如使用如下的条件A和条件B来判定准静止状态和动态的状态,并基于其判定结果来控制切换器55s。
条件A:在进行切换的判定的时刻之前的规定期间,第三姿态角θ3的标准偏差比预先设定的阈值小。
条件B:重力加速度以外的加速度的大小比预先设定的阈值小。
第三姿态角θ3根据IMU29检测到的角速度ω或加速度Ac来求出,包括重力加速度的加速度由IMU29检测。即,处理部55c基于液压挖掘机100具备的IMU29的状态来判定准静止状态与动态的状态。
对前述的条件B进行说明。IMU29如前述那样,检测至少包括重力加速度的加速度,不对检测到的各个加速度的种类进行区别地将检测到的加速度输出。重力加速度为已知。因此,处理部55c根据IMU29输出的加速度来运算x轴方向或y轴方向的加速度。当从求出的x轴方向的加速度减去重力加速度的x轴方向相当的重力加速度时,处理部55c能够求出重力加速度以外的加速度的大小。处理部55c将重力加速度以外的加速度的大小与预先设定的阈值进行比较。需要说明的是,处理部55c可以从求出的y轴方向的加速度减去重力加速度的y轴方向相当的重力加速度,来求出重力加速度以外的加速度的大小,并与预先设定的阈值进行比较来判定条件B是否成立。
处理部55c获取从IMU29获取的加速度Ac及作为第一互补滤波器53的输出的第三姿态角θ3,判定条件A及条件B是否同时成立。在条件A和条件B这双方成立时,可以看作准静止状态即液压挖掘机100静止。这种情况下,处理部55c以使切换器55s与第一互补滤波器53的加法运算部53AD连接的方式使切换器55s动作。切换器55s将第一互补滤波器53输出的第三姿态角θ3作为第二姿态角θ2向车内信号线41输出。
处理部55C经由图16所示的加速度发送线L1或第一姿态角发送线L2来获取从IMU29获取的加速度Ac及作为第一互补滤波器53的输出的第三姿态角θ3,判定条件A及条件B是否同时成立。在条件A和条件B这双方成立时,可以看作准静止状态。在本实施方式中,准静止状态是液压挖掘机100不进行行驶、上部回旋体3的回旋及作业机2的动作而完全静止的状态、或不进行行驶、液压挖掘机100的上部回旋体3的回旋而仅作业机2动作的状态。这种情况下,处理部55c以使切换器55S与第一互补滤波器53的加法运算部53AD连接的方式使切换器55s动作。切换器55s将第一互补滤波器53输出的第三姿态角θ3作为第二姿态角θ2向车内信号线41输出。
在条件A和条件B不成立时,即,条件A及条件B中的至少一方不成立时,可以看作动态的状态即液压挖掘机100动作。这种情况下,处理部55c以使切换器55s与第二互补滤波器54的加法运算部54AD连接的方式使切换器55s动作。切换器55s将第二互补滤波器54输出的第四姿态角θ4作为第二姿态角θ2向车内信号线41输出。若切换部55使用条件A及条件B来切换第三姿态角θ3与第四姿态角θ4,则仅通过IMU29的检测值就能够实现前述的切换。
在本实施方式中,条件A的规定期间设定为例如1秒,但并未限定于此。与条件A的标准偏差进行比较的阈值不受限定,但例如可以设定为0.1度。条件B在重力加速度以外的加速度比预先设定的阈值小时成立,在检测到预先设定的阈值以上的重力加速度以外的加速度时不成立。条件B的阈值不受限定,但例如可以在重力加速度的0.1倍以上的范围内适当设定。
图21是表示第二姿态角运算部50的切换部55输出的第二姿态角θ2、第三姿态角θ3、第四姿态角θ4的时间变化的一例的图。图21的纵轴是姿态角θ,横轴是时间t。图21的Sst所示的区间为准静止状态,将第三姿态角θ3作为第二姿态角θ2而输出。图21的Sdm所示的区间为动态的状态,将第四姿态角θ4作为第二姿态角θ2而输出。在图21所示的例子中,时间t1至时间t2、及时间t3以后为准静止状态Sst,时间t2至时间t3为动态的状态Sdm。
第二姿态角θ2在时间t2从第三姿态角θ3切换为第四姿态角θ4,在时间t3从第四姿态角θ4切换为第三姿态角θ3。第四姿态角θ4由于蓄积有对角速度ω进行积分引起的误差θwn,因此在时间t2,第三姿态角θ3与第四姿态角θ4成为不同的值。同样,在时间t3,第四姿态角θ4与第三姿态角θ3成为不同的值。
切换部55将从第二姿态角运算部50输出的第二姿态角θ2从第三姿态角θ3切换为第四姿态角θ4时或从第四姿态角θ4切换为第三姿态角θ3时,若直接切换,则在切换时,第二姿态角θ2存在不连续的可能性。而且,如前述那样,第四姿态角θ4由于蓄积有对角速度ω进行积分引起的误差θwn,因此在使用第四姿态角θ4作为第二姿态角θ2时,需要减小积分引起的误差θwn。
为了减小在第二姿态角θ2的切换时产生的第二姿态角θ2的不连续及积分引起的误差θwn,在本实施方式中,切换部55的处理部55c使用式(5)至式(10)来求出第二姿态角θ2并输出。
θ2=θ3+dif··(5)
θ2=θ4+dif···(6)
dif=Ftr×dif_prev···(7)
dif=dif_prev···(8)
dif=dif_prev+θ3-θ4···(9)
dif=dif_prev+θ4-θ3···(10)
式(5)在准静止状态下求出第二姿态角θ2时使用,式(6)在动态的状态下求出第二姿态角θ2时使用。式(5)及式(6)的dif是缓和项。式(7)的缓和项dif在准静止状态下使用,式(8)的缓和项dif在动态的状态下使用。式(7)的Ftr是缓和系数。缓和系数Ftr大于0且小于1(0<Ftr<1)。式(9)的缓和项dif在从准静止状态向动态的状态转变的时机下使用。式(10)的缓和项dif在从动态的状态向准静止状态转变的时机下使用。式(8)至式(10)的dif_prev是前一个的IMU29的状态(准静止状态Sst或动态的状态Sdm)下的缓和项dif。dif_prev的初始值为0。
如图21所示,第三姿态角θ3在准静止状态Sst下保持高精度,但是在动态的状态Sdm下产生大的误差。第四姿态角θ4在准静止状态Sst及动态的状态Sdm中的任一状态下也会产生积分的累计引起的误差。由于dif_prev的初始值为0,因此在时间t1至时间t2的准静止状态Sst下,缓和项dif=0。其结果是,根据式(5),准静止状态Sst下的第二姿态角θ2成为第三姿态角θ3。
在从准静止状态Sst切换为动态的状态Sdm时,即时间t=t2时,处理部55c使用式(9)来求出缓和项dif。如前述那样,时间t=t2时的缓和项dif为0,因此第三姿态角θ3与第四姿态角θ4之差成为θ3-θ4的值。如图21所示,这种情况下的缓和项dif为负值。在时间t2时,式(5)的第二姿态角θ2为θ3,向式(6)的缓和项dif输入的是θ3-θ4的值,因此式(6)的第二姿态角θ2也成为θ3。因此,在从准静止状态Sst切换为动态的状态Sdm时,第二姿态角θ2连续变化。
在时间t2至时间t3的动态的状态Sdm下,缓和项dif的值仍保持在切换时即时间t2得到的θ3-θ4的值。动态的状态Sdm下的第二姿态角θ2根据式(6),从动态的状态Sdm下的第四姿态角θ4加上在时间t2得到而保持的缓和项dif=θ3-θ4来求出。根据式(8),此时使用的缓和项dif成为dif_prev,因此在动态的状态Sdm下使用的缓和项dif使用的是在时间t2得到而保持的缓和项dif=θ3-θ4的值。如此,切换部55的处理部55c在将第三姿态角θ3切换为第四姿态角θ4之后,使用切换时的从第三姿态角θ3减去第四姿态角θ4的值即切换时的缓和项dif作为修正值,对求出的第四姿态角θ4进行修正,获取第二姿态角θ2。由此,能够减小在向动态的状态Sdm的切换以前产生的第四姿态角θ4的积分的蓄积引起的误差θwn对第二姿态角θ2造成的影响。
在从动态的状态Sdm再次切换为准静止状态Sst时、即时间t3,处理部55c使用式(10)来求出缓和项dif。式(10)的dif_prev是已经得到而保持的缓和项dif。即,式(10)的dif_prev是时间t2的缓和项dif、即时间t2的θ3-θ4的值。根据式(10),时间t3的缓和项dif是在时间t2得到而保持的θ3-θ4的值与在时间t3得到的θ2-θ1的值相加的值。通过使用式(10)从动态的状态Sdm切换为准静止状态Sst时,第二姿态角θ2连续变化。
在时间t3以后的准静止状态Sst下,处理部55c使用式(5)来求出第二姿态角θ2。此时的缓和项dif通过式(7)来确定。式(7)中的dif_prev是从动态的状态Sdm再次切换为准静止状态Sst的时机即时间t3的缓和项dif。在时间t3以后的准静止状态Sst下,因缓和系数Ftr的效果而缓和项dif的值逐渐减小,收敛于0。即,在时间t3以后的准静止状态Sst下,第二姿态角θ2收敛于第三姿态角θ3。如此,切换部55的处理部55c在将第四姿态角θ4切换为第三姿态角θ3之后,使用切换时的第四姿态角θ4的误差即切换时的缓和项dif乘以作为大于0且小于1的系数的缓和系数Ftr所得到的值作为修正值,对第三姿态角θ3进行修正。如此,在从动态的状态Sdm切换为准静止状态Sst之后,第二姿态角θ2连续变化。
(求出第二姿态角θ2的处理的一例)
图22是表示求出第二姿态角θ2的处理的一例的流程图。在求取第二姿态角θ2时,图16所示的第二姿态角运算部50在步骤S1中,经由车内信号线42获取IMU29产生的角速度ω及加速度Ac的检测值。在步骤S2中,图16所示的第三姿态角运算部51根据IMU29检测到的加速度Ac来求出姿态角θ。在步骤S3中,图16所示的第四姿态角运算部52根据IMU29检测到的角速度ω来求出姿态角θ。步骤S2与步骤S3的顺序不分先后。
在步骤S4中,图16所示的第一互补滤波器53的第一LPFa对于根据加速度Ac而得到的姿态角θ实施滤波处理。在步骤S5中,图16所示的第二互补滤波器54的第二LPFb对于根据加速度Ac而得到的姿态角θ实施滤波处理。在步骤S6中,图16所示的第一互补滤波器53的第一HPFa对于根据角速度ω而得到的姿态角θ实施滤波处理。在步骤S7中,图16所示的第二互补滤波器54的第二HPFb对于根据角速度ω而得到的姿态角θ实施滤波处理。步骤S4、步骤S5、步骤S6、步骤S7的顺序不分先后。
接着,进入步骤S8,第一互补滤波器53求出第三姿态角θ3。具体而言,加法运算部53AD通过将第一LPFa的输出与第一HPFa的输出相加来求出第三姿态角θ3。在步骤S9中,第二互补滤波器54求出第四姿态角θ4。具体而言,加法运算部54AD通过将第二LPFb的输出与第二HPFb的输出相加来求出第四姿态角θ4。步骤S8、步骤S9的顺序不分先后。
进入步骤S10,图16所示的切换部55的处理部55c在液压挖掘机100为准静止状态时(步骤S10为“是”),使处理进入步骤S11。在步骤S11中,处理部55c以使第二姿态角运算部50输出第三姿态角θ3作为第二姿态角θ2的方式控制切换器55s。在液压挖掘机100为动态的状态时(步骤S10为“否”),在步骤S12中,处理部55c以使第二姿态角运算部50输出第四姿态角θ4作为第二姿态角θ2的方式控制切换器55s。
(是准静止状态还是动态的状态的判定的变形例)
在本实施方式中,图16所示的切换部55的处理部55c基于图15所示的IMU29的检测值,对第三姿态角θ3或第四姿态角θ4进行切换,并将其作为第二姿态角θ2而输出。第三姿态角θ3或第四姿态角θ4的选择并不限定于这样的情况,处理部55c例如也可以使用与液压挖掘机100的动作相关的信息(以下,适当称为动作信息)来切换第三姿态角θ3或第四姿态角θ4。
在本实施方式中,动作信息是与液压挖掘机100产生了某种动作的情况相关的信息。例如,动作信息是图1A所示的上部回旋体3是否回旋的信息、行驶装置5是否动作的信息或作业机2是否动作的信息等。动作信息例如使用从检测上部回旋体3回旋的情况的传感器输出的检测值、在用于使上部回旋体3回旋的回旋马达上设置解析器等回旋角度传感器且从这样的角度检测器或旋转传感器输出的检测值、或者从检测由图2所示的操作装置30生成的先导压的液压传感器输出的检测值等。即,动作信息例如可以是上部回旋体3或作业机2等实际上是否动作的信息,也可以是对用于使上部回旋体3或作业机2等动作的操作构件的操作的信息。
图23是表示本实施方式的变形例的第三姿态角θ3与第四姿态角θ4的切换所使用的表TB的一例的图。在本变形例中,切换部55的处理部55c根据基于IMU29的检测值的是准静止状态还是动态的状态的判定、上部回旋体3是否为回旋中的判定,来切换第三姿态角θ3或第四姿态角θ4。表TB记载有相对于上部回旋体3的状态和基于IMU29的检测值的条件A及条件B而作为第二姿态角θ2应输出的姿态角。上部回旋体3的状态由ON或OFF表示,在ON时为回旋中,在OFF时为停止中。条件A及条件B由A&B或NOT(A&B)表示,A&B为准静止状态,NOT(A&B)为动态的状态。
基于IMU29的检测值的判定结果为准静止状态,且根据动作信息而上部回旋体3为回旋中(ON)。这种情况下,切换部55将第四姿态角θ4作为第二姿态角θ2而输出。由于上部回旋体3实际上动作,因此通过使用第四姿态角θ4作为第二姿态角θ2而能够确保第二姿态角θ2的精度。
基于IMU29的检测值的判定结果为准静止状态,且根据动作信息而上部回旋体3为停止中(OFF)。这种情况下,切换部55将第三姿态角θ3作为第二姿态角θ2而输出。由于为准静止状态且上部回旋体3实际上停止,因此通过使用第三姿态角θ3作为第二姿态角θ2而能够减小角速度ω的积分引起的误差。
基于IMU29的检测值的判定结果为动态的状态,且根据动作信息而上部回旋体3为回旋中(ON)。这种情况下,切换部55将第四姿态角θ4作为第二姿态角θ2而输出。由于为动态的状态且上部回旋体3实际上动作,因此通过使用第四姿态角θ4作为第二姿态角θ2而能够确保第二姿态角θ2的精度。
基于IMU29的检测值的判定结果为动态的状态,且根据动作信息而上部回旋体3为停止中(OFF)。这种情况下,切换部55可以将第三姿态角θ3或第四姿态角θ4的任一个作为第二姿态角θ2而输出,但是在本变形例中输出第四姿态角θ4。
在本变形例中,切换部55根据基于IMU29的检测值的是准静止状态还是动态的状态的判定、上部回旋体3是否为回旋中的判定,来切换第三姿态角θ3或第四姿态角θ4。由此,切换部55能够更高精度地判定液压挖掘机100的状态,能够选择适当的姿态角。在本变形例中,并不限定于前述的处理,切换部55也可以基于上部回旋体3是否为回旋中的判定来切换第三姿态角θ3或第四姿态角θ4。例如,可以在上部回旋体3为回旋中时将第四姿态角θ4作为第二姿态角θ2,在上部回旋体3为停止中时将第三姿态角θ3作为第二姿态角θ2。接着,说明本实施方式的姿态角计算方法的第一例。
(姿态角计算方法的第一例)
图24是表示本实施方式的姿态角计算方法的第一例的处理次序的流程图。在步骤S101中,图16所示的IMU29及传感器控制装置24求出姿态角θ。传感器控制装置24的低通滤波器60使从IMU29获取的姿态角θ通过,作为第一姿态角θ1向选择部63输出。第二姿态角运算部50具有的角度运算部50C求出姿态角θ,滤波部50F使姿态角θ通过并作为第二姿态角θ2输出。
在步骤S102中,回旋状态判定部61将经由车内信号线42获取的回旋速度ωz与规定的阈值ωzc进行比较。在回旋速度ωz为规定的阈值ωzc以下时(步骤S102为“是”),回旋状态判定部61将第一输出向选择部63输出。这种情况下,上部回旋体3不回旋、或虽然回旋但是接近于静止状态的状态。获取第一输出的选择部63在步骤S103中输出第一姿态角θ1作为姿态角θo。
在回旋速度ωz大于规定的阈值ωzc时(步骤S102为“否”),回旋状态判定部61将第二输出向选择部63输出。这种情况下,上部回旋体3为回旋的状态。获取了第二输出的选择部63在步骤S104中输出第二姿态角θ2作为姿态角θo。接着,进入步骤S105,回旋状态判定部61判定回旋速度ωz为规定的阈值ωzc以下的状态是否持续时间tc1以上。
在回旋速度ωz为规定的阈值ωzc以下的状态持续时间tc1以上时(步骤S105为“是”),回旋状态判定部61将第一输出向选择部63输出。这种情况下,能够判断为上部回旋体3返回不回旋、或虽然回旋也接近于静止状态的状态。因此,获取了第一输出的选择部63在步骤S106中输出第一姿态角θ1作为姿态角θo。在回旋速度ωz为规定的阈值以下的状态未持续时间tc1以上时(步骤S105为“否”),回旋状态判定部61将第二输出向选择部63输出。这种情况下,上部回旋体3为回旋的状态。获取了第二输出的选择部63返回步骤S104,将第二姿态角θ2作为姿态角θo输出。
第二显示装置39使用经由图2所示的车内信号线41从传感器控制装置24输出的姿态角θo,来求出例如铲斗8的铲边8T的位置。而且,作业机控制装置25使用经由图2所示的车内信号线41从传感器控制装置24输出的姿态角θo,执行例如前述的挖掘控制。
第一姿态角θ1通过IMU29求出的姿态角θ通过低通滤波器60而得到,因此高频成分减少。因此,第二显示装置39及作业机控制装置25在求出铲边8T的位置时,能抑制铲边8T的位置的细微的变化。其结果是,在液压挖掘机100静止时的挖掘控制中,能够更可靠地抑制超过目标挖掘地形73I地挖入挖掘对象的情况。
另外,在上部回旋体3的回旋中,使用未通过低通滤波器60的第二姿态角θ2,因此与液压挖掘机100的姿态的变化对应的第二姿态角θ2的响应性比第一姿态角θ1高。因此,与液压挖掘机100的动作例如上部回旋体3的动作对应的姿态角θ的变化反映到第二姿态角θ2中。因此,在上部回旋体3的回旋中,能够反映铲边8T的位置的变化来计算目标挖掘地形。其结果是,在挖掘控制中,能够更可靠地抑制超过目标挖掘地形73I地挖入挖掘对象的情况。如此,传感器控制装置24能够以无论液压挖掘机100的动作状态如何都能够抑制超过目标挖掘地形73I地挖入挖掘对象的方式控制作业机2。
另外,在液压挖掘机100静止时,第二显示装置39能够显示抑制了铲边8T的位置的细微的变化的指引图像。其结果是,能抑制指引图像所显示的显示用的目标挖掘地形73I及铲边8T的变动。因此,操作员容易按照指引图像来操作作业机2,因此操作性提高,并且能抑制目标挖掘地形73I的过度挖入或挖入不足。而且,第二显示装置39在上部回旋体3的回旋中显示指引图像时,能够显示反映了铲边8T的位置的变化的指引图像。其结果是,操作员一边观察该指引图像一边进行作业,由此能抑制目标挖掘地形73I的过度挖入或挖入不足。
(姿态角计算方法的第二例)
图25是用于说明俯仰角的变化的图。俯仰角θp是在液压挖掘机100的局部坐标系(x,y,z)中、液压挖掘机100绕x轴倾斜时的角度。例如,根据液压挖掘机100的倾斜度状态而俯仰角θp发生变化。姿态角判定部62求出第一姿态角θ1与第二姿态角θ2的差量Δθ。使用第一俯仰角θ1p作为第一姿态角θ1,并使用第二俯仰角θ2p作为第二姿态角θ2。在本实施方式中,通过了低通滤波器60后的第一俯仰角θ1p是地面GD与倾斜GD1所成的角度。从第二姿态角运算部50获取的第二俯仰角θ2p是地面GD与倾斜GD2所成的角度。差量为Δθp。姿态角判定部62将求出的差量Δθp向选择部63输出。选择部63在差量Δθp为规定的阈值以上时,将第二姿态角θ2作为液压挖掘机100的姿态角θo向车内信号线41输出。
在差量Δθp为规定的阈值以上时,液压挖掘机100的绕x轴的倾斜度急剧增大。这种情况下,当将第一姿态角θ1设为液压挖掘机100的姿态角θo时,可能无法将液压挖掘机100的姿态的急变反映到姿态角θo中。因此,选择部63在差量Δθp为规定的阈值以上时,将第二姿态角θ2作为液压挖掘机100的姿态角θo向车内信号线41输出。由此,能够将液压挖掘机100的姿态的急变反映到姿态角θo中。接着,说明本实施方式的第二姿态角计算方法。
图26是表示本实施方式的第二姿态角计算处理方法的处理次序的流程图。在步骤S201中,图16所示的IMU29及传感器控制装置24求出姿态角θ。传感器控制装置24的低通滤波器60使从IMU29获取的姿态角θ通过,作为第一姿态角θ1向选择部63输出。第二姿态角运算部50具有的角度运算部50C求出姿态角θ,滤波部50F使姿态角θ通过而作为第二姿态角θ2输出。
在步骤S202中,姿态角判定部62求出从低通滤波器60获取的第一俯仰角θ1p与从第二姿态角运算部50获取的第二俯仰角θ2p的差量Δθp,并将其向选择部63输出。选择部63在差量Δθp比规定的阈值Δθpc小时(步骤S202为“是”),执行步骤S203至步骤S207的处理。步骤S203至步骤S207的处理与姿态角计算方法的第一例的步骤S102至步骤S160的处理相同,因此省略说明。
在差量Δθp为规定的阈值Δθpc以上时(步骤S202为“否”),选择部63在步骤S208中输出第二姿态角θ2作为姿态角θo。接着,在步骤S209中,回旋状态判定部61判定差量Δθp比规定的阈值Δθpc小的状态是否持续时间tc2以上。在差量Δθp比规定的阈值Δθpc小的状态持续时间tc2以上时(步骤S209为“是”),能够判断为是液压挖掘机100的俯仰角θp的急变可允许的范围。因此,选择部63在步骤S210中输出第一姿态角θ1作为姿态角θo。在差量Δθp比规定的阈值Δθpc小的状态未持续时间tc2以上时(步骤S209为“否”),能够判断为液压挖掘机100的俯仰角θp无法允许的急变继续。这种情况下,选择部63返回步骤S208,将第二姿态角θ2作为姿态角θo输出。
例如,在液压挖掘机100向液压挖掘机100接地的地面GD倾斜的方向进行侵入时,俯仰角θp急变。这样的情况下,液压挖掘机100的操作员对作业机2进行操作而使其接地于地面,由此欲抑制液压挖掘机100的姿态的急变。挖掘控制是以避免过度挖入目标挖掘地形73I的方式进行的控制,但是在操作员以使作业机2大幅地超过目标挖掘地形的方式进行操作来抑制液压挖掘机100的姿态的急变时,需要解除挖掘控制而使操作员的操作优先。这种情况下,与挖掘控制相比,作业机2的操作量增大。
第一姿态角θ1是由IMU29求出的姿态角θ通过低通滤波器60而得到的值,因此高频成分减少。因此,在本实施方式中,在操作员操作作业机2而欲抑制液压挖掘机100的姿态的急变的情况下,使用未通过低通滤波器60的第二姿态角θ2而提高动态的响应性,作业机控制装置25能够迅速地解除挖掘控制。
以上,本实施方式通过选择第一姿态角θ1或第二姿态角θ2而能够掌握正确的地形。而且,本实施方式基于液压挖掘机100的倾斜度状态来切换第一姿态角θ2与第二姿态角θ2切换。具体而言,在第一俯仰角θ1p与第二俯仰角θ2p的差量Δθp为规定的阈值以上时,代替第一姿态角θ2而将第二姿态角θ2作为液压挖掘机100的姿态角θo。由此,在液压挖掘机100的姿态急变时,使用动态的响应性比第一姿态角θ1更接近于真正的行为的第二姿态角θ2,因此能够提高控制的响应性,作业机控制装置25能够迅速地解除挖掘控制。因此,液压挖掘机100的操作员对于液压挖掘机100的姿态的急变,通过操作作业机2而能够迅速地应对。
另外,在本实施方式中,在液压挖掘机100静止时,利用通过了低通滤波器60后的第一姿态角θ1来进行挖掘控制及指引图像的显示,在上部回旋体3的回旋中,利用未通过低通滤波器60的第二姿态角θ2来进行挖掘控制及指引图像的显示。因此,在液压挖掘机100静止时,在抑制了铲边8T的位置的细微的变化的状态下计算目标挖掘地形73I,在上部回旋体3回旋时,反映铲边8T的位置的变化而计算目标挖掘地形73I。其结果是,无论是液压挖掘机100静止时还是上部回旋体3回旋时,都能够更可靠地抑制超过目标挖掘地形73I地挖入挖掘对象的情况。
而且,本实施方式使用设定了第一截止频率的第一互补滤波器53和设定了与第一截止频率不同的第二截止频率的第二互补滤波器54。第一互补滤波器53对角速度ω进行积分,由此减小累积的误差(杂音),第二互补滤波器54减小重力加速度以外的加速度引起的误差(杂音)。在本实施方式中,根据液压挖掘机100的状态,来切换第一互补滤波器53输出的倾斜度角与第二互补滤波器54输出的倾斜度角。其结果是,通过与液压挖掘机100的状态对应的适当的互补滤波器来求出第二姿态角θ2,因此无论是动态的状态还是准静止状态都能抑制第二姿态角θ2的精度下降。
精度高的IMU29造价高,廉价的IMU29的精度相对低。本实施方式即便在使用精度低的IMU29时,无论是动态的状态还是准静止状态也能够抑制第二姿态角θ2的精度下降。因此,能够抑制第二姿态角θ2的精度下降,并降低液压挖掘机100的制造成本。
在本实施方式中,使用了第一互补滤波器53及第二互补滤波器54,但还可以追加设定了与第一截止频率及第二截止频率不同的第三截止频率的第三互补滤波器,或者追加设定了与第一截止频率、第二截止频率及第三截止频率不同的第四截止频率的第四互补滤波器。即,截止频率不同的互补滤波器的个数并不限定于2个。
(具备消除离心力的功能的传感器控制装置的例子)
图27是具备消除离心力的功能的传感器控制装置24a的控制框图。图28是用于说明IMU29的安装位置的一例的图。图29是用于说明液压挖掘机100的局部坐标系和IMU29的局部坐标系的图。
该传感器控制装置24a虽然与前述的传感器控制装置24同样,但是考虑了作用于IMU29的重力加速度以外的加速度的影响。即,不同点是由于伴随着IMU29的设置位置而IMU29输出的加速度含有重力加速度以外的分量,因此输出考虑该分量而修正后的加速度。传感器控制装置24a通过输出考虑了IMU29的设置位置的影响的姿态角,由此能够得到更高精度的姿态角。因此,传感器控制装置24a具备加速度修正部56。加速度修正部56设于第二姿态角运算部50a。加速度修正部56对IMU29检测到的液压挖掘机100的加速度Ac进行修正而输出修正加速度Acc。第三姿态角运算部51根据修正加速度Acc来求出姿态角θ。加速度修正部56的修正例如是将伴随着IMU29的设置位置而作用于IMU29的根据离心力求出的加速度(离心加速度)和角加速度等那样的作用于IMU29的重力加速度以外的加速度从IMU29检测到的加速度Ac去除。需要说明的是,伴随着IMU29的设置位置而作用于IMU29的根据离心力求出的加速度及角加速可以通过IMU29以外的检测装置例如加速度计来检测。这种情况下,加速度修正部56从IMU29检测到的液压挖掘机100的加速度Ac中去除加速度计检测到的重力加速度以外的加速度。接着,说明对于前述的传感器控制装置24进行考虑了与IMU29的设置位置相伴的加速度的影响的处理的必要性。
图28示出从x轴方向观察液压挖掘机100的状态。如前述那样,IMU29设置在上部回旋体3的驾驶室4的下方。当以成为上部回旋体3的回旋中心轴的z轴为基准时,IMU29设置在从z轴起沿着x轴方向及y轴方向这两方向离开了规定的距离的位置。具体而言,如图28所示,在从z轴起以规定的距离R为半径的圆周C上设置IMU29。由于在这样的位置设置IMU29,因此当上部回旋体3以z轴为中心回旋时,IMU29受到根据规定的距离R的大小而作用于IMU29的离心加速度和角加速度的影响。其结果是,IMU29输出的加速度Ac受到离心加速度和角加速度的影响。因此,在IMU29检测到的加速度Ac与作用于液压挖掘机100的实际的加速度即为了求出姿态角所需的加速度之间产生背离。只要在上部回旋体3的回旋中心轴即z轴上能够确保设置IMU29的空间,这样的背离就不会发生,因此能够无需考虑该背离地使用前述的传感器控制装置24。然而,由于在实际的液压挖掘机100的回旋中心轴附近设置回旋马达等,因此无法充分确保设置IMU29的空间。因此,在这样的液压挖掘机100的情况下,必须将IMU29设置在远离z轴的位置。因此,需要如下说明详细情况的变形例的传感器控制装置24a。
如图29所示,在从液压挖掘机100的局部坐标系(x,y,z)的z轴起沿着x轴方向及y轴方向这两方向分别分离了规定的距离的位置、即从z轴分离了距离R的位置处,存在IMU29的局部坐标系(xi,yi,zi)。在本实施方式中,IMU29的局部坐标系的zi轴(垂直轴)例如通过IMU29的重心位置。IMU29受到的重力加速度以外的加速度成为前述的离心加速度及角加速度,因此通过从IMU29检测到的加速度Ac除去这些加速度的分量,由此能够求出作用于液压挖掘机100的加速度即姿态角的运算所需的加速度。
当将液压挖掘机100的局部坐标系的绕z轴的角速度(回旋速度)设为ωz时,作用于IMU29的离心加速度成为R×ωz2。角速度(回旋速度)ωz是从IMU29输出的Zi轴方向的角速度。而且,作用于IMU29的角加速度通过以时间t对角速度(回旋速度)ωz进行微分而能够求出。即,成为角加速度=dωz/dt。关于IMU29检测到的加速度Ac,IMU29的局部坐标系的xi轴方向的加速度为Acx,yi轴方向的加速度为Acy。加速度Acx及加速度Acy是作用于液压挖掘机100的加速度即姿态角的运算所需的加速度。
另外,关于IMU29检测的加速度Ac,当液压挖掘机100的局部坐标系中的x轴方向的加速度分量为Accx、y轴方向的加速度分量为Accy时,它们可以分别由式(11)及式(12)表示。IMU29检测的zi轴方向的加速度作用于IMU29,不会因根据离心力所求出的加速度(离心加速度)等的有无而发生变化,IMU29检测到的zi轴方向的加速度成为液压挖掘机100的z轴方向的加速度。
Accx=Acx-R×ωz2×cosα-R×(dωz/dt)×sinα···(11)
Accy=Acy-R×ωz2×sinα+R×(dωz/dt)×cosα···(12)
在式(11)的右边,将加速度Acx以外的分量作为排除分量。在式(12)的右边将加速度Acy以外的分量作为排除分量。排除分量是根据离心力来求出的与加速度(离心加速度)及角加速度相关的分量。具体而言,根据离心力来求出的与加速度(离心加速度)相关的分量在式(11)中为R×ωz2×cosα,在式(12)中为R×ωz2×sinα。而且,与角加速度相关的分量在式(11)中为R×(dωz/dt)×sinα,在式(12)中为R×(dωz/dt)×cosα。
式(11)及式(12)中的α是通过液压挖掘机100的局部坐标系的y轴与IMU29的设置位置即圆周C上的点处的切线而形成的角度。该角度为设置角α。设置角α在液压挖掘机100的局部坐标系(x,y,z)中表示设置IMU29的位置的倾斜度。如前述那样,加速度Acx及加速度Acy是作用于液压挖掘机100的加速度即姿态角的运算所需的加速度。加速度从式(11)或式(12)可知,Acx及加速度Acy通过从IMU29检测到的x轴方向的加速度分量Accx或y轴方向的加速度分量Accy分别去除前述的排除分量的修正能够求出。
加速度Acx及加速度Acy分别是xi轴方向及yi轴方向的加速度。当重力加速度设为G时,加速度Acx及加速度Acy分别如式(13)及式(14)所示。
Acx=G×sin(γy)···(13)
Acy=-G×sin(γx)×cos(γy)···(14)
在此,γx是绕xi轴的倾滚角,γy是绕yi轴的俯仰角。倾滚角γx及俯仰角γy是IMU29的局部坐标系(xi,yi,zi)中的z轴即垂直轴以外的倾斜度角。在IMU29不回旋时,即,重力加速度以外的加速度未作用于IMU29时,加速度Acx及加速度Acy与IMU29检测的加速度分量Accx及加速度分量Accy相同。若得到加速度Acx及加速度Acy,则根据式(13)及式(14)能求出倾滚角γx及俯仰角γy。
以下,在从IMU29输出的加速度分量Accx及加速度分量Accy未进行区别时,称为被修正加速度Accd。作用于液压挖掘机100的加速度即姿态角的运算所需的加速度即加速度Acx及加速度Acy未进行区别时,称为加速度Ac。
图27所示的加速度修正部56如前述那样基于IMU29的信息来修正IMU29检测到的被修正加速度Accd(加速度Accx、Accy)。IMU29的信息包括设置IMU29的位置的信息,例如是包括于式(11)及式(12)的信息。在本实施方式中,IMU29的信息是倾滚角γx、俯仰角γy、表示设置IMU29的位置的设置角α、以液压挖掘机100的局部坐标系(x,y,z)的z轴为基准的到设置IMU29的场所为止的距离R、及绕着液压挖掘机100的局部坐标系的z轴即垂直轴的角速度ωz。
如以上说明那样,图27所示的加速度修正部56使用式(11)及式(12)对IMU29检测到的加速度Acc进行修正,求出加速度Acx、Acy。加速度Acx、Acy不包括因IMU29绕z轴回旋而产生的离心加速度和角加速度的分量,因此加速度修正部56能够输出与IMU29设置在回旋中心轴上时同样的加速及角速度。因此,传感器控制装置24a输出的姿态角θo的精度提高。而且,传感器控制装置24a无论液压挖掘机100的动作状态如何,都能够计算准确的回旋角度。其结果是,图2所示的作业机控制装置25能够更高精度地计算上部回旋体3回旋时的铲斗8的铲边8T的位置。
回旋速度ωz使用IMU29检测到的xi-yi平面内的角速度,但检测回旋速度ωz的情况并不限定于IMU29。例如,可以将检测上部回旋体3的旋转角度的旋转角度检测装置的检测值作为回旋速度ωz,或者基于使上部回旋体3旋转的回旋马达的转速来求出回旋速度ωz。
需要说明的是,在IMU29无法设置在液压挖掘机100的回旋中心轴上时,为了更高精度地计算作业机2具备的铲斗8的铲边8T的位置,与先前叙述的实施方式的传感器控制装置24相比,更优选使用作为变形例说明的传感器控制装置24a。这是因为作为变形例说明的传感器控制装置24a进行如前述那样考虑了IMU29的设置位置的处理的缘故。
(传感器控制装置的第一变形例)
图30是第一变形例的传感器控制装置24b的控制框图。在本变形例中,图15所示的IMU29的姿态角运算部29CP作为第一姿态角运算部发挥功能,根据通过作为检测装置的回转仪29V及加速度传感器29A而检测到的角速度ω及加速度Ac来求出作业机械的姿态角θ并向低通滤波器60输入。IMU29的检测值经由车内信号线42向传感器控制装置24b输入。传感器控制装置24b被从IMU29输入角速度ω、加速度Ac及姿态角θ。传感器控制装置24b包括第二姿态角运算部50b、低通滤波器60、选择部63。此外,传感器控制装置24b包括回旋状态判定部61和姿态角判定部62。
第二姿态角运算部50b包括角度运算部50Cb和滤波部50Fb。角度运算部50Cb根据由图3所示的IMU29的回转仪29V及加速度传感器29A检测到的角速度ω及加速度Ac来求出姿态角θ。传感器控制装置24b也可以具有第二变形例的传感器控制装置24a所具备的加速度修正部56。
作为第二滤波器的滤波部50Fb在使角度运算部50Cb求出的姿态角θ通过而减少了杂音之后,将其作为第二姿态角θ2而输出。滤波部50Fb比低通滤波器60的截止频率高。第二姿态角运算部50b输出的第二姿态角θ2未通过低通滤波器60而向选择部63输入。传感器控制装置24b具备的滤波部50Fb是比前述的传感器控制装置24具备的滤波部50F简单的结构,因此传感器控制装置24b具有制造成本降低这样的优点。
在本变形例中,第二姿态角运算部50b可以不具备滤波部50Fb。这种情况下,通过角度运算部50Cb求出的姿态角θ作为第二姿态角θ2向姿态角判定部62及选择部63输入。
(传感器控制装置的第二变形例)
图31是第二变形例的传感器控制装置24c的框图。不同点是该传感器控制装置24c不具备图30所示的传感器控制装置24b的第二姿态角运算部50b而将IMU29的图15所示的姿态角运算部29CP求出的姿态角θ作为第二姿态角θ2向选择部63直接输入。传感器控制装置24c的低通滤波器60对IMU29的姿态角运算部29CP求出的姿态角θ进行滤波处理,作为第一姿态角θ1求出并将其向选择部63输出。在IMU29的姿态角运算部29CP中,姿态角θ作为第二姿态角θ2。第二姿态角θ2未通过低通滤波器60而向选择部63输入。传感器控制装置24c不具备第二姿态角运算部50b,因此相应地结构简化且制造成本降低。
以上,说明了本实施方式及其变形例,但没有通过前述的内容来限定本实施方式及其变形例。而且,前述的构成要素包括本领域技术人员能够容易想到的要素、实质上相同的要素、所谓等同的范围的要素。而且,前述的构成要素可以适当组合。此外,在不脱离本实施方式及其变形例的主旨的范围内能够进行构成要素的各种省略、置换及变更中的至少1个。例如,作业机2具有动臂6、斗杆7及作为作业用具的铲斗8,但装配于作业机2的作业用具并不局限于此,并不限定于铲斗8。作业机械并不限定于液压挖掘机100,只要是例如在下部行驶体上具有回旋体的作业机械即可。传感器控制装置24、24a、24b、24c执行的各处理可以由其他的控制器、例如第二显示装置39或作业机控制装置25进行处理。姿态角通过的滤波器并不限定于互补滤波器,也可以是其他的方式的滤波器。挖掘控制并不限定于前述的控制。
附图标记说明如下:
1 车辆主体
2 作业机
3 上部回旋体
5 行驶装置
6 动臂
7 斗杆
8 铲斗
8T 铲边
20、21 天线
23 全局坐标运算部
24、24a、24b、24c 传感器控制装置
25 作业机控制装置
26 发动机控制装置
27 泵控制装置
28 第一显示装置
29 IMU
29V 回转仪
29A 加速度传感器
29CP 姿态角运算部
29PT 物理量转换部
39 第二显示装置
41、42 车内信号线
50、50a 第二姿态角运算部
50C 角度运算部
50F、50Fa、50Fb 滤波部
51 第三姿态角运算部
52 第四姿态角运算部
53 第一互补滤波器
54 第二互补滤波器
55 切换部
60 低通滤波器
61 回旋状态判定部
62 姿态角判定部
63 选择部
100 液压挖掘机
θ1 第一姿态角
θ2 第二姿态角
θ3 第三姿态角
θ4 第四姿态角
Claims (15)
1.一种作业机械的姿态运算装置,该作业机械的姿态运算装置用于求出具备作业机的作业机械的姿态角,其中,
所述作业机械的姿态运算装置包括:
检测装置,其设于所述作业机械,并用于检测角速度及加速度;
第一姿态角运算部,其设于所述检测装置,并根据由所述检测装置检测到的所述角速度及所述加速度来求出所述作业机械的姿态角;
低通滤波器,其使所述第一姿态角运算部求出的姿态角通过并将该姿态角作为使高频成分减少后的第一姿态角输出;
第二姿态角运算部,其将根据由所述检测装置检测到的所述角速度及所述加速度来求出的姿态角作为对所述作业机械的姿态变化的响应性比所述第一姿态角高的第二姿态角输出;
选择部,其求出所述作业机械的姿态角的变动量,基于所述姿态角的变动量而进行所述作业机械的姿态变动的判定,在所述姿态角的变动量为规定的阈值以下的情况下输出所述第一姿态角,在所述姿态角的变动量超出规定的阈值的情况下输出所述第二姿态角。
2.根据权利要求1所述的作业机械的姿态运算装置,其中,
所述第二姿态角运算部包括:
第一互补滤波器,其具有第一低通滤波器、第一高通滤波器、以及将所述第一低通滤波器的输出和所述第一高通滤波器的输出相加而输出的第一加法部,且设定所述第一低通滤波器的增益和所述第一高通滤波器的增益成为规定值时的第一截止频率,通过使由所述检测装置检测出的所述角速度输入至所述第一低通滤波器,使根据所述加速度求出的所述姿态角输入至所述第一高通滤波器,来减少根据由所述检测装置检测到的所述角速度及所述加速度来求出的所述姿态角中含有的杂音,并从所述第一加法部输出第三姿态角;
第二互补滤波器,其具有第二低通滤波器、第二高通滤波器、以及将所述第二低通滤波器的输出和所述第二高通滤波器的输出相加而输出的第二加法部,且设定所述第二低通滤波器的增益和所述第二高通滤波器的增益成为规定值时的、与所述第一截止频率不同的第二截止频率,通过使由所述检测装置检测出的所述角速度输入至所述第二低通滤波器,使根据所述加速度求出的所述姿态角输入至所述第二高通滤波器,来减少根据由所述检测装置检测到的所述角速度及所述加速度来求出的所述姿态角中含有的杂音,并从所述第二加法部输出第四姿态角;
切换部,其根据所述作业机械的动作的状态,切换所述第三姿态角或所述第四姿态角并将其作为所述第二姿态角输出。
3.根据权利要求1所述的作业机械的姿态运算装置,其中,
所述第二姿态角是所述作业机械的姿态角通过了截止频率比所述低通滤波器高的滤波器后的姿态角。
4.根据权利要求2所述的作业机械的姿态运算装置,其中,
所述第二姿态角是所述作业机械的姿态角通过了截止频率比所述低通滤波器高的滤波器后的姿态角。
5.一种作业机械的姿态运算装置,该作业机械的姿态运算装置用于求出具备作业机的作业机械的姿态角,其中,
所述作业机械的姿态运算装置包括:
检测装置,其设于所述作业机械,并用于检测角速度及加速度;
姿态角运算部,其设于所述检测装置,并根据由所述检测装置检测到的所述角速度及所述加速度来求出所述作业机械的姿态角;
低通滤波器,其对由所述姿态角运算部求出的姿态角进行滤波处理,并将该进行了滤波处理的姿态角作为使高频成分减少后的第一姿态角输出;
选择部,其在所述姿态角运算部中将所述姿态角作为对所述作业机械的姿态变化的响应性比所述第一姿态角高的第二姿态角,求出所述作业机械的姿态角的变动量,基于所述姿态角的变动量而进行所述作业机械的姿态变动的判定,在所述作业机械的姿态角的变动量为规定的阈值以下的情况下输出所述第一姿态角,在所述作业机械的姿态角的变动量超出规定的阈值的情况下输出所述第二姿态角。
6.根据权利要求1至5中任一项所述的作业机械的姿态运算装置,其中,
与所述姿态角的变动相关的信息是所述作业机械的与回旋相关的信息。
7.根据权利要求6所述的作业机械的姿态运算装置,其中,
所述作业机械具备行驶体和在所述行驶体的上部设置的回旋体,
所述作业机械的与回旋相关的信息是所述回旋体的回旋速度,
在所述回旋速度为规定的阈值以下时,所述选择部输出所述第一姿态角,在所述回旋速度超过了规定的阈值时,所述选择部输出所述第二姿态角。
8.根据权利要求1至5中任一项所述的作业机械的姿态运算装置,其中,
在所述第一姿态角与所述第二姿态角的差量超过了规定的阈值时,所述选择部输出所述第二姿态角。
9.根据权利要求7所述的作业机械的姿态运算装置,其中,
在所述回旋体的回旋速度为规定的阈值以下时,所述选择部输出所述第一姿态角,在所述回旋速度超过了规定的阈值时,所述选择部输出所述第二姿态角,在所述第一姿态角与所述第二姿态角的差量超过了规定的阈值时,所述选择部输出所述第二姿态角。
10.根据权利要求1至5中任一项所述的作业机械的姿态运算装置,其中,
所述姿态角是所述作业机械的俯仰角。
11.一种作业机械,其中,
所述作业机械具备权利要求1至10中任一项所述的作业机械的姿态运算装置,
所述作业机械使用从所述作业机械的姿态运算装置输出的所述第一姿态角或所述第二姿态角来求出所述作业机械的至少一部分的位置信息。
12.根据权利要求11所述的作业机械,其中,
所述作业机械具有:
位置检测装置,其用于检测所述作业机械的位置信息;
目标挖掘地形生成装置,其基于从所述作业机械的姿态运算装置输出的所述作业机械的至少一部分的位置信息和由所述位置检测装置检测到的位置信息来求出所述作业机的位置,并根据表示目标形状的目标施工面的信息来生成与表示所述作业机的挖掘对象的目标形状的目标挖掘地形相关的信息。
13.根据权利要求12所述的作业机械,其中,
所述作业机械具有显示装置,该显示装置基于用于显示所述目标挖掘地形的显示用的信息来显示所述目标挖掘地形。
14.根据权利要求12或13所述的作业机械,其中,
所述作业机械具有作业机控制部,该作业机控制部基于从所述目标挖掘地形生成装置获取的与所述目标挖掘地形相关的信息,执行以使所述作业机接近挖掘对象的方向的速度成为限制速度以下的方式进行控制的挖掘控制。
15.一种作业机械的姿态运算方法,该作业机械的姿态运算方法用于求出具备作业机的作业机械的姿态角,其中,
使所述作业机械的姿态角通过低通滤波器并将该姿态角作为使高频成分减少后的第一姿态角输出,并将所述作业机械的姿态角作为不通过所述低通滤波器而对所述作业机械的姿态变化的响应性比所述第一姿态角高的第二姿态角输出,
求出所述作业机械的姿态角的变动量,基于所述姿态角的变动量而进行所述作业机械的姿态变动的判定,在所述姿态角的变动量为规定的阈值以下的情况下输出所述第一姿态角,在所述姿态角的变动量超出规定的阈值的情况下输出所述第二姿态角。
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
PCT/JP2014/064891 WO2015186214A1 (ja) | 2014-06-04 | 2014-06-04 | 作業機械の姿勢演算装置、作業機械及び作業機械の姿勢演算方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN105358769A CN105358769A (zh) | 2016-02-24 |
CN105358769B true CN105358769B (zh) | 2017-10-03 |
Family
ID=54766311
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201480000981.8A Active CN105358769B (zh) | 2014-06-04 | 2014-06-04 | 作业机械的姿态运算装置、作业机械及作业机械的姿态运算方法 |
Country Status (6)
Country | Link |
---|---|
US (1) | US9739038B2 (zh) |
JP (1) | JP5893144B1 (zh) |
KR (1) | KR101800103B1 (zh) |
CN (1) | CN105358769B (zh) |
DE (1) | DE112014000134B4 (zh) |
WO (1) | WO2015186214A1 (zh) |
Families Citing this family (41)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
AU2012202213B2 (en) * | 2011-04-14 | 2014-11-27 | Joy Global Surface Mining Inc | Swing automation for rope shovel |
JP6419585B2 (ja) * | 2015-01-13 | 2018-11-07 | 株式会社小松製作所 | 掘削機械、掘削機械の制御方法及び掘削システム |
US10415211B2 (en) * | 2015-02-23 | 2019-09-17 | Komatsu Ltd. | Hydraulic excavator having handrail and camera |
US9871968B2 (en) * | 2015-05-22 | 2018-01-16 | Caterpillar Inc. | Imaging system for generating a surround-view image |
JP6619163B2 (ja) * | 2015-06-17 | 2019-12-11 | 日立建機株式会社 | 作業機械 |
JP6707344B2 (ja) * | 2015-12-25 | 2020-06-10 | 株式会社小松製作所 | 作業車両および作業車両の制御方法 |
JP6447530B2 (ja) * | 2016-01-29 | 2019-01-09 | オムロン株式会社 | 信号処理装置、信号処理装置の制御方法、制御プログラム、および記録媒体 |
JP6691482B2 (ja) * | 2016-02-08 | 2020-04-28 | 株式会社小松製作所 | 作業車両および動作制御方法 |
CN105903590B (zh) * | 2016-06-12 | 2018-12-11 | 成都多来咪智能科技有限公司 | 一种喷洒流量自动控制*** |
JP6618856B2 (ja) * | 2016-06-21 | 2019-12-11 | 株式会社小松製作所 | 施工システム及び施工方法 |
CA3111350A1 (en) * | 2016-07-20 | 2018-01-25 | Prinoth Ltd | Tracked vehicle with rotating upper structure and processes therefor |
US20180110190A1 (en) * | 2016-10-20 | 2018-04-26 | Deere & Company | Work vehicle gyroscopic boom control system and method |
CN106522303B (zh) * | 2016-12-07 | 2018-10-09 | 烟台职业学院 | 一种推耙机推耙铲姿态参数确定方法 |
US10132060B2 (en) * | 2017-02-27 | 2018-11-20 | Caterpillar Inc. | Implement orientation by image processing |
JP6901336B2 (ja) * | 2017-07-14 | 2021-07-14 | 株式会社小松製作所 | 建設機械、較正システム、および方法 |
US9943022B1 (en) * | 2017-08-02 | 2018-04-17 | Caterpillar Trimble Control Technologies Llc | Determining yaw and center-of-rotation of a rotating platform using a single position sensor |
EP3450634B1 (en) * | 2017-08-30 | 2021-03-03 | Topcon Positioning Systems, Inc. | Method and apparatus for machine operator command attenuation |
JP6817457B2 (ja) * | 2018-03-15 | 2021-01-20 | 日立建機株式会社 | 作業機械 |
JP6782271B2 (ja) * | 2018-03-15 | 2020-11-11 | 日立建機株式会社 | 作業機械 |
WO2019180894A1 (ja) * | 2018-03-22 | 2019-09-26 | 日立建機株式会社 | 作業機械 |
GB2573304A (en) * | 2018-05-01 | 2019-11-06 | Caterpillar Inc | A method of operating a machine comprising am implement |
JP7143117B2 (ja) * | 2018-05-22 | 2022-09-28 | 住友重機械工業株式会社 | ショベル |
JP7086764B2 (ja) * | 2018-07-12 | 2022-06-20 | 日立建機株式会社 | 作業機械 |
JP7274831B2 (ja) * | 2018-07-31 | 2023-05-17 | 株式会社小松製作所 | 作業機械 |
JP7096105B2 (ja) * | 2018-08-23 | 2022-07-05 | 株式会社神戸製鋼所 | 掘削作業機械の油圧駆動装置 |
JP7082011B2 (ja) * | 2018-08-23 | 2022-06-07 | 株式会社神戸製鋼所 | 掘削作業機械の油圧駆動装置 |
JP7301514B2 (ja) * | 2018-09-21 | 2023-07-03 | 日立建機株式会社 | 座標変換システム及び作業機械 |
CN109141417B (zh) * | 2018-10-26 | 2022-02-15 | 中国航空工业集团公司北京航空精密机械研究所 | 一种可折叠电动锁紧装置 |
JP7202193B2 (ja) * | 2019-01-18 | 2023-01-11 | 株式会社小松製作所 | 作業機械の制御装置、作業機械、及び作業機械の制御方法 |
US11333767B2 (en) | 2019-04-03 | 2022-05-17 | Caterpillar Inc. | Avoidance modifier system for collision avoidance system |
US10949685B2 (en) | 2019-07-22 | 2021-03-16 | Caterpillar Inc. | Excluding a component of a work machine from a video frame based on motion information |
US11697917B2 (en) * | 2019-07-26 | 2023-07-11 | Deere & Company | Anticipatory modification of machine settings based on predicted operational state transition |
JP7282002B2 (ja) * | 2019-09-10 | 2023-05-26 | ナブテスコ株式会社 | 情報処理装置、情報処理システム、情報処理方法、コンピュータプログラム、建設機械 |
US11828040B2 (en) * | 2019-09-27 | 2023-11-28 | Topcon Positioning Systems, Inc. | Method and apparatus for mitigating machine operator command delay |
US11408449B2 (en) | 2019-09-27 | 2022-08-09 | Topcon Positioning Systems, Inc. | Dithering hydraulic valves to mitigate static friction |
US11846091B2 (en) * | 2020-01-28 | 2023-12-19 | Topcon Positioning Systems, Inc. | System and method for controlling an implement on a work machine using machine vision |
TWI720923B (zh) | 2020-07-23 | 2021-03-01 | 中強光電股份有限公司 | 定位系統以及定位方法 |
CN112526986B (zh) * | 2020-10-28 | 2022-11-22 | 苏州极目机器人科技有限公司 | 跟垄作业方法和装置 |
US20220374011A1 (en) * | 2021-05-18 | 2022-11-24 | Clark Equipment Company | Modulating operator input for work element actuator operation |
US11834813B2 (en) * | 2021-07-09 | 2023-12-05 | Topcon Positioning Systems, Inc. | IMU based system for vertical axis joint angle estimation for swing boom excavators |
CN114411843B (zh) * | 2021-12-31 | 2023-03-14 | 太原重工股份有限公司 | 正铲式挖掘机提升及推压机构的零位标定装置和方法 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN1952827A (zh) * | 2006-09-21 | 2007-04-25 | 上海大学 | 小型机载云台跟踪控制装置 |
CN101232979A (zh) * | 2005-08-01 | 2008-07-30 | 丰田自动车株式会社 | 移动体姿态角检测设备 |
CN201285289Y (zh) * | 2008-11-05 | 2009-08-05 | 北京宇航***工程研究所 | 飞行器测量***用姿态角相敏解调测量装置 |
Family Cites Families (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR950008533B1 (ko) * | 1991-11-30 | 1995-07-31 | 삼성중공업주식회사 | 유압펌프의 토출유량 제어장치 |
JPH0657782A (ja) * | 1992-08-10 | 1994-03-01 | Mitsubishi Heavy Ind Ltd | ブルドーザのブレード自動制御装置 |
KR950001446A (ko) * | 1993-06-30 | 1995-01-03 | 경주현 | 굴삭기의 자동 반복작업 제어방법 |
EP0707118B1 (en) * | 1994-04-28 | 1999-07-28 | Hitachi Construction Machinery Co., Ltd. | Aera limiting digging control device for a building machine |
US5704429A (en) * | 1996-03-30 | 1998-01-06 | Samsung Heavy Industries Co., Ltd. | Control system of an excavator |
US6098322A (en) * | 1996-12-12 | 2000-08-08 | Shin Caterpillar Mitsubishi Ltd. | Control device of construction machine |
JP2007147588A (ja) | 2005-11-01 | 2007-06-14 | Hitachi Constr Mach Co Ltd | 作業機械の位置計測システム |
JP4807301B2 (ja) | 2007-03-30 | 2011-11-02 | 日本電気株式会社 | 姿勢角計測装置及び該姿勢角計測装置に用いられる姿勢角計測方法 |
US9206589B2 (en) * | 2009-03-31 | 2015-12-08 | Caterpillar Inc. | System and method for controlling machines remotely |
DE112012001013B4 (de) | 2011-03-24 | 2019-01-03 | Komatsu Ltd. | Aushubsteuersystem und Baumaschine |
US8909437B2 (en) * | 2012-10-17 | 2014-12-09 | Caterpillar Inc. | Payload Estimation system |
US9411325B2 (en) * | 2012-10-19 | 2016-08-09 | Komatsu Ltd. | Excavation control system for hydraulic excavator |
-
2014
- 2014-06-04 DE DE112014000134.1T patent/DE112014000134B4/de active Active
- 2014-06-04 WO PCT/JP2014/064891 patent/WO2015186214A1/ja active Application Filing
- 2014-06-04 CN CN201480000981.8A patent/CN105358769B/zh active Active
- 2014-06-04 KR KR1020157031800A patent/KR101800103B1/ko active IP Right Grant
- 2014-06-04 JP JP2014529355A patent/JP5893144B1/ja active Active
- 2014-06-04 US US14/390,157 patent/US9739038B2/en active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101232979A (zh) * | 2005-08-01 | 2008-07-30 | 丰田自动车株式会社 | 移动体姿态角检测设备 |
CN1952827A (zh) * | 2006-09-21 | 2007-04-25 | 上海大学 | 小型机载云台跟踪控制装置 |
CN201285289Y (zh) * | 2008-11-05 | 2009-08-05 | 北京宇航***工程研究所 | 飞行器测量***用姿态角相敏解调测量装置 |
Also Published As
Publication number | Publication date |
---|---|
DE112014000134B4 (de) | 2016-09-22 |
US9739038B2 (en) | 2017-08-22 |
WO2015186214A1 (ja) | 2015-12-10 |
JPWO2015186214A1 (ja) | 2017-04-20 |
US20160251836A1 (en) | 2016-09-01 |
CN105358769A (zh) | 2016-02-24 |
JP5893144B1 (ja) | 2016-03-23 |
KR101800103B1 (ko) | 2017-11-21 |
DE112014000134T5 (de) | 2016-04-07 |
KR20160006691A (ko) | 2016-01-19 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN105358769B (zh) | 作业机械的姿态运算装置、作业机械及作业机械的姿态运算方法 | |
CN105339561B (zh) | 作业机械的姿势运算装置、作业机械及作业机械的姿势运算方法 | |
CN105408554B (zh) | 作业机械的控制***、作业机械以及作业机械的控制方法 | |
CN105008623B (zh) | 建筑机械的控制***、建筑机械及建筑机械的控制方法 | |
CN106068354B (zh) | 作业机械的控制装置、作业机械和作业机械的控制方法 | |
CN105636659B (zh) | 作业机械的控制***、作业机械、液压挖掘机的控制***以及作业机械的控制方法 | |
CN105339558B (zh) | 建筑机械的控制***、建筑机械及建筑机械的控制方法 | |
CN104246085B (zh) | 挖掘机械的施工管理装置、液压挖掘机的施工管理装置、挖掘机械以及施工管理*** | |
KR101764521B1 (ko) | 작업 기계의 자세 연산 장치, 유압 셔블의 자세 연산 장치 및 작업 기계 | |
CN105339560B (zh) | 挖掘机械的控制***以及挖掘机械 | |
CN112424430B (zh) | 控制装置、装载机械及控制方法 | |
CN107306500A (zh) | 作业机械的控制装置、作业机械以及作业机械的控制方法 | |
CN103857852A (zh) | 挖掘机械的显示***及挖掘机械 | |
CN108779614A (zh) | 作业机械 | |
CN109511267A (zh) | 作业机械以及作业机械的控制方法 | |
CN106460362A (zh) | 工程机械的控制***、工程机械、以及工程机械的控制方法 | |
CN111094666B (zh) | 作业机械 | |
KR20190109744A (ko) | 작업 기계 | |
KR20210122246A (ko) | 쇼벨 | |
KR20230033461A (ko) | 건설기계 | |
CN109511269A (zh) | 作业机械以及作业机械的控制方法 | |
JP2018021351A (ja) | 施工管理システム、作業機械、及び施工管理方法 | |
CN109511268A (zh) | 作业机械以及作业机械的控制方法 | |
CN113454292A (zh) | 挖土机 | |
WO2022168447A1 (ja) | 自動作業システム |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |