CN116964539A - 系统、方法以及作业车辆 - Google Patents
系统、方法以及作业车辆 Download PDFInfo
- Publication number
- CN116964539A CN116964539A CN202280019618.5A CN202280019618A CN116964539A CN 116964539 A CN116964539 A CN 116964539A CN 202280019618 A CN202280019618 A CN 202280019618A CN 116964539 A CN116964539 A CN 116964539A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- constraint condition
- path
- track
- posture
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims description 44
- 238000004364 calculation method Methods 0.000 claims description 22
- 238000010845 search algorithm Methods 0.000 claims description 18
- 230000036544 posture Effects 0.000 description 38
- 230000006870 function Effects 0.000 description 20
- 238000001514 detection method Methods 0.000 description 15
- 238000010586 diagram Methods 0.000 description 12
- 238000005259 measurement Methods 0.000 description 12
- 230000005540 biological transmission Effects 0.000 description 10
- 238000011156 evaluation Methods 0.000 description 8
- 239000002689 soil Substances 0.000 description 6
- 238000012545 processing Methods 0.000 description 5
- 230000001133 acceleration Effects 0.000 description 4
- 238000004891 communication Methods 0.000 description 4
- 244000273256 Phragmites communis Species 0.000 description 2
- 235000014676 Phragmites communis Nutrition 0.000 description 2
- 238000009412 basement excavation Methods 0.000 description 2
- 230000000052 comparative effect Effects 0.000 description 2
- 230000007423 decrease Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000033001 locomotion Effects 0.000 description 2
- 238000013178 mathematical model Methods 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000008602 contraction Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000009472 formulation Methods 0.000 description 1
- 239000010720 hydraulic oil Substances 0.000 description 1
- 238000012886 linear function Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 239000000203 mixture Substances 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000004549 pulsed laser deposition Methods 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 239000004575 stone Substances 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/20—Control system inputs
- G05D1/22—Command input arrangements
- G05D1/229—Command input data, e.g. waypoints
- G05D1/2295—Command input data, e.g. waypoints defining restricted zones, e.g. no-flight zones or geofences
-
- 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/2025—Particular purposes of control systems not otherwise provided for
- E02F9/205—Remotely operated machines, e.g. unmanned vehicles
-
- 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/60—Intended control result
- G05D1/617—Safety or protection, e.g. defining protection zones around obstacles or avoiding hazards
- G05D1/622—Obstacle avoidance
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/60—Intended control result
- G05D1/644—Optimisation of travel parameters, e.g. of energy consumption, journey time or distance
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D2105/00—Specific applications of the controlled vehicles
- G05D2105/05—Specific applications of the controlled vehicles for soil shifting, building, civil engineering or mining, e.g. excavators
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D2107/00—Specific environments of the controlled vehicles
- G05D2107/20—Land use
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D2107/00—Specific environments of the controlled vehicles
- G05D2107/70—Industrial sites, e.g. warehouses or factories
- G05D2107/73—Mining
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D2109/00—Types of controlled vehicles
- G05D2109/10—Land vehicles
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Aviation & Aerospace Engineering (AREA)
- Structural Engineering (AREA)
- General Engineering & Computer Science (AREA)
- Civil Engineering (AREA)
- Mining & Mineral Resources (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Operation Control Of Excavators (AREA)
Abstract
路径搜索部基于表示障碍物的位置的第一制约条件,搜索用于从车辆的初始位置向目标位置移动的路径、即以车辆的位置及姿势为要素的系列。制约条件生成部生成惩罚值根据距路径的偏离距离而增加的第二制约条件。轨道搜索部基于第二制约条件,搜索用于从车辆的初始位置向目标位置移动的轨道、即以车辆的位置、姿势、速度以及转向角为要素的系列。
Description
技术领域
本公开涉及系统、方法以及作业车辆。
本申请对2021年3月31日在日本申请的特愿2021-061381号主张优先权,并将其内容援引于此。
背景技术
在碎石现场、矿山等作业现场中,有时会导入作业车辆的自动驾驶系统。自动驾驶系统计算用于使作业车辆到达目的位置的轨道,并基于该轨道使作业车辆行驶。在专利文献1中公开了无人车辆的行驶路径生成方法所涉及的技术。
在先技术文献
专利文献
专利文献1:日本特开2010-073080号公报
发明内容
发明要解决的课题
用于供车辆自动行驶的轨道计算是优化问题。另外,在轨道的计算中,存在于作业现场的障碍物作为制约条件而给出。然而,用于轨道的搜索的数理模型由车辆的位置、姿势、转向角、速度、加速度等多维的向量表示,另外,存在障碍物的位置及形状未必简单而制约条件的非线性变高的可能性。因此,就轨道的计算而言,计算量变多,或者产生向局部解的收敛,因此难以在线地计算车辆的行驶轨道。另一方面,若为了削减计算量而降低模型的维数,则在所得到的轨道中,消减后的维度所涉及的动作有可能不连续。
本公开的目的在于提供能够以较少的计算量得到连续的轨道的轨道搜索装置以及轨道搜索方法。
用于解决课题的方案
本发明的第一方案是搜索用于供车辆自动行驶的轨道的系统,且具备处理器。处理器具备:路径搜索部,其基于表示障碍物的位置的第一制约条件,搜索用于从车辆的初始位置向目标位置移动的路径、即以车辆的位置及姿势为要素的系列;制约条件生成部,其生成惩罚值根据距路径的偏离距离而增加的第二制约条件;轨道搜索部,其基于第二制约条件,搜索用于从车辆的初始位置向目标位置移动的轨道、即以车辆的位置、姿势、速度以及转向角为要素的系列。
本发明的第二方案是搜索用于供车辆自动行驶的轨道的方法。该方法包括以下的处理。第一处理基于表示障碍物的位置的第一制约条件,搜索用于从车辆的初始位置向目标位置移动的路径、即以所述车辆的位置及姿势为要素的系列。第二处理生成惩罚值根据距路径的偏离距离而增加的第二制约条件。第三处理基于第二制约条件,搜索用于从车辆的初始位置向目标位置移动的轨道、即以车辆的位置、姿势、速度以及转向角为要素的系列。
本发明的第三方案是作业车辆,且具备:车身;行驶装置,其用于使车身行驶;以及处理器。处理器具备:路径搜索部,其基于表示障碍物的位置的第一制约条件,搜索用于从作业车辆的初始位置向目标位置移动的路径、即以车辆的位置及姿势为要素的系列;制约条件生成部,其生成惩罚值根据距路径的偏离距离而增加的第二制约条件;轨道搜索部,其基于第二制约条件,搜索用于从作业车辆的初始位置向目标位置移动的轨道、即以作业车辆的位置、姿势、速度以及转向角为要素的系列;以及行驶控制部,其根据轨道而控制所述行驶装置。
发明效果
根据上述方式的至少一个,轨道搜索装置能够以较少的计算量得到连续的轨道。
附图说明
图1是第一实施方式的作业车辆的侧视图。
图2是示出第一实施方式的控制装置的结构的概要框图。
图3是示出第一实施方式的映射数据的一例的图。
图4是示出第一实施方式的容许范围系列的例子的图。
图5是示出第一实施方式的容许范围系列的生成方法的流程图。
图6是示出第一实施方式的第一矩形及第二矩形的关系的图。
图7是示出第一实施方式的控制装置的自动行驶处理的流程图。
图8是示出第一实施方式的模型中的时间刻度宽度与计算结果的位置的关系的图。
图9是示出比较例的模型中的时间刻度宽度与计算结果的位置的关系的图。
图10是示出其他实施方式的自动驾驶系统的结构的概要图。
具体实施方式
<第一实施方式>
《作业车辆的结构》
以下,参照附图对实施方式进行详细说明。
图1是第一实施方式的作业车辆的侧视图。
第一实施方式的作业车辆100是轮式装载机。作业车辆100通过自动驾驶而行驶。即作业车辆100是无人行驶车。作业车辆100具备车身110、工作装置120、以及控制装置140。
车身110具备前车身111、后车身112、以及转向致动器113。前车身111和后车身112安装成能够绕沿车身110的上下方向延伸的转向轴转动。在前车身111的下部设置有一对前轮WF。在后车身112的下部设置有一对后轮WR。
后车身112具备动力源1121及动力传递装置1122。动力源1121例如是柴油发动机。动力传递装置1122将输入至输入轴的驱动力变速而从输出轴输出。动力传递装置1122的输出轴经由未图示的前桥而与前轮WF连接,经由未图示的后桥而与后轮WR连接。前桥将前车身111支承为能够行驶。后桥将后车身112支承为能够行驶。在前桥及后桥具备未图示的制动装置。在动力传递装置1122设置有旋转传感器1123。旋转传感器1123计测动力传递装置1122的输出轴的转速。控制装置140能够基于旋转传感器1123的计测数据来确定作业车辆100的速度及加速度。另外,在后车身112的上部设置有检测装置1124及测位装置1125。检测装置1124检测存在于作业车辆100的周围的物体的三维位置。作为检测装置1124的例子,可举出LiDar(Light Detection and Ranging)、立体相机、UWB(Ultra Wide Band)测距装置等。检测装置1124例如以检测方向朝向前方的方式设置。测位装置1125通过GNSS(GlobalNavigation Satellite System)等计测作业车辆100的全局坐标中的位置及姿势(姿势角)。位置例如由纬度及经度表示。姿势例如由以北为基准方位的后车身112的方位角表示。以下,将作业车辆100的位置及姿势也称作作业车辆100的姿态。
转向致动器113例如是液压缸。转向致动器113的基端部安装于后车身112,前端部安装于前车身111。转向致动器113通过工作油进行伸缩,从而规定前车身111与后车身112所成的角度。即,通过转向致动器113的伸缩,规定前轮WF的转向角。在转向致动器113设置有计测转向致动器113的行程长度的行程传感器1131。控制装置140能够基于行程传感器1131的计测数据来确定作业车辆100的转向角。
动力传递装置1122、制动装置以及转向致动器113是用于使车身110行驶的行驶装置。
工作装置120用于砂土等作业对象物的挖掘以及运输。工作装置120设置于车身110的前部。
控制装置140基于检测装置1124的检测数据,使作业车辆100行驶。
《控制装置140的结构》
图2是示出第一实施方式的控制装置140的结构的概要框图。
控制装置140是具备处理器210、主存储器230、储存器250、接口270的计算机。
储存器250是非临时的有形的存储介质。作为储存器250的例子,可举出磁盘、光盘、光磁盘、半导体存储器等。储存器250可以是与控制装置140的总线直接连接的内部介质,也可以是经由接口270或通信线路而与控制装置140连接的外部介质。储存器250存储用于控制作业车辆100的程序。
程序可以是用于实现使控制装置140发挥的功能的一部分的程序。例如,程序可以是通过与已存储于储存器250的其他程序的组合、或者与安装于其他装置的其他程序的组合来发挥功能的程序。需要说明的是,在其他实施方式中,控制装置140也可以除了上述结构之外或者取代上述结构而具备PLD(Programmable Logic Device)等定制LSI(LargeScale Integrated Circuit)。作为PLD的例子,可举出PAL(Programmable Array Logic)、GAL(Generic Array Logic)、CPLD(Complex Programmable Logic Device)、FPGA(FieldProgrammable Gate Array)。在该情况下,由处理器实现的功能的一部分或全部也可以由该集成电路来实现。
《软件结构》
处理器210通过执行程序,具备取得部211、映射生成部213、目标决定部212、路径搜索部214、制约条件生成部215、轨道搜索部216、行驶控制部217。控制装置140是轨道搜索装置的一例。
取得部211取得行程传感器1131、旋转传感器1123、检测装置1124以及测位装置1125的计测数据。由此,取得部211确定作业车辆100的位置、姿势角、转向角、速度及加速度、以及存在于作业车辆100的周围的障碍物的位置。
映射生成部213使用取得部211所取得的检测装置1124的计测数据与测位装置1125的计测数据来生成表示作业车辆100的周围的映射数据。映射生成部213例如通过SLAM(Simultaneous Localization and Mapping)技术来生成映射数据。映射数据例如由全局坐标系所涉及的占有格子地图来表示。图3是示出第一实施方式的映射数据的一例的图。另外,在映射数据的各格子标注有表示距障碍物的距离的得分。表示障碍物存在的位置的格子的得分是零,距障碍物的距离越远则得分越高。得分例如由距最近的障碍物的曼哈顿距离表示。
检测装置1124固定于后车身112,因此映射生成部213基于测位装置1125的计测数据表示的位置,使检测装置1124的计测数据表示的障碍物的位置平行移动,且基于姿势角进行旋转,由此能够确定全局坐标系中的障碍物的位置。映射生成部213所生成的映射数据被记录在主存储器230中。
目标决定部212决定作业车辆100的目标姿态(目标位置)。例如目标决定部212在工作装置120未保持砂土的情况下,将预先指定的装入场的位置及与装入场相对的姿势角决定为目标姿态。另外,目标决定部212在工作装置120保持有砂土的情况下,将预先指定的卸土场的位置及与卸土场相对的姿势角决定为目标姿态。此时,例如目标决定部212也可以基于映射数据,来修正装入场及卸土场的位置。
路径搜索部214搜索从取得部211所取得的作业车辆100的当前的姿态(初始位置)连结到目标决定部212所决定的目标姿态的路径。“路径”由姿态的顺序集合表示。即,路径搜索部214求出的路径是作业车辆100的位置和姿势角的系列。路径搜索部214通过动态规划法等的具有完整性的搜索算法(第一搜索算法)来搜索路径。作为动态规划法的算法的例子,可以举出迪杰斯特拉法、Hybrid A*等。在使用Hybrid A*的情况下,启发式代价(heuristic cost)可以通过Reeds Shepp法或Laplace Potential法而求出。Reeds Shepp法是求出无视障碍物或曲率的连续性时的非完整系统的最短路径的方法。LaplacePotential法是求出无视非完整制约时的最短路径的方法。
利用路径搜索部214进行的搜索的制约条件(第一制约条件)是不与映射生成部213生成的映射数据所表示的障碍物接触、终点中的姿态与目标姿态一致、以及转向角成为规定的可动范围内。利用路径搜索部214进行的搜索的评价函数是行驶距离越短则值越小的函数。用于利用路径搜索部214进行搜索的模型由式(1)表示。
[式1]
式(1)中,x[k]表示时刻k中的x坐标(例如纬度)。y[k]表示时刻k中的y坐标(例如经度)。φ[k]表示时刻k中的姿势角。δ[k]表示时刻k中的转向角。v[k]表示时刻k中的速度。k+1表示时刻k的单位时间后的时刻。Δt表示时间刻度宽度。L表示从铰接中心到前轮WF的长度。即,路径搜索部214使用的数理模型是作业车辆100的x坐标、y坐标以及姿势角这3要素的模型,在要素中不包含转向角及速度。
动态规划法所涉及的搜索算法具有完整性,另一方面,计算量相对于模型、输入的维数呈指数函数地增大。因此,第一实施方式的路径搜索部214能够通过减小模型的要素数来抑制计算量。
需要说明的是,在式(1)中,L/tan(δ[k])表示作业车辆100的回转半径。即,在式(1)中,x坐标的增量表示为将姿势角φ[k]的正弦与姿势角φ[k+1]的正弦之差乘以时刻k中的作业车辆100的回转半径而得到的值。同样地,在式(1)中,y坐标的增量表示为将姿势角φ[k]的余弦与姿势角φ[k+1]的余弦之差乘以时刻k中的作业车辆100的回转半径而得到的值。
路径搜索部214通过基于式(1)所示的模型的搜索,取得作业车辆100的姿态的系列来作为路径。需要说明的是,式(1)所示的模型将时刻作为索引,但在制约条件生成部215中,路径的索引不作为表示时刻的指标来处理。例如,路径中的姿态x[i]不是表示时刻i中的姿态,而是仅表示第i个姿态。另一方面,由路径搜索部214得到的转向角及速度的时间序列被用作后述的轨道搜索部216的初始值。需要说明的是,路径搜索部214使用的模型是姿态的模型,在要素中不包含转向角及速度,因此搜索出的转向角及加速度的系列缺乏连续性。即,路径搜索部214搜索到的路径有时曲率不连续,或者包含不必要的折返。
制约条件生成部215基于路径搜索部214所得到的路径,生成用于利用轨道搜索部216进行的搜索的制约条件(第二制约条件)。利用轨道搜索部216进行的搜索中使用的制约条件是作业车辆100的位置位于包含由路径搜索部214得到的路径的容许范围系列内。制约条件由惩罚函数表示,该惩罚函数根据距容许范围系列的偏离距离而惩罚值增加,作业车辆100位于容许范围系列内时的惩罚值成为零。图4是示出第一实施方式的容许范围系列的例子的图。容许范围系列是由矩形表示的容许范围的系列。容许范围由矩形的四个顶点的坐标表示。制约条件生成部215基于映射生成部213所生成的映射数据及路径搜索部214所生成的路径来决定容许范围系列。通过由制约条件生成部215将路径转换为容许范围系列,能够无视路径的曲率的不连续、不必要的折返。
制约条件生成部215按照以下的步骤生成容许范围系列。图5是示出第一实施方式的容许范围系列的生成方法的流程图。
制约条件生成部215从路径所涉及的姿态的系列中逐一选择姿态(步骤S51),针对所选择的姿态执行以下的步骤S52~S56的处理。制约条件生成部215基于在步骤S51中选择出的姿态,在记录于主存储器230的映射数据上配置表示作业车辆100的外形的第一矩形R1(步骤S52)。制约条件生成部215基于映射数据中的与在第一矩形R1的四个边上设定的多个点重叠的多个格子的得分,计算从各点到最近的障碍物的距离(欧几里距离)(步骤S53)。图6是示出第一实施方式的第一矩形R1及第二矩形R2的关系的图。在第一矩形R1由两个长边E1及E2以及两个短边E3及E4构成的情况下,例如设定第一矩形的四个顶点p1、p3、p6、p8、将长边E1分为三等分的两个点p4、p5、将长边E2分为三等分的两个点p9、p10、将短边E3一分为二的点p2、以及将短边E4一分为二的点p7。第一实施方式的得分表示距障碍物的曼哈顿距离,因此距障碍物的距离d使用得分n,通过以下的式(2)求出。
[式2]
制约条件生成部215基于映射数据,针对第一矩形的四个边分别确定在步骤S53中求出的距离d中的最短的距离(步骤S54)。制约条件生成部215使用在步骤S54中求出的距离d,通过以下的式(3)求出表示容许范围的第二矩形R2的四个顶点P1、P2、P3、P4的位置(步骤S55)。
[式3]
即,容许范围由内含表示作业车辆100的第一矩形R1的中心点p0的第二矩形R2表示。
制约条件生成部215通过配置基于在步骤S51中选择出的姿态而计算出的容许范围的长方形,决定全局坐标系中的容许范围的位置及方位角(步骤S56)。
制约条件生成部215通过针对路径的全部的姿态执行从上述的步骤S52至步骤S56的处理,能够得到容许范围系列。
图2所示的轨道搜索部216搜索从取得部211所取得的作业车辆100的当前的姿态连结到目标决定部212所决定的目标姿态的轨道。“轨道”由将时间作为参数的姿态的系列表示。即,轨道搜索部216求出用于实现作业车辆100的每个时刻的姿态、以及该每个时刻的姿态的转向角及速度的系列来作为轨道。轨道搜索部216通过使用评价函数的二次近似的搜索算法(第二搜索算法)来搜索轨道。作为使用评价函数的二次近似的搜索算法的例子,可以举出作为微分动态规划法、iLQR(iterative Linear Quadratic Regulator)、SQP(Sequential Quadratic Programming)等。
利用轨道搜索部216进行的搜索以通过行驶时间越短其值越小的评价函数和制约条件生成部215所生成的惩罚函数之和而得到的评价值成为最小的方式执行。在利用轨道搜索部216进行的搜索中,式(1)所示的时间刻度宽度Δt也成为优化对象。
使用评价函数的二次近似的搜索算法与动态规划法所涉及的搜索算法相比,计算量相对于维数的增加缓慢。因此,即使将转向角、速度以及时间刻度宽度加入到最优化的对象,也不会产生计算量的爆发。另一方面,使用评价函数的二次近似的搜索算法不具有若制约条件的非线性强则有时会产生向局部解的收敛等的完整性。因此,通过使用能够由制约条件生成部215所生成的线性函数表示的制约条件,能够使问题简单化,防止向局部解的收敛。
行驶控制部217按照轨道搜索部216求出的轨道,控制转向致动器113、动力源1121、动力传递装置1122以及制动装置。即,行驶控制部217从轨道所涉及的系列读出当前时刻所涉及的转向角及速度,以实现该转向角的方式控制转向致动器113,另外,以实现该速度的方式控制动力源1121、动力传递装置1122及制动装置。
《控制装置140的动作》
图7是示出第一实施方式的控制装置140的自动行驶处理的流程图。
控制装置140的目标决定部212基于作业车辆100的状态来决定目标姿态(步骤S1)。取得部211取得行程传感器1131、旋转传感器1123、检测装置1124以及测位装置1125的计测数据(步骤S2)。行驶控制部217基于在步骤S2中所取得的测位装置1125的计测数据,判定在步骤S1中决定的目标姿态与当前的姿态的误差是否为规定的阈值以下(步骤S3)。
在与目标姿态的误差比阈值大的情况下(步骤S3:否),基于在步骤S2中所取得的检测装置1124的计测数据,更新在主存储器230中记录的映射数据(步骤S4)。路径搜索部214搜索从作业车辆100的当前的姿态连结到步骤S1中所决定的目标姿态的路径(步骤S5)。
接下来,制约条件生成部215基于在步骤S5中搜索到的路径,执行图5所示的容许范围系列生成处理(步骤S6)。制约条件生成部215基于所生成的容许范围系列来生成惩罚函数(步骤S7)。轨道搜索部216使用在步骤S7中所生成的惩罚函数,搜索从作业车辆100的当前的姿态连结到目标姿态的轨道(步骤S8)。此时,轨道搜索部216将在步骤S5的路径搜索中所得到的姿态、转向角及速度的系列用于搜索的初始值。
行驶控制部217按照在步骤S8中求出的轨道,控制转向致动器113、动力源1121、动力传递装置1122以及制动装置(步骤S9)。然后,控制装置140使处理返回到步骤S2。
在步骤S3中与目标姿态的误差比阈值大的情况下(步骤S3:否),控制装置140结束自动行驶处理。控制装置140在执行自动挖掘处理、自动卸土处理之后,再次执行自动行驶处理。
《作用/效果》
第一实施方式的控制装置140基于表示障碍物的位置的第一制约条件,搜索用于从作业车辆100的初始位置向目标位置移动的路径,且基于惩罚值根据距路径的偏离距离而增加的第二制约条件(惩罚函数)来搜索轨道。这样,控制装置140改变制约条件而以两个阶段搜索轨道。通过将用于轨道的搜索的制约条件设为与距路径的偏离距离相应的制约条件,能够减弱轨道的搜索所涉及的制约条件的非线性来抑制计算量的增加。即,控制装置140能够以较少的计算量得到连续的轨道。
另外,第一实施方式的控制装置140使用具有完整性的搜索算法来搜索路径,使用计算量相对于要素数的增加缓慢的第二搜索算法来搜索轨道。由此,控制装置140能够在抑制计算量且不向局部解收敛的情况下得到路径及轨道。
另外,第一实施方式的用于轨道的搜索的惩罚函数是惩罚值根据距包含路径所涉及的各位置且在作业车辆100的宽度方向上扩展的第二矩形的偏离距离而增加、且作业车辆100位于第二矩形内时的惩罚值恒定的函数。这样,控制装置140能够得到线性度高的惩罚函数。
另外,在第一实施方式中,使用式(1)所示的模型进行路径及轨道的搜索。式(1)所示的模型基于作业车辆100的当前的位置(x[k],y[k])、根据作业车辆100的转向角δ[k]求出的回转半径、作业车辆100的当前的姿势角φ[k]、以及作业车辆100的单位时间后的姿势角φ[k+1]来表示作业车辆100的单位时间后的位置(x[k+1],y[k+1])。
图8是示出第一实施方式的模型中的时间刻度宽度与计算结果的位置的关系的图。图9是示出比较例的模型中的时间刻度宽度与计算结果的位置的关系的图。
作为车辆的模型,有时使用以下的式(4)所示的模型。
[式4]
然而,式(4)所示的模型由于将x坐标的增量以及y坐标的增量近似为直线移动,因此若时间的刻度宽度Δt的值变化,则如图9所示,位置的计算结果产生误差。与此相对地,式(1)所示的模型由于通过回转半径和姿势角将x坐标的增量及y坐标的增量近似为圆弧,因此即使如图8所示那样时间的刻度宽度Δt的值变化,位置的计算结果也不会产生误差。因此,第一实施方式的控制装置140通过使用式(1)所示的模型,能够无视时间刻度的影响,因此能够将路径搜索部214生成的路径容易地转换为轨道。由此,控制装置140能够不与由路径搜索部214求出的障碍物接触,将到达目标状态的轨道作为初始值开始轨道搜索。通过设定到达目标状态的轨道作为初始值,路径搜索部214能够在不向局部解收敛的情况下计算适当的轨道。
<其他实施方式>
以上,参照附图对一实施方式进行了详细说明,但具体结构并不局限于上述方式,而能够进行各种设计变更等。即,在其他实施方式中,也可以适当变更上述的处理的顺序。另外,也可以并行执行一部分的处理。
上述实施方式的控制装置140既可以是由单独的计算机构成的装置,也可以是将控制装置140的结构分开地配置于多个计算机,并通过多个计算机相互配合来作为控制装置140发挥功能的装置。此时,构成控制装置140的一部分的计算机也可以搭载于作业车辆100的内部,其他计算机也可以设置于作业车辆100的外部。
图10是示出其他实施方式的自动驾驶系统2的结构的概要图。第一实施方式的作业车辆100通过内部具备的控制装置140的控制进行自主行驶。与此相对地,图9所示的自动驾驶系统2与作业车辆100远程设置的控制服务器800控制作业车辆100。控制服务器800与控制装置140通过无线通信连接。
在该情况下,图2所示的各处理部设置于控制服务器800。取得部211通过无线通信得到各计测装置的计测数据。另外,行驶控制部217通过无线通信向控制装置140发送控制指令。
另外,第一实施方式的控制装置140在路径的搜索中使用动态规划法,但并不限定于此。例如,在其他实施方式中,也可以通过直线、圆弧、贝塞尔曲线、回旋曲线、B样条曲线等曲线的组合来几何学地生成路径。
另外,第一实施方式的控制装置140使用在轨道的搜索中使用评价函数的二次近似的搜索算法,但并不限定于此。例如,在其他实施方式中,控制装置140也可以通过作为数理规划问题的再公式化来求出轨道。
另外,第一实施方式的控制装置140设定由第二矩形表示的可动范围来作为轨道的搜索的制约条件,但并不限定于此。例如,其他实施方式的制约条件也可以由通过路径所涉及的点并仅在作业车辆100的宽度方向上扩展的线段来表示。
另外,第一实施方式的控制装置140基于式(3)来求出第二矩形R2,但并不限定于此。例如,其他实施方式的控制装置140也可以按照以下所示的步骤求出第二矩形R2。控制装置140也可以使用距作业车辆100最近的障碍物存在的方向的方位角α以及到该障碍物的距离d,通过以下的式(5)求出第二矩形R2的四个顶点R1、R2、R3、R4的位置。在式(5)中,L表示从铰接中心至前轮WF的长度,l表示胎面宽度。
[式5]
另外,第一实施方式的作业车辆100是作为铰接车辆的轮式装载机,但并不限定于此。例如,其他实施方式的车辆可以是自卸车等其他车辆,也可以不是铰接车辆。在不是铰接车辆的情况下,例如,转向角是指被转向的车轮与车身所成的角度。另外,其他实施方式的作业车辆100可以是履带式车辆,也可以是滑移装载机等的具有无法转向的车轮的车辆。在该情况下,例如,能够根据左右的履带或车轮的速度差来求出回转半径。
工业上的可利用性
根据上述方式的至少一个,轨道搜索装置能够以较少的计算量得到连续的轨道。
附图标记说明
100…作业车辆;110…车身;111…前车身;112…后车身;1121…动力源;1122…动力传递装置;1123…旋转传感器;1124…检测装置;1125…测位装置;113…转向致动器;1131…行程传感器;120…工作装置;140…控制装置;210…处理器;211…取得部;212…目标决定部;213…映射生成部;214…路径搜索部;215…制约条件生成部;216…轨道搜索部;217…行驶控制部;230…主存储器;250…储存器;270…接口;WF…前轮;WR…后轮;800…控制服务器;2…自动驾驶系统。
Claims (11)
1.一种系统,其是搜索用于供车辆自动行驶的轨道的系统,
其中,
所述系统具备处理器,
所述处理器具备:
路径搜索部,其基于表示障碍物的位置的第一制约条件,搜索用于从所述车辆的初始位置向目标位置移动的路径、即以所述车辆的位置及姿势为要素的系列;
制约条件生成部,其生成惩罚值根据距所述路径的偏离距离而增加的第二制约条件;以及
轨道搜索部,其基于所述第二制约条件,搜索用于从所述车辆的初始位置向目标位置移动的轨道、即以所述车辆的位置、姿势、速度以及转向角为要素的系列。
2.根据权利要求1所述的系统,其中,
所述路径搜索部使用具有完整性的第一搜索算法来搜索所述路径,
所述轨道搜索部使用与所述第一搜索算法相比计算量相对于要素数的增加缓慢的第二搜索算法来搜索所述轨道。
3.根据权利要求1或2所述的系统,其中,
所述第二制约条件是如下条件:所述惩罚值根据距包含所述路径的各要素所涉及的位置且在所述车辆的宽度方向上扩展的区域的偏离距离而增加,且所述车辆位于所述区域内时的所述惩罚值恒定。
4.根据权利要求3所述的系统,其中,
所述制约条件生成部基于所述障碍物的位置和所述路径的各要素所涉及的所述位置及所述姿势,生成所述第二制约条件。
5.根据权利要求1至4中任一项所述的系统,其中,
所述路径搜索部基于表示所述车辆的模型来搜索所述路径,
所述模型基于所述车辆的当前的位置、根据所述车辆的转向角而求出的回转半径、所述车辆的当前的姿势角以及所述车辆的单位时间后的姿势角来表示所述车辆的单位时间后的位置。
6.一种方法,其是搜索用于供车辆自动行驶的轨道的方法,其中,
所述方法包括:
基于表示障碍物的位置的第一制约条件,搜索用于从所述车辆的初始位置向目标位置移动的路径、即以所述车辆的位置及姿势为要素的系列的步骤;
生成惩罚值根据距所述路径的偏离距离而增加的第二制约条件的步骤;以及
基于所述第二制约条件,搜索用于从所述车辆的初始位置向目标位置移动的轨道、即以所述车辆的位置、姿势、速度以及转向角为要素的系列的步骤。
7.根据权利要求6所述的方法,其中,
在搜索所述路径的步骤中,使用具有完整性的第一搜索算法来搜索所述路径,
在搜索所述轨道的步骤中,使用与所述第一搜索算法相比计算量相对于要素数的增加缓慢的第二搜索算法来搜索所述轨道。
8.根据权利要求6或7所述的方法,其中,
所述第二制约条件是如下条件:所述惩罚值根据距包含所述路径的各要素所涉及的位置且在所述车辆的宽度方向上扩展的区域的偏离距离而增加,且所述车辆位于所述区域内时的所述惩罚值恒定。
9.根据权利要求8所述的方法,其中,
生成所述第二制约条件的步骤基于所述障碍物的位置和所述路径的各要素所涉及的所述位置及所述姿势,生成所述第二制约条件。
10.根据权利要求6至9中任一项所述的方法,其中,
在搜索所述路径的步骤中,基于表示所述车辆的模型来搜索所述路径,
所述模型基于所述车辆的当前的位置、根据所述车辆的转向角而求出的回转半径、所述车辆的当前的姿势角以及所述车辆的单位时间后的姿势角来表示所述车辆的单位时间后的位置。
11.一种作业车辆,其具备:
车身;
行驶装置,其用于使所述车身行驶;以及
处理器,
其中,
所述处理器具备:
路径搜索部,其基于表示障碍物的位置的第一制约条件,搜索用于从所述作业车辆的初始位置向目标位置移动的路径、即以所述作业车辆的位置及姿势为要素的系列;
制约条件生成部,其生成惩罚值根据距所述路径的偏离距离而增加的第二制约条件;
轨道搜索部,其基于所述第二制约条件,搜索用于从所述作业车辆的初始位置向目标位置移动的轨道、即以所述作业车辆的位置、姿势、速度以及转向角为要素的系列;以及
行驶控制部,其根据所述轨道而控制所述行驶装置。
Applications Claiming Priority (3)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2021-061381 | 2021-03-31 | ||
JP2021061381A JP2022157259A (ja) | 2021-03-31 | 2021-03-31 | システム、方法および作業車両 |
PCT/JP2022/007611 WO2022209465A1 (ja) | 2021-03-31 | 2022-02-24 | システム、方法および作業車両 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN116964539A true CN116964539A (zh) | 2023-10-27 |
Family
ID=83458370
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202280019618.5A Pending CN116964539A (zh) | 2021-03-31 | 2022-02-24 | 系统、方法以及作业车辆 |
Country Status (5)
Country | Link |
---|---|
US (1) | US20240152155A1 (zh) |
EP (1) | EP4290020A1 (zh) |
JP (1) | JP2022157259A (zh) |
CN (1) | CN116964539A (zh) |
WO (1) | WO2022209465A1 (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116481557B (zh) * | 2023-06-20 | 2023-09-08 | 北京斯年智驾科技有限公司 | 一种路口通行路径规划方法、装置、电子设备及存储介质 |
Family Cites Families (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP5332034B2 (ja) | 2008-09-22 | 2013-11-06 | 株式会社小松製作所 | 無人車両の走行経路生成方法 |
US10429847B2 (en) * | 2017-09-22 | 2019-10-01 | Locus Robotics Corp. | Dynamic window approach using optimal reciprocal collision avoidance cost-critic |
CN111796588A (zh) * | 2019-03-22 | 2020-10-20 | 北京京东尚科信息技术有限公司 | 路径规划方法、路径规划装置及计算机可读存储介质 |
JP2021033685A (ja) * | 2019-08-26 | 2021-03-01 | 株式会社デンソー | 学習プログラム及び学習方法 |
JP7408335B2 (ja) | 2019-10-09 | 2024-01-05 | 株式会社ディスコ | ワーク収容機構 |
-
2021
- 2021-03-31 JP JP2021061381A patent/JP2022157259A/ja active Pending
-
2022
- 2022-02-24 US US18/548,800 patent/US20240152155A1/en active Pending
- 2022-02-24 CN CN202280019618.5A patent/CN116964539A/zh active Pending
- 2022-02-24 EP EP22779690.1A patent/EP4290020A1/en active Pending
- 2022-02-24 WO PCT/JP2022/007611 patent/WO2022209465A1/ja active Application Filing
Also Published As
Publication number | Publication date |
---|---|
JP2022157259A (ja) | 2022-10-14 |
EP4290020A1 (en) | 2023-12-13 |
WO2022209465A1 (ja) | 2022-10-06 |
US20240152155A1 (en) | 2024-05-09 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110361013B (zh) | 一种用于车辆模型的路径规划系统及方法 | |
JP4738472B2 (ja) | 障害物回避機能を有する移動制御装置 | |
JP7133298B2 (ja) | 運搬車両の管制システム及び運搬車両の管理方法 | |
Rajamani et al. | Lateral control of a backward driven front-steering vehicle | |
US20080059015A1 (en) | Software architecture for high-speed traversal of prescribed routes | |
KR101133037B1 (ko) | 자율이동차량용 충돌회피를 위한 경로갱신방법 및 그 장치 | |
JP7349561B2 (ja) | 地図情報補正方法、運転支援方法及び地図情報補正装置 | |
CN104960520A (zh) | 基于Pure Pursuit算法的预瞄点确定方法 | |
Yang et al. | An algorithm of curved path tracking with prediction model for autonomous vehicle | |
US20220266874A1 (en) | Systems and methods for vehicle motion planning | |
CN113670305A (zh) | 泊车轨迹生成方法、装置、计算机设备和存储介质 | |
Schmidt et al. | Construction site navigation for the autonomous excavator Thor | |
WO2022209465A1 (ja) | システム、方法および作業車両 | |
US20230150485A1 (en) | Vehicle path adjustment | |
US20240124024A1 (en) | Auto-tunable path controller with dynamic avoidance capability | |
JPH11194822A (ja) | 移動ロボットのグローバル地図構築方法 | |
CN108334074B (zh) | 控制系统 | |
US20230150532A1 (en) | Vehicle path adjustment | |
JP7346754B2 (ja) | ダンプトラックの制御システム | |
JP5895682B2 (ja) | 障害物検出装置及びそれを備えた移動体 | |
JP2023147640A (ja) | 作業車両 | |
CN115542899A (zh) | 车辆路径跟踪的方法、装置、车辆、电子设备及介质 | |
JP7289640B2 (ja) | 車両案内ディスプレイ及び経路ナビゲーション方法 | |
JP4269170B2 (ja) | 軌道追従制御方法および装置 | |
JP6717132B2 (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 |