WO2022153402A1 - 経路追従装置、経路追従方法および経路追従プログラム - Google Patents

経路追従装置、経路追従方法および経路追従プログラム Download PDF

Info

Publication number
WO2022153402A1
WO2022153402A1 PCT/JP2021/000864 JP2021000864W WO2022153402A1 WO 2022153402 A1 WO2022153402 A1 WO 2022153402A1 JP 2021000864 W JP2021000864 W JP 2021000864W WO 2022153402 A1 WO2022153402 A1 WO 2022153402A1
Authority
WO
WIPO (PCT)
Prior art keywords
mobility
bending angle
area
target position
route
Prior art date
Application number
PCT/JP2021/000864
Other languages
English (en)
French (fr)
Inventor
道学 吉田
Original Assignee
三菱電機株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 三菱電機株式会社 filed Critical 三菱電機株式会社
Priority to DE112021005924.6T priority Critical patent/DE112021005924T5/de
Priority to JP2022556068A priority patent/JP7191281B1/ja
Priority to PCT/JP2021/000864 priority patent/WO2022153402A1/ja
Priority to CN202180088717.4A priority patent/CN116802103A/zh
Publication of WO2022153402A1 publication Critical patent/WO2022153402A1/ja
Priority to US18/195,162 priority patent/US20230273616A1/en

Links

Images

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/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
    • B60W30/10—Path keeping
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W60/00—Drive control systems specially adapted for autonomous road vehicles
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0217—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with energy consumption, time reduction or distance reduction criteria

Definitions

  • This disclosure relates to the path tracking of mobility.
  • Patent Document 1 discloses a method for moving mobility following a path.
  • An object of the present disclosure is to move mobility following a path by a method different from the direction of Patent Document 1.
  • the path tracking device of the present disclosure is Based on the route data indicating a plurality of waypoints located on the route to the destination, the bending angle of the route at the reference waypoint, which is the number of reference points from the mobility position toward the destination, is determined.
  • FIG. 6 is a configuration diagram of a path following device 200 according to a second embodiment.
  • the flowchart of step S240 in Embodiment 2. The figure which shows the example of the midpoint and the risk area in Embodiment 2.
  • FIG. 3 is a configuration diagram of a path following device 200 according to a third embodiment.
  • the flowchart of step S340 in Embodiment 3. The figure which shows the example of the representative point and the detour route in Embodiment 3.
  • FIG. 3 The explanatory view of the effect in Embodiment 3.
  • FIG. 3 is a hardware configuration diagram of the path tracking device 200 according to the embodiment.
  • Embodiment 1 The mobility system 100 will be described with reference to FIGS. 1 to 7.
  • the configuration of the mobility system 100 includes a mobility 110.
  • the mobility 110 is a vehicle capable of turning the vehicle body on the spot, and is also called personal mobility. Specific examples of the mobility 110 are electric wheelchairs or senior cars.
  • the mobility 110 includes a sensor group 120, a route tracking device 200, a vehicle control device 130, and a vehicle 140. These hardware are connected to each other via a signal line.
  • the sensor group 120 is one or more sensors including an object detection sensor, a positioning sensor, and the like.
  • the object detection sensor is a sensor for detecting an object existing around the mobility 110. Specific examples of the object detection sensor are a camera and a laser sensor.
  • the positioning sensor is a sensor for positioning the position of the mobility 110. Specific examples of the positioning sensor are a receiver of a satellite positioning system and an inertial measurement unit.
  • the route following device 200 determines the target position of the mobility 110 and the moving speed of the mobility 110.
  • the target position is a position to be the next destination of the mobility 110, and is repeatedly determined until the mobility 110 reaches the destination point.
  • the moving speed is the speed of the mobility 110 until the mobility 110 reaches the target position.
  • the vehicle control device 130 automatically controls the vehicle 140 in order to move the mobility 110 to the target position.
  • the vehicle 140 is the main body of the mobility 110, and includes a speed control device, an attitude control device, and the like.
  • the sensor group 120 may not be provided in the mobility 110, but may be provided in various places on the road, for example.
  • the path following device 200 may be provided outside the mobility 110.
  • the mobility 110 includes a communication device for communicating with the path tracking device 200.
  • the configuration of the path tracking device 200 will be described with reference to FIG.
  • the path tracking device 200 is a computer including hardware such as a processor 201, a memory 202, an auxiliary storage device 203, a communication device 204, and an input / output interface 205. These hardware are connected to each other via a signal line.
  • the processor 201 is an IC that performs arithmetic processing and controls other hardware.
  • processor 201 is a CPU, DSP or GPU.
  • IC is an abbreviation for Integrated Circuit.
  • CPU is an abbreviation for Central Processing Unit.
  • DSP is an abbreviation for Digital Signal Processor.
  • GPU is an abbreviation for Graphics Processing Unit.
  • the memory 202 is a volatile or non-volatile storage device.
  • the memory 202 is also referred to as a main storage device or a main memory.
  • the memory 202 is a RAM.
  • the data stored in the memory 202 is stored in the auxiliary storage device 203 as needed.
  • RAM is an abbreviation for Random Access Memory.
  • the auxiliary storage device 203 is a non-volatile storage device.
  • the auxiliary storage device 203 is a ROM, HDD, or flash memory.
  • the data stored in the auxiliary storage device 203 is loaded into the memory 202 as needed.
  • ROM is an abbreviation for Read Only Memory.
  • HDD is an abbreviation for Hard Disk Drive.
  • Communication device 204 is a receiver and a transmitter.
  • the communication device 204 is a communication chip or NIC.
  • Communication of the path tracking device 200 is performed using the communication device 204.
  • NIC is an abbreviation for Network Interface Card.
  • the input / output interface 205 is a port to which an input device and an output device are connected.
  • the input / output interface 205 is a USB terminal.
  • the input / output of the route following device 200 is performed by using the input / output interface 205 or the communication device 204.
  • USB is an abbreviation for Universal Serial Bus.
  • the route following device 200 includes elements such as a reception unit 210, a route right angle determination unit 220, a target position determination unit 230, a movement speed determination unit 240, and an output unit 250.
  • the path right angle determination unit 220 includes elements such as a bending angle calculation unit 221 and a bending angle determination unit 222. These elements are realized in software.
  • the auxiliary storage device 203 stores a route tracking program for operating the computer as a reception unit 210, a path right angle determination unit 220, a target position determination unit 230, a movement speed determination unit 240, and an output unit 250.
  • the path-following program is loaded into memory 202 and executed by processor 201.
  • the OS is further stored in the auxiliary storage device 203. At least part of the OS is loaded into memory 202 and executed by processor 201.
  • the processor 201 executes the route tracking program while executing the OS.
  • OS is an abbreviation for Operating System.
  • the input / output data of the route tracking program is stored in the storage unit 290.
  • the memory 202 functions as a storage unit 290.
  • a storage device such as an auxiliary storage device 203, a register in the processor 201, and a cache memory in the processor 201 may function as a storage unit 290 instead of the memory 202 or together with the memory 202.
  • the path tracking device 200 may include a plurality of processors that replace the processor 201.
  • the path tracking program can be computer-readablely recorded (stored) on a non-volatile recording medium such as an optical disk or a flash memory.
  • the operation procedure of the path following device 200 corresponds to the path following method. Further, the operation procedure of the path tracking device 200 corresponds to the processing procedure by the path tracking program.
  • the reception unit 210 receives various data and stores the various data in the storage unit 290.
  • various data are route data, position data, and the like.
  • the route data indicates a plurality of waypoints located on the route from the start point to the destination point.
  • the route from the start point to the destination point is called a "long-term route”.
  • the position data indicates the position (current position) of the mobility 110.
  • the position of the mobility 110 is determined by the positioning sensor of the sensor group 120.
  • the bending angle calculation unit 221 calculates the bending angle of the route at the reference waypoint based on the route data.
  • the reference waypoint is the number of reference waypoints from the position of the mobility 110 toward the destination point. The number of reference points is predetermined.
  • the bending angle represents the magnitude of the bending of the path.
  • the bending angle at the reference waypoint corresponds to the turning angle of the mobility 110 when the mobility 110 passes the reference waypoint.
  • step S121 the bending angle calculation unit 221 determines a reference waypoint from a plurality of waypoints shown in the route data based on the position of the mobility 110. Specifically, the bending angle calculation unit 221 determines the Xth waypoint as the reference waypoint, counting from the position of the mobility 110. "X" represents the number of reference points.
  • step S122 the bending angle calculation unit 221 acquires the coordinate value of the reference waypoint from the route data.
  • the bending angle calculation unit 221 acquires the coordinate value of the front waypoint and the coordinate value of the front waypoint from the route data.
  • the front waypoint is a waypoint located before the reference waypoint. Specifically, the front waypoint is the (X-1) th waypoint counting from the position of the mobility 110.
  • the destination waypoint is a waypoint located beyond the reference waypoint. Specifically, the destination waypoint is the (X + 1) th waypoint counting from the position of the mobility 110.
  • the bending angle calculation unit 221 calculates the bending angle of the route at the reference waypoint based on the coordinate value of the reference waypoint, the coordinate value of the front waypoint, and the coordinate value of the destination waypoint. do. For example, the bending angle calculation unit 221 calculates a vector from the front waypoint to the reference waypoint and a vector from the reference waypoint to the destination waypoint. Then, the bending angle calculation unit 221 calculates the angle formed by the two vectors. The calculated angle is the bending angle.
  • the bending angle ⁇ can be expressed by the equation (1).
  • Wi represents the position of the reference waypoint.
  • Wi-1 represents the position of the front waypoint.
  • Wi + 1 represents the position of the destination waypoint.
  • step S130 the bending angle determination unit 222 compares the bending angle with the angle threshold value.
  • the angle threshold is a predetermined angle. Specifically, the angle threshold is an angle close to 90 degrees.
  • step S140 the target position determination unit 230 determines the target position based on the comparison result between the bending angle and the angle threshold value.
  • step S141 the target position determining unit 230 calculates the movement curve of the mobility 110.
  • the movement curve is a curve along the route before and after the reference waypoint, and represents the locus of the mobility 110 when the mobility 110 passes through the route before and after the reference waypoint.
  • the target position determination unit 230 calculates a movement curve based on the position of the mobility 110, the coordinate value of the front waypoint, the coordinate value of the reference waypoint, and the coordinate value of the destination waypoint. .
  • the movement curve is an arc having a specific curvature, with the position of the mobility 110 as the starting point and the leading waypoint as the ending point.
  • the method of calculating the movement curve is arbitrary.
  • step S142 the target position determining unit 230 determines whether the bending angle is equal to or less than the angle threshold value based on the comparison result in step S130. If the bending angle is equal to or less than the angle threshold value, the process proceeds to step S143. If the bending angle is greater than the angle threshold, the process proceeds to step S144. However, the process may proceed to step S144 when the bending angle is equal to the angle threshold value.
  • the target position determination unit 230 determines the target position from the movement curve. Specifically, the target position determination unit 230 determines the position ahead of the position of the mobility 110 by a reference distance as the target position.
  • the reference distance is a predetermined distance.
  • the target position determination unit 230 determines the target position from the movement curve. Specifically, the target position determination unit 230 determines the position ahead of the position of the mobility 110 by a shortened distance as the target position.
  • the shortened distance is shorter than the reference distance and is predetermined. For example, if the reference distance is 1 meter, the shortened distance is 0.5 meters, which is half of the reference distance.
  • step S150 the movement speed determination unit 240 determines the movement speed based on the comparison result between the bending angle and the angle threshold value.
  • step S151 the moving speed determination unit 240 determines whether the bending angle is equal to or less than the angle threshold value based on the comparison result in step S130. If the bending angle is equal to or less than the angle threshold value, the process proceeds to step S152. If the bending angle is greater than the angle threshold, the process proceeds to step S153. However, the process may proceed to step S153 when the bending angle is equal to the angle threshold value.
  • step S152 the moving speed determination unit 240 determines the reference speed as the moving speed.
  • the reference speed is a predetermined speed.
  • step S153 the moving speed determination unit 240 determines the deceleration speed as the moving speed.
  • the deceleration speed is slower than the reference speed and is predetermined. For example, the deceleration speed is 0.2 times the reference speed.
  • step S160 the output unit 250 outputs the target position data and the moving speed data to the vehicle control device 130.
  • the target position data indicates the target position determined in step S140.
  • the movement speed data indicates the movement speed determined in step S150.
  • the vehicle control device 130 moves the mobility 110 to the determined target position at the determined movement speed.
  • Steps S110 to S160 are repeatedly executed. However, in step S110, the route data may be received only once at the beginning.
  • the mobility 110 when the bending angle is large, the moving speed is changed to a slow speed position. As a result, the propulsive force of the mobility 110 is reduced, so that the lateral shake of the mobility 110 is reduced and the riding comfort of the mobility 110 is improved.
  • Embodiment 2 A mode for avoiding a collision with an object existing around the mobility 110 will be described mainly different from the first embodiment with reference to FIGS. 8 to 13.
  • the configuration of the path tracking device 200 will be described with reference to FIG.
  • the path following device 200 further includes a collision risk determination unit 260.
  • the collision risk determination unit 260 includes elements such as a risk area setting unit 261, a risk area determination unit 262, and an area tangent setting unit 263.
  • the path tracking program further causes the computer to function as a collision risk determination unit 260.
  • Step S210 the reception unit 210 receives various data.
  • Step S210 corresponds to step S110 of the first embodiment.
  • the various data include object detection data.
  • the object detection data indicates the range in which an object existing around the mobility 110 is detected. Objects existing around the mobility 110 are detected by the object detection sensor of the sensor group 120.
  • step S220 the bending angle calculation unit 221 calculates the bending angle of the route at the reference waypoint based on the route data.
  • Step S220 is the same as step S120 of the first embodiment.
  • step S230 the bending angle determination unit 222 compares the bending angle with the angle threshold value.
  • Step S230 is the same as step S130 of the first embodiment.
  • step S240 the area tangent setting unit 263 sets the area tangent, which will be described later.
  • step S240 the procedure of step S240 will be described with reference to FIG.
  • the risk area setting unit 261 sets the risk area based on the object detection data.
  • the risk area is an area where the mobility 110 is at risk of colliding with an object.
  • the risk area setting unit 261 calculates the midpoint of the object. Then, the risk area setting unit 261 sets an elliptical area centered on the midpoint of the object, with the length direction of the object as the direction of the major axis, and the normal direction with respect to the length direction of the object as the direction of the minor axis. calculate. The calculated area is the risk area.
  • FIG. 11 shows examples of midpoints and risk areas.
  • a wall exists on the right front side of the mobility 110, and the object detection data shows the detected wall.
  • the wall is an example of an object.
  • the risk area setting unit 261 calculates the midpoint of the wall. Then, the risk area setting unit 261 calculates an elliptical area centered on the midpoint as a risk area.
  • step S242 the target position determining unit 230 calculates the movement curve of the mobility 110.
  • Step S242 is the same as step S141 of the first embodiment.
  • step S243 the risk area determination unit 262 determines whether the movement curve passes through the risk area. If the movement curve passes through the risk area, the process proceeds to step S244. If the movement curve does not pass through the risk area, the area tangent is not set and the process ends.
  • step S244 the area tangent setting unit 263 sets the area tangent based on the position of the mobility 110 and the risk area.
  • the area tangent is a tangent that passes through the position of the mobility 110 and touches the risk area.
  • step S250 the target position determination unit 230 determines the target position based on the comparison result between the bending angle and the angle threshold value. Step S250 corresponds to step S140 of the first embodiment.
  • step S250 the target position determining unit 230 determines whether the bending angle is equal to or less than the angle threshold value based on the comparison result in step S230. If the bending angle is equal to or less than the angle threshold value, the process proceeds to step S252. If the bending angle is greater than the angle threshold, the process proceeds to step S253. However, the process may proceed to step S253 when the bending angle is equal to the angle threshold value.
  • the target position determination unit 230 determines the target position from the movement curve or the area tangent. Specifically, when the area tangent is not set, the target position determination unit 230 determines the target position from the movement curve. When the area tangent is set, the target position determination unit 230 determines the target position from the area tangent.
  • the target position is a position that is a reference distance ahead of the position of the mobility 110.
  • the target position determination unit 230 determines the target position from the movement curve or the area tangent. Specifically, when the area tangent is not set, the target position determination unit 230 determines the target position from the movement curve. When the area tangent is set, the target position determination unit 230 determines the target position from the area tangent.
  • the target position is a position a short distance ahead of the position of the mobility 110.
  • step S260 the movement speed determination unit 240 determines the movement speed based on the comparison result between the bending angle and the angle threshold value. Step S260 is the same as step S150 of the first embodiment.
  • step S270 the output unit 250 outputs the target position data and the moving speed data to the vehicle control device 130.
  • Step S270 is the same as step S160 of the first embodiment.
  • the effect of the second embodiment will be described with reference to FIG. (1)
  • the bending angle of the route at the reference waypoint ( Wi ) is a right angle. Therefore, the bending angle is large.
  • the target position is set on the tangent line of the area where there is a collision risk when an object such as a wall exists. This makes it possible to avoid the collision risk while ensuring the followability to the route.
  • Embodiment 3 The mode of bypassing the route and avoiding a collision with an object will be described mainly different from the first and second embodiments with reference to FIGS. 14 to 19.
  • the route following device 200 further includes an avoidance route setting unit 270.
  • the avoidance route setting unit 270 includes elements such as a safety area determination unit 271 and a detour curve setting unit 272.
  • the route-following program further causes the computer to function as an avoidance route setting unit 270.
  • Step S310 the reception unit 210 receives various data.
  • Step S210 corresponds to step S110 of the first embodiment.
  • the various data include safety area data.
  • the safety area data is an area where no object is detected.
  • step S320 the bending angle calculation unit 221 calculates the bending angle of the route at the reference waypoint based on the route data.
  • Step S320 is the same as step S120 of the first embodiment.
  • step S330 the bending angle determination unit 222 compares the bending angle with the angle threshold value. Step S330 is the same as step S130 of the first embodiment.
  • step S340 the area tangent setting unit 263 sets the area tangent or the detour curve.
  • step S340 the procedure of step S340 will be described with reference to FIG.
  • step S341 the risk area setting unit 261 sets the risk area based on the object detection data.
  • Step S341 is the same as step S241 of the second embodiment.
  • step S342 the target position determining unit 230 calculates the movement curve of the mobility 110.
  • Step S342 is the same as step S141 of the first embodiment.
  • step S343 the risk area determination unit 262 determines whether the movement curve passes through the risk area.
  • Step S343 corresponds to step S243 of the second embodiment. If the movement curve passes through the risk area, the process proceeds to step S344. If the movement curve does not pass through the risk area, no area tangents and new waypoints are set and the process ends.
  • step S344 the safety area determination unit 271 determines whether the safety area can be used to avoid the risk area based on the safety area data. Specifically, the safety area determination unit 271 determines whether or not there is a safety area on the opposite side of the risk area across the route to the reference waypoint. If there is a safety area opposite the risk area across the route to the reference waypoint, the safety area can be used to avoid the risk area. If a safe area is available to avoid the risk area, the process proceeds to step S345. If the safe area is not available to avoid the risk area, the process proceeds to step S346.
  • the detour curve setting unit 272 sets a representative point in the safety area and sets the detour curve.
  • the representative point is a point in the safety area.
  • the detour curve setting unit 272 sets an arc having a specific curvature through the reference waypoint and the safety area, and sets a representative point on the set arc.
  • the detour curve is a cubic curve that passes through the position of the mobility 110, the representative point, and the reference waypoint.
  • FIG. 17 shows an example of a representative point and a detour route.
  • the black circle represents the representative point, and the broken line represents the detour route.
  • the detour curve setting unit 272 sets a representative point in the safety area. Then, the detour curve setting unit 272 generates a detour route passing through the representative point and the reference waypoint ( Wi ).
  • step S346 will be described.
  • the area tangent setting unit 263 sets the area tangent based on the position of the mobility 110 and the risk area.
  • Step S346 is the same as step S244 of the second embodiment.
  • step S350 the target position determination unit 230 determines the target position based on the comparison result between the bending angle and the angle threshold value. Step S350 corresponds to step S140 of the first embodiment.
  • step S350 the target position determining unit 230 determines whether the bending angle is equal to or less than the angle threshold value based on the comparison result in step S330. If the bending angle is equal to or less than the angle threshold value, the process proceeds to step S352. If the bending angle is greater than the angle threshold, the process proceeds to step S353. However, the process may proceed to step S353 when the bending angle is equal to the angle threshold value.
  • the target position determination unit 230 determines the target position from the movement curve, the area tangent line, or the detour curve. Specifically, when the area tangent line and the detour curve are not set, the target position determination unit 230 determines the target position from the movement curve. Further, when the area tangent line is set and the detour curve is not set, the target position determination unit 230 determines the target position from the area tangent line. When a detour curve is set, the target position determination unit 230 determines the target position from the detour curve.
  • the target position is a position that is a reference distance ahead of the position of the mobility 110.
  • the target position determination unit 230 determines the target position from the movement curve, the area tangent line, or the detour curve. Specifically, when the area tangent line and the detour curve are not set, the target position determination unit 230 determines the target position from the movement curve. Further, when the area tangent line is set and the detour curve is not set, the target position determination unit 230 determines the target position from the area tangent line. When a detour curve is set, the target position determination unit 230 determines the target position from the detour curve. The target position is a position a short distance ahead of the position of the mobility 110.
  • step S360 the movement speed determination unit 240 determines the movement speed based on the comparison result between the bending angle and the angle threshold value. Step S360 is the same as step S150 of the first embodiment.
  • step S370 the output unit 250 outputs the target position data and the moving speed data to the vehicle control device 130.
  • Step S370 is the same as step S160 of the first embodiment.
  • the effect of the third embodiment will be described with reference to FIG. (1)
  • the bending angle of the route at the reference waypoint ( Wi ) is a right angle. Therefore, the bending angle is large.
  • a detour route passing through the safety area is set. This makes it possible to more reliably avoid a collision with an object. Further, since the detour route is a gentle route in which the mobility 110 does not make a sharp turn, the riding comfort of the mobility 110 is maintained.
  • the path tracking device 200 includes a processing circuit 209.
  • the processing circuit 209 is hardware that realizes the reception unit 210, the path right angle determination unit 220, the target position determination unit 230, the movement speed determination unit 240, the output unit 250, the collision risk determination unit 260, and the collision risk determination unit 260. ..
  • the processing circuit 209 may be dedicated hardware or a processor 201 that executes a program stored in the memory 202.
  • the processing circuit 209 is dedicated hardware, the processing circuit 209 is, for example, a single circuit, a composite circuit, a programmed processor, a parallel programmed processor, an ASIC, an FPGA, or a combination thereof.
  • ASIC is an abbreviation for Application Special Integrated Circuit.
  • FPGA is an abbreviation for Field Programmable Gate Array.
  • the path tracking device 200 may include a plurality of processing circuits that replace the processing circuit 209.
  • processing circuit 209 some functions may be realized by dedicated hardware, and the remaining functions may be realized by software or firmware.
  • the function of the path tracking device 200 can be realized by hardware, software, firmware, or a combination thereof.
  • Each embodiment is an example of a preferred embodiment and is not intended to limit the technical scope of the present disclosure. Each embodiment may be partially implemented or may be implemented in combination with other embodiments. The procedure described using the flowchart or the like may be appropriately changed.
  • the "part" which is an element of the path tracking device 200 may be read as “processing”, “process”, “circuit” or “circuit re”.
  • 100 mobility system 110 mobility, 120 sensor group, 130 vehicle control device, 140 vehicle, 200 route tracking device, 201 processor, 202 memory, 203 auxiliary storage device, 204 communication device, 205 input / output interface, 209 processing circuit, 210 reception Unit, 220 Path right angle determination unit, 221 Bend angle calculation unit, 222 Bend angle determination unit, 230 Target position determination unit, 240 Movement speed determination unit, 250 Output unit, 260 Collision risk determination unit, 261 Risk area setting unit, 262 Risk Area determination unit, 263 area tangent setting unit, 270 avoidance route setting unit, 271 safety area determination unit, 272 detour curve setting unit, 290 storage unit.

Landscapes

  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Human Computer Interaction (AREA)
  • Traffic Control Systems (AREA)
  • Navigation (AREA)

Abstract

経路追従装置（２００）は、複数のウェイポイントを示す経路データに基づいて、モビリティ（１１０）の位置からＸ番目のウェイポイントである基準ウェイポイントでの前記経路の曲がり角度を算出する。前記曲がり角度が前記角度閾値より小さい場合、経路追従装置は、前記モビリティの前記位置から基準距離だけ先の位置を、前記モビリティの次の移動先となる目標位置に決定する。前記曲がり角度が前記角度閾値より大きい場合、経路追従装置は、前記モビリティの前記位置から前記基準距離より短い短縮距離だけ先の位置を、前記目標位置に決定する。

Description

経路追従装置、経路追従方法および経路追従プログラム
　本開示は、モビリティの経路追従に関するものである。
　電動イスまたはシニアカーなどのモビリティを事前に設定された経路に追従させて移動することが望まれる。
　特許文献１は、モビリティを経路に追従して移動させるための方法を開示している。
国際公開ＷＯ２０１２／１６４６９１号
　本開示は、特許文献１の方向とは異なる方法でモビリティを経路に追従して移動させることを目的とする。
　本開示の経路追従装置は、
　目的地点までの経路に位置する複数のウェイポイントを示す経路データに基づいて、モビリティの位置から前記目的地点に向かって基準ポイント数番目のウェイポイントである基準ウェイポイントでの前記経路の曲がり角度を算出する曲がり角度算出部と、
　前記曲がり角度を角度閾値と比較する曲がり角度判定部と、
　前記曲がり角度が前記角度閾値より小さい場合に前記モビリティの前記位置から基準距離だけ先の位置を前記モビリティの次の移動先となる目標位置に決定し、前記曲がり角度が前記角度閾値より大きい場合に前記モビリティの前記位置から前記基準距離より短い短縮距離だけ先の位置を前記目標位置に決定する目標位置決定部と、を備える。
　本開示によれば、経路の曲がり角度に基づいて、モビリティを経路に追従して移動させることが可能となる。
実施の形態１におけるモビリティシステム１００の構成図。 実施の形態１における経路追従装置２００の構成図。 実施の形態１における経路追従方法のフローチャート。 実施の形態１におけるステップＳ１２０のフローチャート。 実施の形態１におけるステップＳ１４０のフローチャート。 実施の形態１におけるステップＳ１５０のフローチャート。 実施の形態１における効果の説明図。 実施の形態２における経路追従装置２００の構成図。 実施の形態２における経路追従方法のフローチャート。 実施の形態２におけるステップＳ２４０のフローチャート。 実施の形態２における中間点およびリスクエリアの例を示す図。 実施の形態２におけるステップＳ２５０のフローチャート。 実施の形態２における効果の説明図。 実施の形態３における経路追従装置２００の構成図。 実施の形態３にける経路追従方法のフローチャート。 実施の形態３におけるステップＳ３４０のフローチャート。 実施の形態３における代表点および迂回経路の例を示す図。 実施の形態３におけるステップＳ３５０のフローチャート。 実施の形態３における効果の説明図。 実施の形態における経路追従装置２００のハードウェア構成図。
　実施の形態および図面において、同じ要素または対応する要素には同じ符号を付している。説明した要素と同じ符号が付された要素の説明は適宜に省略または簡略化する。図中の矢印はデータの流れ又は処理の流れを主に示している。
　実施の形態１．
　モビリティシステム１００について、図１から図７に基づいて説明する。
＊＊＊構成の説明＊＊＊
　図１に基づいて、モビリティシステム１００の構成を説明する。
　モビリティシステム１００は、モビリティ１１０を備える。
　モビリティ１１０は、車体をその場で旋回させることができる車両であり、パーソナルモビリティともいう。モビリティ１１０の具体例は、電動車いすまたはシニアカーである。
　モビリティ１１０は、センサ群１２０と、経路追従装置２００と、車両制御装置１３０と、車両１４０と、を備える。これらのハードウェアは、信号線を介して互いに接続されている。
　センサ群１２０は、物体検知センサと測位センサなどを含む１つ以上のセンサである。
　物体検知センサは、モビリティ１１０の周囲に存在する物体を検知するためのセンサである。物体検知センサの具体例は、カメラおよびレーザセンサである。
　測位センサは、モビリティ１１０の位置を測位するためのセンサである。測位センサの具体例は、衛星測位システムの受信機および慣性計測装置である。
　経路追従装置２００は、モビリティ１１０の目標位置とモビリティ１１０の移動速度を決定する。
　目標位置は、モビリティ１１０の次の移動先となる位置であり、モビリティ１１０が目的地点に到達するまでの間に繰り返し決定される。
　移動速度は、モビリティ１１０が目標位置に到達するまでの間のモビリティ１１０の速度である。
　車両制御装置１３０は、モビリティ１１０を目標位置に移動するために、車両１４０を自動制御する。
　車両１４０は、モビリティ１１０の本体であり、速度制御装置および姿勢制御装置などを備える。
　但し、センサ群１２０は、モビリティ１１０に設けられずに、例えば道路の各所に設けられてもよい。
　また、経路追従装置２００は、モビリティ１１０の外部に設けられてもよい。その場合、モビリティ１１０は、経路追従装置２００と通信するための通信装置を備える。
　図２に基づいて、経路追従装置２００の構成を説明する。
　経路追従装置２００は、プロセッサ２０１とメモリ２０２と補助記憶装置２０３と通信装置２０４と入出力インタフェース２０５といったハードウェアを備えるコンピュータである。これらのハードウェアは、信号線を介して互いに接続されている。
　プロセッサ２０１は、演算処理を行うＩＣであり、他のハードウェアを制御する。例えば、プロセッサ２０１は、ＣＰＵ、ＤＳＰまたはＧＰＵである。
　ＩＣは、Ｉｎｔｅｇｒａｔｅｄ　Ｃｉｒｃｕｉｔの略称である。
　ＣＰＵは、Ｃｅｎｔｒａｌ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔの略称である。
　ＤＳＰは、Ｄｉｇｉｔａｌ　Ｓｉｇｎａｌ　Ｐｒｏｃｅｓｓｏｒの略称である。
　ＧＰＵは、Ｇｒａｐｈｉｃｓ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔの略称である。
　メモリ２０２は揮発性または不揮発性の記憶装置である。メモリ２０２は、主記憶装置またはメインメモリとも呼ばれる。例えば、メモリ２０２はＲＡＭである。メモリ２０２に記憶されたデータは必要に応じて補助記憶装置２０３に保存される。
　ＲＡＭは、Ｒａｎｄｏｍ　Ａｃｃｅｓｓ　Ｍｅｍｏｒｙの略称である。
　補助記憶装置２０３は不揮発性の記憶装置である。例えば、補助記憶装置２０３は、ＲＯＭ、ＨＤＤまたはフラッシュメモリである。補助記憶装置２０３に記憶されたデータは必要に応じてメモリ２０２にロードされる。
　ＲＯＭは、Ｒｅａｄ　Ｏｎｌｙ　Ｍｅｍｏｒｙの略称である。
　ＨＤＤは、Ｈａｒｄ　Ｄｉｓｋ　Ｄｒｉｖｅの略称である。
　通信装置２０４はレシーバ及びトランスミッタである。例えば、通信装置２０４は通信チップまたはＮＩＣである。経路追従装置２００の通信は通信装置２０４を用いて行われる。
　ＮＩＣは、Ｎｅｔｗｏｒｋ　Ｉｎｔｅｒｆａｃｅ　Ｃａｒｄの略称である。
　入出力インタフェース２０５は、入力装置および出力装置が接続されるポートである。例えば、入出力インタフェース２０５はＵＳＢ端子である。経路追従装置２００の入出力は入出力インタフェース２０５または通信装置２０４を用いて行われる。
　ＵＳＢは、Ｕｎｉｖｅｒｓａｌ　Ｓｅｒｉａｌ　Ｂｕｓの略称である。
　経路追従装置２００は、受付部２１０と、経路直角判定部２２０と目標位置決定部２３０と移動速度決定部２４０と出力部２５０といった要素を備える。経路直角判定部２２０は、曲がり角度算出部２２１と曲がり角度判定部２２２といった要素を備える。これらの要素はソフトウェアで実現される。
　補助記憶装置２０３には、受付部２１０と経路直角判定部２２０と目標位置決定部２３０と移動速度決定部２４０と出力部２５０としてコンピュータを機能させるための経路追従プログラムが記憶されている。経路追従プログラムは、メモリ２０２にロードされて、プロセッサ２０１によって実行される。
　補助記憶装置２０３には、さらに、ＯＳが記憶されている。ＯＳの少なくとも一部は、メモリ２０２にロードされて、プロセッサ２０１によって実行される。
　プロセッサ２０１は、ＯＳを実行しながら、経路追従プログラムを実行する。
　ＯＳは、Ｏｐｅｒａｔｉｎｇ　Ｓｙｓｔｅｍの略称である。
　経路追従プログラムの入出力データは記憶部２９０に記憶される。
　メモリ２０２は記憶部２９０として機能する。但し、補助記憶装置２０３、プロセッサ２０１内のレジスタおよびプロセッサ２０１内のキャッシュメモリなどの記憶装置が、メモリ２０２の代わりに、又は、メモリ２０２と共に、記憶部２９０として機能してもよい。
　経路追従装置２００は、プロセッサ２０１を代替する複数のプロセッサを備えてもよい。
　経路追従プログラムは、光ディスクまたはフラッシュメモリ等の不揮発性の記録媒体にコンピュータ読み取り可能に記録（格納）することができる。
＊＊＊動作の説明＊＊＊
　経路追従装置２００の動作の手順は経路追従方法に相当する。また、経路追従装置２００の動作の手順は経路追従プログラムによる処理の手順に相当する。
　図３に基づいて、経路追従方法を説明する。
　ステップＳ１１０において、受付部２１０は、各種データを受け付け、各種データを記憶部２９０に記憶する。
　各種データの具体例は、経路データおよび位置データなどである。
　経路データは、開始地点から目的地点までの経路に位置する複数のウェイポイントを示す。開始地点から目的地点までの経路を「長期経路」と称する。
　位置データは、モビリティ１１０の位置（現在位置）を示す。モビリティ１１０の位置は、センサ群１２０の測位センサによって測位される。
　ステップＳ１２０において、曲がり角度算出部２２１は、経路データに基づいて基準ウェイポイントでの経路の曲がり角度を算出する。
　基準ウェイポイントは、モビリティ１１０の位置から目的地点に向かって基準ポイント数番目のウェイポイントである。基準ポイント数は予め決められる。
　曲がり角度は、経路の曲がりの大きさを表す。基準ウェイポイントでの曲がり角度は、モビリティ１１０が基準ウェイポイントを通過する際のモビリティ１１０の旋回角度に相当する。
　図４に基づいて、ステップＳ１２０の手順を説明する。
　ステップＳ１２１において、曲がり角度算出部２２１は、モビリティ１１０の位置に基づいて、経路データに示される複数のウェイポイントから、基準ウェイポイントを決定する。
　具体的には、曲がり角度算出部２２１は、モビリティ１１０の位置から数えてＸ番目のウェイポイントを基準ウェイポイントに決定する。「Ｘ」は基準ポイント数を表す。
　ステップＳ１２２において、曲がり角度算出部２２１は、経路データから、基準ウェイポイントの座標値を取得する。
　ステップＳ１２３において、曲がり角度算出部２２１は、経路データから、手前ウェイポイントの座標値と、先ウェイポイントの座標値と、を取得する。
　手前ウェイポイントは、基準ウェイポイントの手前に位置するウェイポイントである。具体的には、手前ウェイポイントは、モビリティ１１０の位置から数えて（Ｘ－１）番目のウェイポイントである。
　先ウェイポイントは、基準ウェイポイントの先に位置するウェイポイントである。具体的には、先ウェイポイントは、モビリティ１１０の位置から数えて（Ｘ＋１）番目のウェイポイントである。
　ステップＳ１２４において、曲がり角度算出部２２１は、基準ウェイポイントの座標値と、手前ウェイポイントの座標値と、先ウェイポイントの座標値と、に基づいて、基準ウェイポイントでの経路の曲がり角度を算出する。
　例えば、曲がり角度算出部２２１は、手前ウェイポイントから基準ウェイポイントへのベクトルと、基準ウェイポイントから先ウェイポイントへのベクトルと、を算出する。そして、曲がり角度算出部２２１は、２つのベクトルが成す角度を算出する。算出される角度が曲がり角度である。
　曲がり角度θは、式（１）で表すことができる。
　「Ｗｉ」は、基準ウェイポイントの位置を表す。
　「Ｗｉ－１」は、手前ウェイポイントの位置を表す。
　「Ｗｉ＋１」は、先ウェイポイントの位置を表す。
Figure JPOXMLDOC01-appb-M000001
　図３に戻り、ステップＳ１３０から説明を続ける。
　ステップＳ１３０において、曲がり角度判定部２２２は、曲がり角度を角度閾値と比較する。角度閾値は、予め決められた角度である。具体的には、角度閾値は９０度に近い角度である。
　ステップＳ１４０において、目標位置決定部２３０は、曲がり角度と角度閾値との比較結果に基づいて、目標位置を決定する。
　図５に基づいて、ステップＳ１４０の手順を説明する。
　ステップＳ１４１において、目標位置決定部２３０は、モビリティ１１０の移動曲線を算出する。
　移動曲線は、基準ウェイポイントの前後の経路に沿った曲線であり、モビリティ１１０が基準ウェイポイントの前後の経路を通過する際のモビリティ１１０の軌跡を表す。
　具体的には、目標位置決定部２３０は、モビリティ１１０の位置と、手前ウェイポイントの座標値と、基準ウェイポイントの座標値と、先ウェイポイントの座標値と、に基づいて移動曲線を算出する。例えば、移動曲線は、モビリティ１１０の位置を始点とし、先ウェイポイントを終点とし、特定の曲率を有する円弧である。なお、移動曲線の算出方法は任意である。
　ステップＳ１４２において、目標位置決定部２３０は、ステップＳ１３０の比較結果に基づいて、曲がり角度が角度閾値以下であるか判定する。
　曲がり角度が角度閾値以下である場合、処理はステップＳ１４３に進む。
　曲がり角度が角度閾値より大きい場合、処理はステップＳ１４４に進む。
　但し、曲がり角度が角度閾値と等しい場合に処理がステップＳ１４４に進むようにしてもよい。
　ステップＳ１４３において、目標位置決定部２３０は、移動曲線から目標位置を決定する。具体的には、目標位置決定部２３０は、モビリティ１１０の位置から基準距離だけ先の位置を目標位置に決定する。
　基準距離は、予め決められた距離である。
　ステップＳ１４４において、目標位置決定部２３０は、移動曲線から目標位置を決定する。具体的には、目標位置決定部２３０は、モビリティ１１０の位置から短縮距離だけ先の位置を目標位置に決定する。
　短縮距離は、基準距離より短い距離であり、予め決められる。例えば、基準距離が１メートルである場合、短縮距離は、基準距離の半分の０．５メートルである。
　図３に戻り、ステップＳ１５０から説明を続ける。
　ステップＳ１５０において、移動速度決定部２４０は、曲がり角度と角度閾値との比較結果に基づいて、移動速度を決定する。
　図６に基づいて、ステップＳ１５０の手順を説明する。
　ステップＳ１５１において、移動速度決定部２４０は、ステップＳ１３０の比較結果に基づいて、曲がり角度が角度閾値以下であるか判定する。
　曲がり角度が角度閾値以下である場合、処理はステップＳ１５２に進む。
　曲がり角度が角度閾値より大きい場合、処理はステップＳ１５３に進む。
　但し、曲がり角度が角度閾値と等しい場合に処理がステップＳ１５３に進むようにしてもよい。
　ステップＳ１５２において、移動速度決定部２４０は、基準速度を移動速度に決定する。
　基準速度は、予め決められた速度である。
　ステップＳ１５３において、移動速度決定部２４０は、減速速度を移動速度に決定する。
　減速速度は、基準速度より遅い速度であり、予め決められる。例えば、減速速度は、基準速度の０．２倍である。
　図３に戻り、ステップＳ１６０を説明する。
　ステップＳ１６０において、出力部２５０は、目標位置データおよび移動速度データを車両制御装置１３０に出力する。
　目標位置データは、ステップＳ１４０で決定された目標位置を示す。
　移動速度データは、ステップＳ１５０で決定された移動速度を示す。
　車両制御装置１３０は、車両１４０を制御することによって、モビリティ１１０を決定された移動速度で決定された目標位置に移動する。
　ステップＳ１１０からステップＳ１６０は、繰り返し実行される。但し、ステップＳ１１０において、経路データの受け付けは最初の１回のみで構わない。
＊＊＊実施の形態１の効果＊＊＊
　図７に基づいて、実施の形態１の効果を説明する。白丸はウェイポイントを表し、網掛けの丸は目標位置を表す。複数のウェイポイントを繋ぐ実線は長期経路を表す。また、矢印線はモビリティ１１０の移動軌跡を表す。
　（１）基準ウェイポイント（Ｗｉ）での経路の曲がり角度が直角である。そのため、曲がり角度が大きい。この場合、目標位置が近くなければ、モビリティ１１０は、経路から大きく内側にずれた曲線を移動することとなる。
　（２）一方、実施の形態１では、曲がり角度が大きい場合に目標位置が近くの位置に変更される。これにより、モビリティ１１０は、経路に近い曲線を移動することとなる。つまり、モビリティ１１０を経路に追従させながら移動することが可能となる。また、実施の形態１では、曲がり角度が大きい場合に移動速度が遅い速度位置に変更される。これにより、モビリティ１１０の推進力が小さくなるため、モビリティ１１０の横方向のブレが低減し、モビリティ１１０の乗り心地が良くなる。
　また、特許文献１の方法では、経路がジグザグである場合にモビリティがジグザグに移動するため、モビリティの乗り心地が悪くなってしまう。
　一方、実施の形態１では、長期経路がジグザグな経路であってもモビリティ１１０をジグザグに移動させずに滑らかな曲線上を移動させることが可能となる。そのため、モビリティ１１０の乗り心地が良くなる。
　実施の形態２．
　モビリティ１１０の周囲に存在する物体との衝突を回避するための形態について、主に実施の形態１と異なる点を図８から図１３に基づいて説明する。
＊＊＊構成の説明＊＊＊
　モビリティシステム１００の構成は、実施の形態１における構成と同じである（図１を参照）。
　図８に基づいて、経路追従装置２００の構成を説明する。
　経路追従装置２００は、さらに、衝突リスク判定部２６０を備える。衝突リスク判定部２６０は、リスクエリア設定部２６１とリスクエリア判定部２６２とエリア接線設定部２６３といった要素を備える。
　経路追従プログラムは、さらに、衝突リスク判定部２６０としてコンピュータを機能させる。
＊＊＊動作の説明＊＊＊
　図９に基づいて、経路追従方法を説明する。
　ステップＳ２１０において、受付部２１０は、各種データを受け付ける。ステップＳ２１０は、実施の形態１のステップＳ１１０に相当する。
　各種データには、物体検知データが含まれる。
　物体検知データは、モビリティ１１０の周囲に存在する物体が検知された範囲を示す。モビリティ１１０の周囲に存在する物体はセンサ群１２０の物体検知センサによって検知される。
　ステップＳ２２０において、曲がり角度算出部２２１は、経路データに基づいて基準ウェイポイントでの経路の曲がり角度を算出する。
　ステップＳ２２０は、実施の形態１のステップＳ１２０と同じである。
　ステップＳ２３０において、曲がり角度判定部２２２は、曲がり角度を角度閾値と比較する。
　ステップＳ２３０は、実施の形態１のステップＳ１３０と同じである。
　ステップＳ２４０において、エリア接線設定部２６３は、後述するエリア接線を設定する。
　図１０に基づいて、ステップＳ２４０の手順を説明する。
　ステップＳ２４１において、リスクエリア設定部２６１は、物体検知データに基づいて、リスクエリアを設定する。
　リスクエリアは、モビリティ１１０が物体と衝突するリスクがあるエリアである。
　具体的には、リスクエリア設定部２６１は、物体の中間点を算出する。そして、リスクエリア設定部２６１は、物体の中間点を中心とし、物体の長さ方向を長軸の方向とし、物体の長さ方向に対する法線方向を短軸の方向とする楕円状のエリアを算出する。算出されるエリアがリスクエリアである。
　図１１に、中間点およびリスクエリアの例を示す。
　図１１において、モビリティ１１０の右前方に壁が存在し、物体検知データは検知された壁を示す。壁は物体の一例である。
　リスクエリア設定部２６１は、壁の中間点を算出する。そして、リスクエリア設定部２６１は、中間点を中心とする楕円状のエリアをリスクエリアとして算出する。
　図１０に戻り、ステップＳ２４２から説明を続ける。
　ステップＳ２４２において、目標位置決定部２３０は、モビリティ１１０の移動曲線を算出する。
　ステップＳ２４２は、実施の形態１のステップＳ１４１と同じである。
　ステップＳ２４３において、リスクエリア判定部２６２は、移動曲線がリスクエリアを通るか判定する。
　移動曲線がリスクエリアを通る場合、処理はステップＳ２４４に進む。
　移動曲線がリスクエリアを通らない場合、エリア接線は設定されず、処理は終了する。
　ステップＳ２４４において、エリア接線設定部２６３は、モビリティ１１０の位置と、リスクエリアと、に基づいて、エリア接線を設定する。
　エリア接線は、モビリティ１１０の位置を通りリスクエリアと接する接線である。
　図９に戻り、ステップＳ２５０から説明を続ける。
　ステップＳ２５０において、目標位置決定部２３０は、曲がり角度と角度閾値との比較結果に基づいて、目標位置を決定する。
　ステップＳ２５０は、実施の形態１のステップＳ１４０に相当する。
　図１２に基づいて、ステップＳ２５０の手順を説明する。
　ステップＳ２５１において、目標位置決定部２３０は、ステップＳ２３０の比較結果に基づいて、曲がり角度が角度閾値以下であるか判定する。
　曲がり角度が角度閾値以下である場合、処理はステップＳ２５２に進む。
　曲がり角度が角度閾値より大きい場合、処理はステップＳ２５３に進む。
　但し、曲がり角度が角度閾値と等しい場合に処理がステップＳ２５３に進むようにしてもよい。
　ステップＳ２５２において、目標位置決定部２３０は、移動曲線またはエリア接線から目標位置を決定する。
　具体的には、エリア接線が設定されなかった場合、目標位置決定部２３０は、移動曲線から目標位置を決定する。また、エリア接線が設定された場合、目標位置決定部２３０は、エリア接線から目標位置を決定する。
　目標位置は、モビリティ１１０の位置から基準距離だけ先の位置である。
　ステップＳ２５３において、目標位置決定部２３０は、移動曲線またはエリア接線から目標位置を決定する。
　具体的には、エリア接線が設定されなかった場合、目標位置決定部２３０は、移動曲線から目標位置を決定する。また、エリア接線が設定された場合、目標位置決定部２３０は、エリア接線から目標位置を決定する。
　目標位置は、モビリティ１１０の位置から短縮距離だけ先の位置である。
　図９に戻り、ステップＳ２６０から説明を続ける。
　ステップＳ２６０において、移動速度決定部２４０は、曲がり角度と角度閾値との比較結果に基づいて、移動速度を決定する。
　ステップＳ２６０は、実施の形態１のステップＳ１５０と同じである。
　ステップＳ２７０において、出力部２５０は、目標位置データおよび移動速度データを車両制御装置１３０に出力する。
　ステップＳ２７０は、実施の形態１のステップＳ１６０と同じである。
＊＊＊実施の形態２の効果＊＊＊
　図１３に基づいて、実施の形態２の効果を説明する。
　（１）基準ウェイポイント（Ｗｉ）での経路の曲がり角度が直角である。そのため、曲がり角度が大きい。また、経路の内側に壁が存在する。この場合、目標位置が近くなければ、モビリティ１１０は、経路から大きく内側にずれた曲線を移動して壁に衝突することとなる。
　（２）一方、実施の形態２では、壁のような物体が存在する場合に衝突リスクがあるエリアの接線上に目標位置が設定される。これにより、経路に対する追従性を確保しつつ衝突リスクを回避することが可能となる。
　実施の形態３．
　経路を迂回して物体との衝突を回避する形態について、主に実施の形態１および実施の形態２と異なる点を図１４から図１９に基づいて説明する。
＊＊＊構成の説明＊＊＊
　モビリティシステム１００の構成は、実施の形態１における構成と同じである（図１を参照）。
　図１４に基づいて、経路追従装置２００の構成を説明する。
　経路追従装置２００は、さらに、回避経路設定部２７０を備える。回避経路設定部２７０は、安全エリア判定部２７１と迂回曲線設定部２７２といった要素を備える。
　経路追従プログラムは、さらに、回避経路設定部２７０としてコンピュータを機能させる。
＊＊＊動作の説明＊＊＊
　図１５に基づいて、経路追従方法を説明する。
　ステップＳ３１０において、受付部２１０は、各種データを受け付ける。ステップＳ２１０は、実施の形態１のステップＳ１１０に相当する。
　各種データには、安全エリアデータが含まれる。
　安全エリアデータは、物体が検知されなかったエリアである。
　ステップＳ３２０において、曲がり角度算出部２２１は、経路データに基づいて基準ウェイポイントでの経路の曲がり角度を算出する。
　ステップＳ３２０は、実施の形態１のステップＳ１２０と同じである。
　ステップＳ３３０において、曲がり角度判定部２２２は、曲がり角度を角度閾値と比較する。
　ステップＳ３３０は、実施の形態１のステップＳ１３０と同じである。
　ステップＳ３４０において、エリア接線設定部２６３は、エリア接線または迂回曲線を設定する。
　図１６に基づいて、ステップＳ３４０の手順を説明する。
　ステップＳ３４１において、リスクエリア設定部２６１は、物体検知データに基づいて、リスクエリアを設定する。
　ステップＳ３４１は、実施の形態２のステップＳ２４１と同じである。
　ステップＳ３４２において、目標位置決定部２３０は、モビリティ１１０の移動曲線を算出する。
　ステップＳ３４２は、実施の形態１のステップＳ１４１と同じである。
　ステップＳ３４３において、リスクエリア判定部２６２は、移動曲線がリスクエリアを通るか判定する。ステップＳ３４３は、実施の形態２のステップＳ２４３に相当する。
　移動曲線がリスクエリアを通る場合、処理はステップＳ３４４に進む。
　移動曲線がリスクエリアを通らない場合、エリア接線および新たなウェイポイントは設定されず、処理は終了する。
　ステップＳ３４４において、安全エリア判定部２７１は、安全エリアデータに基づいて、リスクエリアを回避するために安全エリアを利用できるか判定する。
　具体的には、安全エリア判定部２７１は、基準ウェイポイントへの経路を挟んでリスクエリアの向かい側に安全エリアがあるか判定する。基準ウェイポイントへの経路を挟んでリスクエリアの向かい側に安全エリアがある場合、リスクエリアを回避するために安全エリアを利用できる。
　リスクエリアを回避するために安全エリアを利用できる場合、処理はステップＳ３４５に進む。
　リスクエリアを回避するために安全エリアを利用できない場合、処理はステップＳ３４６に進む。
　ステップＳ３４５において、迂回曲線設定部２７２は、安全エリアの中に代表点を設定し、迂回曲線を設定する。
　代表点は、安全エリアの中の地点である。例えば、迂回曲線設定部２７２は、基準ウェイポイントと安全エリアを通り特定の曲率を有する円弧を設定し、設定した円弧上に代表点を設定する。
　迂回曲線は、モビリティ１１０の位置と、代表点と、基準ウェイポイントと、を通る３次曲線である。
　図１７に、代表点および迂回経路の例を示す。黒丸は代表点を表し、破線は迂回経路を表す。
　図１７において、基準ウェイポイント（Ｗｉ）への経路を挟んでリスクエリアの向かい側に安全エリアが存在する。
　迂回曲線設定部２７２は、安全エリアの中に代表点を設定する。そして、迂回曲線設定部２７２は、代表点と基準ウェイポイント（Ｗｉ）とを通る迂回経路を生成する。
　図１６に戻り、ステップＳ３４６を説明する。
　ステップＳ３４６において、エリア接線設定部２６３は、モビリティ１１０の位置と、リスクエリアと、に基づいて、エリア接線を設定する。
　ステップＳ３４６は、実施の形態２のステップＳ２４４と同じである。
　図１５に戻り、ステップＳ３５０から説明を続ける。
　ステップＳ３５０において、目標位置決定部２３０は、曲がり角度と角度閾値との比較結果に基づいて、目標位置を決定する。
　ステップＳ３５０は、実施の形態１のステップＳ１４０に相当する。
　図１８に基づいて、ステップＳ３５０の手順を説明する。
　ステップＳ３５１において、目標位置決定部２３０は、ステップＳ３３０の比較結果に基づいて、曲がり角度が角度閾値以下であるか判定する。
　曲がり角度が角度閾値以下である場合、処理はステップＳ３５２に進む。
　曲がり角度が角度閾値より大きい場合、処理はステップＳ３５３に進む。
　但し、曲がり角度が角度閾値と等しい場合に処理がステップＳ３５３に進むようにしてもよい。
　ステップＳ３５２において、目標位置決定部２３０は、移動曲線、エリア接線または迂回曲線から目標位置を決定する。
　具体的には、エリア接線および迂回曲線が設定されなかった場合、目標位置決定部２３０は、移動曲線から目標位置を決定する。また、エリア接線が設定されて迂回曲線が設定されなかった場合、目標位置決定部２３０は、エリア接線から目標位置を決定する。また、迂回曲線が設定された場合、目標位置決定部２３０は、迂回曲線から目標位置を決定する。
　目標位置は、モビリティ１１０の位置から基準距離だけ先の位置である。
　ステップＳ３５３において、目標位置決定部２３０は、移動曲線、エリア接線または迂回曲線から目標位置を決定する。
　具体的には、エリア接線および迂回曲線が設定されなかった場合、目標位置決定部２３０は、移動曲線から目標位置を決定する。また、エリア接線が設定されて迂回曲線が設定されなかった場合、目標位置決定部２３０は、エリア接線から目標位置を決定する。また、迂回曲線が設定された場合、目標位置決定部２３０は、迂回曲線から目標位置を決定する。
　目標位置は、モビリティ１１０の位置から短縮距離だけ先の位置である。
　図１５に戻り、ステップＳ３６０から説明を続ける。
　ステップＳ３６０において、移動速度決定部２４０は、曲がり角度と角度閾値との比較結果に基づいて、移動速度を決定する。
　ステップＳ３６０は、実施の形態１のステップＳ１５０と同じである。
　ステップＳ３７０において、出力部２５０は、目標位置データおよび移動速度データを車両制御装置１３０に出力する。
　ステップＳ３７０は、実施の形態１のステップＳ１６０と同じである。
＊＊＊実施の形態３の効果＊＊＊
　図１９に基づいて、実施の形態３の効果を説明する。
　（１）基準ウェイポイント（Ｗｉ）での経路の曲がり角度が直角である。そのため、曲がり角度が大きい。また、経路の内側に壁が存在する。この場合、目標位置が近くなければ、モビリティ１１０は、経路から大きく内側にずれた曲線を移動して壁に衝突することとなる。
　（２）モビリティ１１０を迂回させることが可能な安全エリアが存在する。実施の形態３では、安全エリアが存在する場合に安全エリアを通過する迂回経路が設定される。これにより、物体との衝突をより確実に回避することができる。また、迂回経路は、モビリティ１１０が急旋回しない緩やかな経路であるため、モビリティ１１０の乗り心地が維持される。
＊＊＊実施の形態の補足＊＊＊
　図２０に基づいて、経路追従装置２００のハードウェア構成を説明する。
　経路追従装置２００は処理回路２０９を備える。
　処理回路２０９は、受付部２１０と経路直角判定部２２０と目標位置決定部２３０と移動速度決定部２４０と出力部２５０と衝突リスク判定部２６０と衝突リスク判定部２６０とを実現するハードウェアである。
　処理回路２０９は、専用のハードウェアであってもよいし、メモリ２０２に格納されるプログラムを実行するプロセッサ２０１であってもよい。
　処理回路２０９が専用のハードウェアである場合、処理回路２０９は、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ＡＳＩＣ、ＦＰＧＡまたはこれらの組み合わせである。
　ＡＳＩＣは、Ａｐｐｌｉｃａｔｉｏｎ　Ｓｐｅｃｉｆｉｃ　Ｉｎｔｅｇｒａｔｅｄ　Ｃｉｒｃｕｉｔの略称である。
　ＦＰＧＡは、Ｆｉｅｌｄ　Ｐｒｏｇｒａｍｍａｂｌｅ　Ｇａｔｅ　Ａｒｒａｙの略称である。
　経路追従装置２００は、処理回路２０９を代替する複数の処理回路を備えてもよい。
　処理回路２０９において、一部の機能が専用のハードウェアで実現されて、残りの機能がソフトウェアまたはファームウェアで実現されてもよい。
　このように、経路追従装置２００の機能はハードウェア、ソフトウェア、ファームウェアまたはこれらの組み合わせで実現することができる。
　各実施の形態は、好ましい形態の例示であり、本開示の技術的範囲を制限することを意図するものではない。各実施の形態は、部分的に実施してもよいし、他の形態と組み合わせて実施してもよい。フローチャート等を用いて説明した手順は、適宜に変更してもよい。
　経路追従装置２００の要素である「部」は、「処理」、「工程」、「回路」または「サーキットリ」と読み替えてもよい。
　１００　モビリティシステム、１１０　モビリティ、１２０　センサ群、１３０　車両制御装置、１４０　車両、２００　経路追従装置、２０１　プロセッサ、２０２　メモリ、２０３　補助記憶装置、２０４　通信装置、２０５　入出力インタフェース、２０９　処理回路、２１０　受付部、２２０　経路直角判定部、２２１　曲がり角度算出部、２２２　曲がり角度判定部、２３０　目標位置決定部、２４０　移動速度決定部、２５０　出力部、２６０　衝突リスク判定部、２６１　リスクエリア設定部、２６２　リスクエリア判定部、２６３　エリア接線設定部、２７０　回避経路設定部、２７１　安全エリア判定部、２７２　迂回曲線設定部、２９０　記憶部。

Claims (7)

  1. 　目的地点までの経路に位置する複数のウェイポイントを示す経路データに基づいて、モビリティの位置から前記目的地点に向かって基準ポイント数番目のウェイポイントである基準ウェイポイントでの前記経路の曲がり角度を算出する曲がり角度算出部と、
    　前記曲がり角度を角度閾値と比較する曲がり角度判定部と、
    　前記曲がり角度が前記角度閾値より小さい場合に前記モビリティの前記位置から基準距離だけ先の位置を前記モビリティの次の移動先となる目標位置に決定し、前記曲がり角度が前記角度閾値より大きい場合に前記モビリティの前記位置から前記基準距離より短い短縮距離だけ先の位置を前記目標位置に決定する目標位置決定部と、
    を備える経路追従装置。
  2. 　前記曲がり角度算出部は、前記経路データから、前記基準ウェイポイントの座標値と、前記基準ウェイポイントの手前に位置する手前ウェイポイントの座標値と、前記基準ウェイポイントの先に位置する先ウェイポイントの座標値と、を取得し、取得した座標値に基づいて前記曲がり角度を算出する
    請求項１に記載の経路追従装置。
  3. 　前記モビリティの周囲に存在する物体が検知された範囲を示す物体検知データに基づいて、前記モビリティが前記物体と衝突するリスクがあるリスクエリアを設定するリスクエリア設定部と、
    　前記基準ウェイポイントの前後の前記経路に沿う移動曲線が前記リスクエリアを通るか判定するリスクエリア判定部と、
    　前記移動曲線が前記リスクエリアを通る場合に、前記モビリティの前記位置を通り前記リスクエリアと接するエリア接線を設定するエリア接線設定部と、を備え、
    　前記目標位置決定部は、前記エリア接線が設定された場合に前記エリア接線から前記目標位置を決定する
    請求項１または請求項２に記載の経路追従装置。
  4. 　前記リスクエリア設定部は、前記物体の中間点を中心とし、前記物体の長さ方向を長軸の方向とし、前記物体の前記長さ方向に対する法線方向を短軸の方向とする楕円状のエリアを前記リスクエリアとして設定する
    請求項３に記載の経路追従装置。
  5. 　前記移動曲線が前記リスクエリアを通る場合に、前記物体が検知されなかった安全エリアを示す安全エリアデータに基づいて、前記リスクエリアを回避するために前記安全エリアを利用できるか判定する安全エリア判定部と、
    　前記リスクエリアを回避するために前記安全エリアを利用できる場合に、前記モビリティの前記位置と、前記安全エリアの中の代表点と、前記基準ウェイポイントと、を通る迂回曲線を設定する迂回曲線設定部と、を備え、
    　前記目標位置決定部は、前記迂回曲線が設定された場合に前記迂回曲線から前記目標位置を決定する
    請求項３または請求項４に記載の経路追従装置。
  6. 　曲がり角度算出部が、目的地点までの経路に位置する複数のウェイポイントを示す経路データに基づいて、モビリティの位置から前記目的地点に向かって基準ポイント数番目のウェイポイントである基準ウェイポイントでの前記経路の曲がり角度を算出し、
    　曲がり角度判定部が、前記曲がり角度を角度閾値と比較し、
    　目標位置決定部が、前記曲がり角度が前記角度閾値より小さい場合に前記モビリティの前記位置から基準距離だけ先の位置を前記モビリティの次の移動先となる目標位置に決定し、前記曲がり角度が前記角度閾値より大きい場合に前記モビリティの前記位置から前記基準距離より短い短縮距離だけ先の位置を前記目標位置に決定する
    経路追従方法。
  7. 　目的地点までの経路に位置する複数のウェイポイントを示す経路データに基づいて、モビリティの位置から前記目的地点に向かって基準ポイント数番目のウェイポイントである基準ウェイポイントでの前記経路の曲がり角度を算出する曲がり角度算出処理と、
    　前記曲がり角度を角度閾値と比較する曲がり角度判定処理と、
    　前記曲がり角度が前記角度閾値より小さい場合に前記モビリティの前記位置から基準距離だけ先の位置を前記モビリティの次の移動先となる目標位置に決定し、前記曲がり角度が前記角度閾値より大きい場合に前記モビリティの前記位置から前記基準距離より短い短縮距離だけ先の位置を前記目標位置に決定する目標位置決定処理と、
    をコンピュータに実行させるための経路追従プログラム。
PCT/JP2021/000864 2021-01-13 2021-01-13 経路追従装置、経路追従方法および経路追従プログラム WO2022153402A1 (ja)

Priority Applications (5)

Application Number Priority Date Filing Date Title
DE112021005924.6T DE112021005924T5 (de) 2021-01-13 2021-01-13 Route-verfolgungsvorrichtung, route-verfolgungsverfahren und route-verfolgungsprogramm
JP2022556068A JP7191281B1 (ja) 2021-01-13 2021-01-13 経路追従装置、経路追従方法および経路追従プログラム
PCT/JP2021/000864 WO2022153402A1 (ja) 2021-01-13 2021-01-13 経路追従装置、経路追従方法および経路追従プログラム
CN202180088717.4A CN116802103A (zh) 2021-01-13 2021-01-13 路径追随装置、路径追随方法和路径追随程序
US18/195,162 US20230273616A1 (en) 2021-01-13 2023-05-09 Route following apparatus, route following method, and computer readable medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/JP2021/000864 WO2022153402A1 (ja) 2021-01-13 2021-01-13 経路追従装置、経路追従方法および経路追従プログラム

Related Child Applications (1)

Application Number Title Priority Date Filing Date
US18/195,162 Continuation US20230273616A1 (en) 2021-01-13 2023-05-09 Route following apparatus, route following method, and computer readable medium

Publications (1)

Publication Number Publication Date
WO2022153402A1 true WO2022153402A1 (ja) 2022-07-21

Family

ID=82448008

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2021/000864 WO2022153402A1 (ja) 2021-01-13 2021-01-13 経路追従装置、経路追従方法および経路追従プログラム

Country Status (5)

Country Link
US (1) US20230273616A1 (ja)
JP (1) JP7191281B1 (ja)
CN (1) CN116802103A (ja)
DE (1) DE112021005924T5 (ja)
WO (1) WO2022153402A1 (ja)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2005211442A (ja) * 2004-01-30 2005-08-11 Tottori Univ 自律移動型車椅子
JP2019043395A (ja) * 2017-09-04 2019-03-22 日産自動車株式会社 運転支援車両の走行制御方法及び走行制御装置
JP2019108097A (ja) * 2017-12-20 2019-07-04 アイシン・エィ・ダブリュ株式会社 自動運転制御システムおよび自動運転制御プログラム
JP2019108098A (ja) * 2017-12-20 2019-07-04 アイシン・エィ・ダブリュ株式会社 自動運転制御システムおよび自動運転制御プログラム
JP2020009085A (ja) * 2018-07-06 2020-01-16 パナソニックＩｐマネジメント株式会社 移動体制御システム、移動体システム、移動体制御方法、及びプログラム
JP2020148506A (ja) * 2019-03-11 2020-09-17 本田技研工業株式会社 移動支援装置および移動支援システム

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP5830533B2 (ja) 2011-05-31 2015-12-09 株式会社日立製作所 自律移動システム

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2005211442A (ja) * 2004-01-30 2005-08-11 Tottori Univ 自律移動型車椅子
JP2019043395A (ja) * 2017-09-04 2019-03-22 日産自動車株式会社 運転支援車両の走行制御方法及び走行制御装置
JP2019108097A (ja) * 2017-12-20 2019-07-04 アイシン・エィ・ダブリュ株式会社 自動運転制御システムおよび自動運転制御プログラム
JP2019108098A (ja) * 2017-12-20 2019-07-04 アイシン・エィ・ダブリュ株式会社 自動運転制御システムおよび自動運転制御プログラム
JP2020009085A (ja) * 2018-07-06 2020-01-16 パナソニックＩｐマネジメント株式会社 移動体制御システム、移動体システム、移動体制御方法、及びプログラム
JP2020148506A (ja) * 2019-03-11 2020-09-17 本田技研工業株式会社 移動支援装置および移動支援システム

Also Published As

Publication number Publication date
JP7191281B1 (ja) 2022-12-16
DE112021005924T5 (de) 2024-02-22
JPWO2022153402A1 (ja) 2022-07-21
CN116802103A (zh) 2023-09-22
US20230273616A1 (en) 2023-08-31

Similar Documents

Publication Publication Date Title
US11104336B2 (en) Method for planning a trajectory for a self-driving vehicle
JP2015155878A (ja) 車両用障害物検出装置
KR20180135847A (ko) 자율 주행 차량을 위한 감속 연석 기반 방향 검출 및 차선 유지 시스템
EP3524494A1 (en) Lane self-localization system using multiple cameras for autonomous driving vehicles
US20190138825A1 (en) Apparatus and method for associating sensor data in vehicle
CN112602129B (zh) 车载装置、信息处理方法及计算机可读取记录介质
JP7412598B2 (ja) 逆走判定装置および逆走判定方法
JP6968288B2 (ja) 進路予測装置、進路予測プログラムおよび進路予測方法
JPWO2019239463A1 (ja) 車両走行制御装置、車両走行制御方法、制御回路および記憶媒体
US10562531B2 (en) Adaptive cruise control apparatus and method thereof
JPWO2019207639A1 (ja) 行動選択装置、行動選択プログラム及び行動選択方法
JP2018199433A (ja) 自動操舵制御装置および操舵制御方法
WO2022153402A1 (ja) 経路追従装置、経路追従方法および経路追従プログラム
JP6739881B2 (ja) 走行軌跡認識装置、走行軌跡認識方法、車両制御装置、および車両制御方法
CN110612561A (zh) 物体辨识装置、路侧装置以及物体辨识方法
JP5049853B2 (ja) 車両の物体検知装置
EP3698228B1 (en) Optimal planner switch method for three point turn of autonomous driving vehicles
EP4026744A1 (en) Travel path estimating device
JP6594565B1 (ja) 車載装置、情報処理方法及び情報処理プログラム
WO2020158020A1 (ja) 計測装置、計測方法及び計測プログラム
JP6661695B2 (ja) 移動体検出装置、車両制御システム、移動体検出方法および車両制御方法
JP6440795B1 (ja) 車両用操舵装置および操舵制御方法
JP2004272426A (ja) 道路形状認識装置
JP6533269B2 (ja) 車両走行制御装置および車両走行制御方法
WO2021234895A1 (ja) 走行経路生成装置および走行経路生成方法

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 21919297

Country of ref document: EP

Kind code of ref document: A1

ENP Entry into the national phase

Ref document number: 2022556068

Country of ref document: JP

Kind code of ref document: A

WWE Wipo information: entry into national phase

Ref document number: 202180088717.4

Country of ref document: CN

WWE Wipo information: entry into national phase

Ref document number: 112021005924

Country of ref document: DE

122 Ep: pct application non-entry in european phase

Ref document number: 21919297

Country of ref document: EP

Kind code of ref document: A1