Control device, control method, and program

Abstract

A control device for causing a moving body to operate in a tracking mode of tracking a tracking target person, the control device comprising: a first detection unit for detecting the position of the tracking target person; a first determination unit for determining a benefit function indicating a degree of recommendation of traveling of the moving body on the basis of the detected position of the tracking target person; a generation unit for generating a target trajectory of the moving body on the bases of the benefit function; and a control unit for controlling the moving body on the basis of the target trajectory.

Description

Control device, control method, and program

[0001] The present invention relates to a control device, a control method, and a program.

[0002] A trajectory generation device has been proposed that generates a trajectory of a moving body using a model formula that models the trajectory of the moving body as an arc so as to avoid interfering objects such as pedestrians, other vehicles, and walls (Patent Document 1). This trajectory generation device generates a trajectory of the moving body taking into consideration the risk of interference between the moving body and the interfering object.

[0003] Patent Publication No. 2022-153059

[0004] However, although conventional trajectory generation devices can generate a path with a low risk of interference between a moving body and an interfering object, when the moving body operates in a following mode to follow a following object such as a pedestrian, there are cases where the moving body is unable to travel a path suitable for following.

[0005] The present invention has been made in consideration of these circumstances, and one of its objects is to provide a control device, a control method, and a program that can cause a mobile body to travel a route suitable for following when operating in a following mode in which the mobile body follows a person to be followed.

[0006] The control device, control method, and program according to the present invention employ the following configuration: (1): A control device according to one aspect of the present invention is a control device that operates a mobile body in a following mode to follow a person to be followed, and includes a first detection unit that detects the position of the person to be followed, a first determination unit that determines a benefit function that indicates a degree to which it is recommended that the mobile body travels based on the detected position of the person to be followed, a generation unit that generates a target trajectory of the mobile body based on the benefit function, and a control unit that controls the mobile body based on the target trajectory.

[0007] (2) In the aspect (1) above, the first determination unit determines the benefit function based on the current and past positions of the person to be followed.

[0008] (3): In the aspect (1) above, the system further includes a calculation unit that calculates an evaluation function for evaluating the route along which the moving body will travel based on the benefit function, and the generation unit generates a target trajectory for the moving body based on the evaluation function.

[0009] (4): In the above aspect (3), the system further includes a second detection unit that detects the position of an interfering object around the moving body that the moving body should avoid, and a second determination unit that determines a risk function that indicates the degree of risk of interference between the moving body and the interfering object based on the position of the interfering object, and the calculation unit calculates the evaluation function based on the benefit function and the risk function.

[0010] (5): In the above aspect (4), the second determination unit defines the risk function to have a gradient in a first sign direction, and the first determination unit defines the benefit function to have a gradient in a second sign direction opposite to the first sign direction.

[0011] (6) In the aspect (5) above, the generation unit generates the target trajectory using a model that represents a portion of the periphery of a geometric shape.

[0012] (7): In the aspect (6) above, the model is an arc model representing an arc, and the generation unit uses the arc model to model the trajectory of the moving body, and generates the target trajectory by calculating the curvature or radius of curvature of the arc so as to change the evaluation function in the second sign direction.

[0013] (8) In the aspect (1) above, the first determination unit determines the benefit function based on the position of the person to be followed and information about the field of view of the person to be followed.

[0014] (9): In the aspect (8) above, the first determination unit determines a relative position at which the moving body should follow the person to be followed based on information about the field of view of the person to be followed, and sets the benefit function to the relative position.

[0015] (10) In the aspect (9) above, the first determination unit detects a degree of congestion around the moving object, and determines the relative position based on the detected degree of congestion.

[0016] (11): In another aspect of the control method of the present invention, a control device that operates a mobile body in a following mode to follow a person to be followed executes the following processes: detecting the position of the person to be followed; determining a benefit function indicating the degree to which it is recommended that the mobile body should move based on the detected position of the person to be followed; generating a target trajectory for the mobile body based on the benefit function; and controlling the mobile body based on the target trajectory.

[0017] (12): A program according to another aspect of the present invention causes a processor of a control device that operates a mobile body in a following mode to follow a person to be followed to execute the following processes: detecting the position of the person to be followed; determining a benefit function indicating the degree to which it is recommended that the mobile body should move based on the detected position of the person to be followed; generating a target trajectory for the mobile body based on the benefit function; and controlling the mobile body based on the target trajectory.

[0018] According to aspects (1) to (12), when the mobile body operates in a follow-up mode in which the mobile body follows a person to be followed, the mobile body can be made to travel along a route suitable for following.

[0019] FIG. 1 is a perspective view showing a moving body 100. FIG. 2 is a diagram showing the configuration of the moving body 100 equipped with a control device 200. FIG. 3 is a diagram showing an example of the configuration of the control device 200. FIG. 4 is a diagram showing an example of a target trajectory generated by a generation unit 260. FIG. 5 is a diagram for explaining an environmental target risk function and an environmental target-based benefit function. X ｅ_ｌ 1 is a diagram illustrating an example of an environmental target risk function and an environmental target-based benefit function in an axial direction. ｅ_ｌ1 is a diagram showing an example of a potential function in an axial direction; FIG. 2 is an image diagram of an environmental target-based benefit function when environmental targets are detected at multiple detection positions; FIG. 3 is a diagram for explaining a traffic participant / obstacle risk function; FIG. 4 is a diagram for explaining a traffic participant / obstacle risk function based on traffic participant U2; X ｔｐ_ｌ 1 is a diagram illustrating an example of a traffic participant / obstacle risk function in the axial direction. ｔｐ_ｌ FIG. 1 is a diagram showing an example of a traffic participant / obstacle risk function in an axial direction. FIG. 2 is a diagram for explaining a follow mode benefit function. FIG. 3 is a diagram showing an example of a movement amount of a moving body 100. FIG. 4 is a diagram for explaining the field of view of a user (person to be followed). FIG. 5 is a diagram showing an example of a plurality of target positions. FIG. 6 is a diagram showing an example of a state in which follow mode benefit functions are set at a plurality of target positions. FIG. 7 is a diagram showing an example of a follow mode benefit function in the X-axis direction. FIG. 8 is a diagram showing an example of a follow mode benefit function in the Y-axis direction. FIG. 9 is a flowchart showing an example of processing executed by the control device 200.

[0020] Hereinafter, with reference to the drawings, embodiments of a control device, a control method, and a program of the present invention will be described. The control device of the present invention controls the movement mechanism of a mobile object to move the mobile object. The mobile object of the present invention autonomously moves in an area where pedestrians walk. The area where pedestrians walk includes sidewalks, public open spaces, floors within buildings, etc., and may also include roadways. In the following description, it is assumed that no person rides on the mobile object, but it is acceptable for a person to ride on the mobile object. The mobile object operates in a following mode to follow a person to be followed. The person to be followed is, for example, a pedestrian, but may also be a robot or an animal.

[0021] 1 is a perspective view showing a mobile body 100. The mobile body 100 includes, for example, a base body 10, a door unit 60 provided on the base body 10, and wheels (a first wheel 20, a second wheel 30, and a third wheel 40) attached to the base body 10. For example, a user can open the door unit 60 to put luggage into a storage compartment provided on the base body 10 or take luggage out of the storage compartment. The first wheel 20 and the second wheel 30 are drive wheels, and the third wheel 40 is an auxiliary wheel (a driven wheel).

[0022] A cylindrical support body 50 extending upward is provided on the upper surface of the base body 10. A camera 80 that captures images of the surroundings of the moving body 100 is provided at the end of the support body 50. The position at which the camera 80 is provided may be any position different from the above.

[0023] The camera 80 is, for example, a camera that can capture images of the periphery of the moving body 100 at a wide angle (for example, 360 degrees). The camera 80 may include multiple cameras. For example, the camera 80 may be realized by combining multiple 120-degree cameras or multiple 60-degree cameras.

[0024] 2 is a diagram showing the configuration of a mobile object 100 equipped with a control device 200. The mobile object 100 includes, for example, a base 160 equipped with an HMI 110, a detection device 120, a position identification device 130, and the control device 200, a moving mechanism 140 attached to the base 160, and a sensor 150 attached to the moving mechanism 140, etc. The base 160 may be the same as the base 10 in FIG.

[0025] The HMI 110 presents various information to a user such as a person to be followed, and receives input operations from the user. The HMI 110 includes various display devices, a speaker, a buzzer, a touch panel, switches, keys, and the like.

[0026] The detection device 120 is a device that generates data for recognizing objects and a person to be followed that exist around the mobile body 100. The detection device 120 includes, for example, an object recognition device that recognizes objects based on the output of the camera 80. Note that the detection device 120 may recognize objects by using sensors such as a radar device, a LIDAR (Light Detection and Ranging), an ultrasonic sensor, etc., in addition to the camera 80, and by performing sensor fusion processing based on the outputs of these sensors.

[0027] The positioning device 130 is a device that determines the position of the mobile body 100. The positioning device 130 includes, for example, a Global Navigation Satellite System (GNSS) receiver that determines the position of the mobile body 100 based on signals received from GNSS satellites. The positioning device 130 may determine or supplement the position of the mobile body 100 using an Inertial Navigation System (INS) that uses the output of a sensor 150, which will be described later. The positioning device 130 may also have an electromagnetic wave receiving function and determine or supplement the position of the mobile body 100 based on the intensity of electromagnetic waves arriving from surrounding electromagnetic wave sources (whose positions are known).

[0028] The movement mechanism 140 is a mechanism for moving the moving body 100 in any direction. The movement mechanism 140 includes, for example, a plurality of wheels (first wheel 20, second wheel 30, and third wheel 40), a drive motor attached to one or more of the wheels, and a steering device attached to one or more of the wheels. There are no particular restrictions on the configuration of the movement mechanism 140, and the movement mechanism 140 may include components other than wheels, such as pseudo feet for bipedal walking or caterpillars.

[0029] The sensor 150 is a sensor for detecting the behavior of the mobile body 100. The sensor 150 includes, for example, a wheel speed sensor for detecting the speed of the wheels, an acceleration sensor for detecting the acceleration acting on the mobile body 100, a yaw rate sensor attached near the center of gravity of the base body 160 in the horizontal direction, a steering angle sensor for detecting the steering angle of the steered wheels (steered wheels), and an orientation sensor for detecting the orientation of the mobile body 100 in the horizontal direction.

[0030] 3 is a diagram illustrating an example of the configuration of the control device 200. The control device 200 includes, for example, a first detection unit 210, a first determination unit 220, a second detection unit 230, a second determination unit 240, a calculation unit 250, a generation unit 260, and a control unit 270. These components are realized by, for example, a hardware processor such as a CPU (Central Processing Unit) executing a program (software). Some or all of these components may be realized by hardware (including circuitry) such as an LSI (Large Scale Integration), an ASIC (Application Specific Integrated Circuit), an FPGA (Field-Programmable Gate Array), or a GPU (Graphics Processing Unit), or may be realized by a combination of software and hardware. The program may be stored in advance in a storage device (a storage device with a non-transitory storage medium) such as a hard disk drive (HDD) or flash memory, or may be stored in a removable storage medium (a non-transitory storage medium) such as a DVD or CD-ROM, and installed in the storage device by inserting the storage medium into a drive device. Note that the control device 200 may store map information including at least a local map of the location where the mobile object 100 operates in the storage device.

[0031] The first detection unit 210 detects the position of the person to be followed based on information input from the detection device 120. When the moving object 100 is operating in the following mode, the first detection unit 210 repeatedly detects the position of the person to be followed at predetermined time intervals. The first detection unit 210 outputs the detected position of the person to be followed to the first determination unit 220.

[0032] The second detection unit 230 detects the position of an interfering object around the mobile body 100 that the mobile body 100 must avoid, based on information input from the detection device 120. The interfering object is an object that interferes with the traveling of the mobile body 100. For example, the interfering object is an object around the mobile body 100 that the mobile body 100 must avoid, such as an environmental target, a traffic participant, or an obstacle. The environmental target is an object at the boundary of the traveling path that the mobile body 100 cannot enter, such as a wall or a lawn. The traffic participant is, for example, a pedestrian or a vehicle. The obstacle is, for example, a stationary object on the traveling path that obstructs the traveling of the mobile body 100. The mobile body 100 repeatedly detects the position of the interfering object at predetermined time intervals to avoid contact with the interfering object while traveling. The second detection unit 230 outputs the detected position of the interfering object to the first determination unit 220 and the second determination unit 240.

[0033] The first determination unit 220 determines a following mode benefit function indicating the degree to which it is recommended that the mobile object 100 travel, based on the current and past positions of the person to be followed detected by the first detection unit 210. The first determination unit 220 also determines an environmental target-based benefit function based on the positions of the environmental targets detected by the second detection unit 230. The following mode benefit function and the environmental target-based benefit function will be described in detail later. The first determination unit 220 outputs the following mode benefit function and the environmental target-based benefit function to the calculation unit 250.

[0034] The second determination unit 240 determines a risk function indicating the degree of risk of interference between the mobile body 100 and the interfering object, based on the position of the interfering object detected by the second detection unit 230. Specifically, the second determination unit 240 determines the environmental object risk function based on the position of the environmental object detected by the second detection unit 230. The second determination unit 240 also determines a traffic participant / obstacle risk function based on the positions of the traffic participants and obstacles detected by the second detection unit 230. Details of the environmental object risk function and the traffic participant / obstacle risk function will be described later. The second determination unit 240 outputs the environmental object risk function and the traffic participant / obstacle risk function to the calculation unit 250.

[0035] The calculation unit 250 calculates an evaluation function for evaluating the route along which the mobile object 100 will travel, based on the following mode benefit function and the environmental landmark-based benefit function determined by the first determination unit 220, and the environmental landmark risk function and the traffic participant / obstacle risk function determined by the second determination unit 240. Details of the evaluation function will be described later.

[0036] The generation unit 260 generates a target trajectory for the moving body 100 based on the evaluation function calculated by the calculation unit 250. For example, the generation unit 260 generates the target trajectory for the moving body 100 using a model that represents a portion of the periphery of a geometric shape. The model may be an arc model that represents an arc. The generation unit 260 generates the target trajectory for the moving body 100 by modeling the trajectory of the moving body 100 using the arc model. The generation process of the target trajectory by the generation unit 260 will be described below.

[0037] [Process for generating target trajectory] Fig. 4 is a diagram showing an example of a target trajectory generated by the generation unit 260. As shown in Fig. 4, the generation unit 260 generates a target trajectory for the moving body 100 by combining first to third predicted trajectories, which are arc-shaped.

[0038] 4, an XY coordinate system is defined with the center of the current position of the moving body 100 as the origin, the X axis in front of the moving body 100 as the front, and the Y axis to the left of the moving body 100 as the left.

[0039] Then, the position after the first predicted time from the origin in this XY coordinate system is defined as the first predicted position Z ｍ１ and the first predicted position Z ｍ１ The trajectory up to is defined as a first predicted trajectory in the form of an arc.

[0040] The radius of curvature of the first predicted trajectory is R ｍ１ The rotation angle of the first predicted trajectory is θ ｍ１ The rotation angle θ ｍ１ corresponds to the angle between the Y axis and the Y' axis, which will be described later.

[0041] Also, the first predicted position Z ｍ１ The predicted position when the trajectory is divided into three equal parts is calculated by dividing the trajectory from the origin to the first predicted position Zｍ１ , three predicted positions Z ｍ１１ , Z ｍ１２ , Z ｍ１３ (=Z ｍ１ )

[0042] Next, the second predicted trajectory will be described. ｍ１ (= predicted position Z ｍ１３ ) is the origin, and the first predicted position Z ｍ１ An X'-Y' coordinate system is defined in which the tangent direction at is the X' axis and the direction perpendicular to this tangent line is the Y' axis.

[0043] Then, the origin of this X'-Y' coordinate system (i.e., the first predicted position Z ｍ１ ) and the position after the second predicted time is set as the second predicted position Z ｍ２ and the first predicted position Z ｍ１ to the second predicted position Z ｍ２ The trajectory up to is defined as a second predicted trajectory in the form of an arc.

[0044] The radius of curvature of the second predicted orbit is R ｍ２ The rotation angle of the second predicted orbit is θ ｍ２ The rotation angle θ ｍ２ corresponds to the angle between the Y′ axis and the Y″ axis described below.

[0045] Next, the third predicted trajectory will be described. ｍ２ is set as the origin, and the second predicted position Z ｍ２ An X''-Y'' coordinate system is defined in which the tangent direction at is the X'' axis and the direction perpendicular to this tangent line is the Y'' axis.

[0046] The origin of this X'-Y' coordinate system (i.e., the second predicted position Z ｍ２ ) and the position after the third predicted time is the third predicted position Z ｍ３ and the second predicted position Z ｍ２ to the third predicted position Z ｍ３ The trajectory up to is defined as a third predicted trajectory in the form of an arc.

[0047] As described above, the first predicted orbit is determined as a combination of three predicted orbits that are shorter than the second and third predicted orbits. ｍ１１ The trajectory to and predicted position Z ｍ１１ From predicted position Z ｍ１２ The trajectory to and predicted position Z ｍ１２ From predicted position Z ｍ１３ The first predicted trajectory is determined as a combination of the trajectories up to

[0048] This is because the first predicted trajectory is closer to the moving body 100 than the second predicted trajectory and the third predicted trajectory, and therefore it is necessary to generate a predicted trajectory that can more reliably avoid interference with an interfering object. Note that the generation unit 260 may determine the first predicted trajectory as a combination of two or four or more predicted trajectories.

[0049] The generator 260 calculates the rotation angle θ ｍ１ ~θ ｍ３ and the radius of curvature R ｍ１ ~R ｍ３ A plurality of trajectories are generated by determining a combination of a plurality of patterns of the above. Then, the generation unit 260 evaluates each of the generated plurality of trajectories using the evaluation function calculated by the calculation unit 250. Thereafter, the generation unit 260 determines one of the generated plurality of trajectories as a target trajectory for the moving body 100 based on the evaluation result using the evaluation function. By evaluating the generated plurality of trajectories based on the evaluation function, the generation unit 260 can generate a target trajectory for the moving body 100 that has a low risk of interfering with the interfering object and is suitable for the follow-up mode. Details of the evaluation process using the evaluation function will be described later.

[0050] 3, the control unit 270 controls the movement mechanism 140 so that the moving body 100 moves along the target trajectory generated by the generation unit 260. The control unit 270 controls the drive motor and steering device so that the position and behavior of the moving body 100 obtained from the output of the sensor 150 approach the target trajectory.

[0051] [Environmental Target Risk Function and Environmental Target-Based Benefit Function] Next, the environmental target risk function and the environmental target-based benefit function will be described. The environmental target risk function is a function that indicates the degree of risk of interference between the mobile object 100 and an environmental target (e.g., a wall, a lawn, etc.). Conversely, the environmental target-based benefit function is a function that indicates the degree of non-interference between the mobile object 100 and an environmental target.

[0052] 5 is a diagram for explaining the environmental target risk function and the environmental target-based benefit function. As shown in FIG. 5, the traveling direction of the mobile object 100 is the Y axis, and the direction perpendicular to the Y axis is the X axis. Since there are multiple environmental targets R1 to R3 around the mobile object 100, the mobile object 100 needs to travel in a manner that does not interfere with these multiple environmental targets R1 to R3.

[0053] In the example shown in Fig. 5, the second detection unit 230 detects the position of the environmental target R2 based on information input from the detection device 120. Here, the detected position of the environmental target R2 is assumed to be E_l. In Fig. 5, only one detected position E_l is shown, but in reality, the environmental target R2 is detected at multiple locations.

[0054] In the XY coordinate system defined by the X and Y axes, the coordinates of the detected position E_l are defined as (xe_l, ye_l), where the subscript l represents the number of the detected environmental object, and l=1, 2, ..., n. ｅ Let us say that ｅ represents the total number of detected positions of environmental targets.

[0055] Furthermore, the direction from the center of the moving body 100 toward the detection position E_l is defined as X ｅ_ｌ axis, X ｅ_ｌ The direction perpendicular to the axis is Y ｅ_ｌ The X axis and the X ｅ_ｌ The angle between the axis and the ｅ_ｌ Then, ψ ｅ_ｌ is expressed as the following equation (1).

[0056] ψ ｅ_ｌ =tan －１ (ye_l / xe_l) …(1)

[0057] Furthermore, the coordinates in the XY coordinate system of the predicted position on the trajectory generated by the generation unit 260 are expressed as Z ｍｊ (X ｍｊ , Y ｍｊ The subscript j represents the number of the predicted position on the trajectory generated by the generator 260, where j=1, 2, ..., n ｍ Let us say that ｍ represents the total number of predicted positions on the trajectory generated by the generation unit 260. In the example shown in FIG. 4, five predicted positions (Z ｍ１１ , Z ｍ１２ , Z ｍ１３ (=Z ｍ１ ), Z ｍ２ , Z ｍ３ ) is obtained, so n ｍ =5.

[0058] Furthermore, the coordinate Z of the predicted position is calculated based on the following equations (2) and (3): ｍｊ (X ｍｊ , Y ｍｊ ) to X ｅ_ｌ Axis and Y ｅ_ｌ X axis defined by ｅ_ｌ -Y ｅ_ｌ Coordinate Z in the coordinate system ｍｊ_ｅｌ (X ｍｊ_ｅｌ , Y ｍｊ_ｅｌ ) to

[0059] X ｍｊ_ｅｌ = (X ｍｊ -xe_l) cos ψ ｅ_ｌ + (Y ｍｊ -ye_l) sinψ ｅ_ｌ …(2)

[0060] Y ｍｊ_ｅｌ =-(X ｍｊ -xe_l) sinψ ｅ_ｌ + (Y ｍｊ -ye_l) cos ψ ｅ_ｌ …(3)

[0061] FIG. 6 shows the X ｅ_ｌ6 is a diagram showing an example of an environmental target risk function and an environmental target-based benefit function in the axial direction. As shown in FIG. 6, the first determination unit 220 defines the environmental target-based benefit function Pe_x_b with the detection position E_l of the environmental target R2 as the origin. Also, the second determination unit 240 defines the environmental target-based benefit function Pe_x_b with the detection position E_l of the environmental target R2 as the origin. ｅ_ｌ Define the environmental target risk function Pe_x_r in the axial direction.

[0062] The second determination unit 240 determines X so that it has a gradient in the first sign direction (positive direction). ｅ_ｌ The first determination unit 220 defines an environmental target risk function Pe_x_r in the axial direction, and defines an environmental target-based benefit function Pe_x_b to have a gradient in a second sign direction (negative direction) opposite to the first sign direction (positive direction).

[0063] As shown in FIG. ｅ_ｌ The environmental target risk function Pe_x_r in the axial direction is ｅ_ｌ The larger the coordinate value on the X axis, the larger the risk function value. ｅ_ｌ The axis value is set only in the positive area. ｅ_ｌ The environmental target-based benefit function Pe_x_b in the axial direction is ｅ_ｌ The smaller the coordinate value of the X axis, the smaller the benefit function value. ｅ_ｌ The axis values ​​are set only in the negative range. Note that the larger the risk function value, the higher the risk of the moving object 100 interfering with the interfering object, and the smaller the benefit function value, the lower the risk of the moving object 100 interfering with the interfering object.

[0064] FIG. ｅ_ｌ 7 is a diagram illustrating an example of a potential function in the axial direction. As shown in FIG. 7, the potential function Pe_y is a function whose origin is the detected position E_l of the environmental object R2. The second determination unit 240 determines the potential function Pe_y by ｅ_ｌ It is defined as the environmental target risk function in the axial direction.

[0065] Y ｅ_ｌThe environmental target risk function (potential function Pe_y) in the axial direction is expressed as Y ｅ_ｌ The closer the coordinate of the axis is to 0, the larger the risk function value becomes. As shown in FIGS. 6 and 7, ｅ_ｌ The environmental target risk function Pe_x_r in the axial direction and Y ｅ_ｌ The environmental target risk function Pe_y in the axial direction has a different shape from the environmental target risk function Pe_y in the axial direction. ｅ_ｌ The environmental target risk function Pe_x_r in the axial direction is defined as X ｅ_ｌ By defining the risk function value so that it increases as the coordinate value of the axis increases, it is possible to prevent the target trajectory of the moving body 100 from being generated toward a position beyond the environmental target. ｅ_ｌ The environmental target risk function Pe_y in the axial direction is also expressed as X ｅ_ｌ If it is defined as a function similar to the environmental target risk function Pe_x_r in the axial direction, the calculation load on the control device 200 will increase. ｅ_ｌ The environmental object risk function Pe_y in the axial direction is defined so that the position of the origin (i.e., the position where the environmental object R2 is detected) is at its peak.

[0066] Next, a method for calculating the environmental target risk function value will be described. As described above, the coordinate Z of the predicted position on the predicted trajectory in the XY coordinate system is ｍｊ (X ｍｊ , Y ｍｊ ) is X ｅ_ｌ -Y ｅ_ｌ Coordinate Z of the predicted position in the coordinate system ｍｊ_ｅｌ (X ｍｊ_ｅｌ , Y ｍｊ_ｅｌ At the current time k, the X coordinate of the predicted position X ｍｊ_ｅｌ The risk function value based on the above is Pe_x_r_j_l(k), and the Y coordinate of the predicted position Y ｍｊ_ｅｌ The risk function value based on this is defined as Pe_y_j_l(k). Pe_x_r_j_l(k) can be found from Pe_x_r shown in Fig. 6. Pe_y_j_l(k) can be found from Pe_y shown in Fig. 7.

[0067] In this case, the risk function value Pe_r_j_l(k) for each detection position on the environmental target can be expressed as in equation (4). That is, Pe_r_j_l(k) can be obtained by multiplying Pe_x_r_j_l(k) and Pe_y_j_l(k).

[0068] Pe_r_j_l(k)=Pe_x_r_j_l(k)×Pe_y_j_l(k)...(4)

[0069] Furthermore, the total risk function value Pe_r_j(k) for the coordinates of the predicted position on the predicted trajectory can be expressed as in equation (5). That is, the risk function values ​​Pe_r_j_l(k) (l=1, 2, ..., n) for all detected positions on the environmental targets can be expressed as ｅ ) can be used to find Pe_r_j(k).

[0070] Pe_r_j(k) = Σ ｌ＝１ ｎｅ (Pe_r_j_l(k)) …(5)

[0071] The environmental target risk function value Pe_r(k) can be expressed as in equation (6). That is, the total risk function value Pe_r_j(k) (j=1, 2, ..., n) for the coordinates of all predicted positions on the predicted trajectory is ｍ ) can be used to determine the environmental target risk function value Pe_r(k).

[0072] Pe_r(k) = Σ ｊ＝１ ｎｍ (Pe_r_j(k)) …(6)

[0073] Next, a method for calculating the environmental target-based benefit function value will be described. As described in FIG. 6, the first determination unit 220 defines the environmental target-based benefit function Pe_x_b with the detected position E_l of the environmental target R2 as the origin. In addition, the first determination unit 220 calculates the potential function Pe_y shown in FIG. 7 by multiplying Y ｅ_ｌ It is defined as the environmental target-based benefit function in the axial direction.

[0074] As mentioned above, the coordinate Z of the predicted position on the predicted trajectory in the XY coordinate system ｍｊ (X ｍｊ , Y ｍｊ ) is X ｅ_ｌ -Y ｅ_ｌ Coordinate Z of the predicted position in the coordinate system ｍｊ_ｅｌ (X ｍｊ_ｅｌ , Y ｍｊ_ｅｌ At the current time k, the X coordinate of the predicted position X ｍｊ_ｅｌ The benefit function value based on the above is Pe_x_b_j_l(k), and the Y coordinate of the predicted position Y ｍｊ_ｅｌ The benefit function value based on the above is defined as Pe_y_j_l(k). Pe_x_b_j_l(k) can be found from Pe_x_b shown in Fig. 6. Pe_y_j_l(k) can be found from Pe_y shown in Fig. 7.

[0075] In this case, the benefit function value Pe_b_j_l(k) for each detection position on the environmental object can be expressed as in equation (7). That is, Pe_b_j_l(k) can be obtained by multiplying Pe_x_b_j_l(k) and Pe_y_j_l(k).

[0076] Pe_b_j_l(k)=Pe_x_b_j_l(k)×Pe_y_j_l(k)...(7)

[0077] Furthermore, the total benefit function value Pe_b_j(k) for the coordinates of the predicted position on the predicted trajectory can be expressed as in equation (8). That is, the benefit function values ​​Pe_b_j_l(k) (l=1, 2, ..., n) for all detected positions on the environmental targets can be expressed as ｅ ) can be obtained as Pe_b_j(k).

[0078] Pe_b_j(k)=max Pe_b_j_l(k) =max Pe_x_b_j_l(k)×Pe_y_j_l(k)...(8)

[0079] Fig. 8 is an image diagram of the environmental target-based benefit function when environmental targets are detected at multiple detection positions. As shown in Fig. 8, for example, assume that there are walls on both the left and right sides, and environmental targets are detected on the right and left sides of the traveling direction of the mobile body 100. Furthermore, the environmental target-based benefit function set based on the detection position on the left side of the traveling direction of the mobile body 100 is denoted as Pe_x_b_1, and the environmental target-based benefit function set based on the detection position on the right side of the traveling direction of the mobile body 100 is denoted as Pe_x_b_2.

[0080] In this case, the first determination unit 220 determines the maximum value of the multiple benefit function values ​​(Pe_x_b_1 and Pe_x_b_2) as X ｅ_ｌ The environmental target-based benefit function value in the axial direction is set as Pe_b_j(k). The maximum value is selected because the benefit function should be set taking into consideration the balance with all environmental targets. For this reason, in the above-mentioned equation (8), Pe_b_j(k) is calculated taking into consideration the maximum value.

[0081] The environmental object-based benefit function value Pe_b(k) can be expressed as in equation (9). That is, the total benefit function value Pe_b_j(k) (j=1, 2, ..., n) for the coordinates of all predicted positions on the predicted trajectory is ｍ ) to obtain the environmental object-based benefit function value Pe_b(k).

[0082] Pe_b(k) = Σ ｊ＝１ ｎｍ (Pe_b_j(k)) …(9)

[0083] [Traffic Participant / Obstacle Risk Function] Next, the traffic participant / obstacle risk function will be described. The traffic participant / obstacle risk function is a function that indicates the degree of risk of interference between the mobile object 100 and a traffic participant or an obstacle.

[0084] 9 is a diagram for explaining the traffic participant / obstacle risk function. As shown in FIG. 9, the traveling direction of the mobile body 100 is the Y axis, and the direction perpendicular to the Y axis is the X axis. The mobile body 100 travels so as to follow the target person U1. Since multiple traffic participants U2 and U3 exist around the mobile body 100, the mobile body 100 must travel in a manner that does not interfere with these multiple traffic participants U2 and U3.

[0085] 9 , the second detection unit 230 detects the positions of traffic participants U2 and U3 based on information input from the detection device 120. In addition, the second determination unit 240 sets a traffic participant / obstacle risk function to the positions of traffic participants U2 and U3 detected by the second detection unit 230.

[0086] FIG. 10 is a diagram for explaining the traffic participant / obstacle risk function based on the traffic participant U2. The following describes the process of determining the traffic participant / obstacle risk function based on the traffic participant U2 by the second determination unit 240. In FIG. 10, the detected position of the traffic participant U2 is designated as TP_l. Although FIG. 10 only shows the detected position TP_l of the traffic participant U2, in reality, the traffic participant U3 is also detected. Note that if there are obstacles on the road in addition to the traffic participants, the second detection unit 230 will also detect the obstacles.

[0087] In the XY coordinate system defined by the X and Y axes, the coordinates of the detected position TP_l are defined as (xtp_l, ytp_l), where the subscript l represents the number of the detected position of the traffic participant / obstacle, and l = 1, 2, ..., n. ｔ Let us say that ｔ represents the total number of detected positions of traffic participants and obstacles. The traveling direction of traffic participant U2 is the Ytp_l axis, and the direction perpendicular to the Ytp_l axis is the Xtp_l axis.

[0088] Furthermore, the coordinates in the XY coordinate system of the predicted position on the trajectory generated by the generation unit 260 are expressed as Z ｍｊ (X ｍｊ , Y ｍｊ ) is defined as follows. Using the same method as in the above equations (2) and (3), the coordinate Z of the predicted position is calculated.ｍｊ (X ｍｊ , Y ｍｊ ) is X ｔｐ_ｌ Axis and Y ｔｐ_ｌ X axis defined by ｔｐ_ｌ -Y ｔｐ_ｌ Coordinate Z in the coordinate system ｍｊ_ｔｐｌ (X ｍｊ_ｔｐｌ , Y ｍｊ_ｔｐｌ ) is converted to

[0089] FIG. 11 shows the X ｔｐ_ｌ 11 is a diagram illustrating an example of a traffic participant / obstacle risk function in the axial direction. As shown in FIG. 11, the second determination unit 240 determines the detected position TP_l of the traffic participant U2 as the origin, and calculates the X ｔｐ_ｌ The traffic participant / obstacle risk function Ptp_x in the axial direction is defined. The second determination unit 240 determines X so that it has a gradient in the first sign direction (positive direction). ｔｐ_ｌ Define a traffic participant / obstacle risk function Ptp_x in the axial direction.

[0090] As shown in FIG. ｔｐ_ｌ The risk function Ptp_x for traffic participants and obstacles in the axial direction is ｔｐ_ｌ The closer the axis coordinate is to 0, the larger the risk function value becomes.

[0091] FIG. 12 shows the Y ｔｐ_ｌ 12 is a diagram illustrating an example of a traffic participant / obstacle risk function in the axial direction. As shown in FIG. 12, the second determination unit 240 determines the detected position TP_l of the traffic participant U2 as the origin, and calculates the Y ｔｐ_ｌ The traffic participant / obstacle risk function Ptp_y in the axial direction is defined. The second determination unit 240 determines Y so that it has a gradient in the first sign direction (positive direction). ｔｐ_ｌ Define a traffic participant / obstacle risk function Ptp_y in the axial direction.

[0092] As shown in FIG. ｔｐ_ｌ The risk function Ptp_y for traffic participants and obstacles in the axial direction is ｔｐ_ｌ The closer the axis coordinate is to 0, the larger the risk function value becomes.

[0093] Next, we will explain how to calculate the traffic participant / obstacle risk function value. As mentioned above, the coordinate Z of the predicted position on the predicted trajectory in the XY coordinate system is ｍｊ (X ｍｊ , Y ｍｊ ) is X ｔｐ_ｌ -Y ｔｐ_ｌ Coordinate Z of the predicted position in the coordinate system ｍｊ_ｔｐｌ (X ｍｊ_ｔｐｌ , Y ｍｊ_ｔｐｌ At the current time k, the X coordinate of the predicted position X ｍｊ_ｔｐｌ The risk function value based on the above is Ptp_x_j_l(k), and the Y coordinate of the predicted position is Y ｍｊ_ｔｐｌ The risk function value based on this is defined as Ptp_y_j_l(k). Ptp_x_j_l(k) can be found from Ptp_x shown in Fig. 11. Ptp_y_j_l(k) can be found from Ptp_y shown in Fig. 12.

[0094] In this case, the risk function value Ptp_r_j_l(k) for each detected position of a traffic participant / obstacle can be expressed as in equation (10). That is, Ptp_r_j_l(k) can be obtained by multiplying Ptp_x_j_l(k) and Ptp_y_j_l(k).

[0095] Ptp_r_j_l(k)=Ptp_x_j_l(k)×Ptp_y_j_l(k)...(10)

[0096] The total risk function value Ptp_r_j(k) for the coordinates of the predicted position on the predicted trajectory can be expressed as in equation (11). That is, the risk function values ​​Ptp_r_j_l(k) (l=1, 2, ..., n) for all detected positions of traffic participants and obstacles can be expressed as ｔ ) can be used to find Ptp_r_j(k).

[0097] Ptp_r_j(k)=Σ ｌ＝１ ｎｔ (Ptp_r_j_l(k)) …(11)

[0098] The traffic participant / obstacle risk function value Ptp_r(k) can be expressed as in equation (12). That is, the total risk function value Ptp_r_j(k) (j=1, 2, ..., n) for the coordinates of all predicted positions on the predicted trajectory is ｍ ) can be used to calculate the traffic participant / obstacle risk function value Ptp_r(k).

[0099] Ptp_r(k) = Σ ｊ＝１ ｎｍ (Ptp_r_j(k)) …(12)

[0100] [Follow-up Mode Benefit Function] Next, the follow-up mode benefit function will be described. The follow-up mode benefit function is a function that indicates the degree to which it is recommended that the mobile object 100 should move, and is determined based on the current and past positions of the person U1 to be followed.

[0101] 13 is a diagram for explaining the following mode benefit function. As shown in FIG. 13, the traveling direction of the moving body 100 is the Y axis, and the direction perpendicular to the Y axis is the X axis. Since multiple environmental targets R4 and R5 exist around the moving body 100, the moving body 100 needs to follow the following target U1 without interfering with these multiple environmental targets R4 and R5.

[0102] The first detection unit 210 detects the position Vu_0(k) of the person to be followed U1 at the current time k based on the detection result of the detection device 120. The first detection unit 210 repeatedly detects the position Vu_0(k) of the person to be followed U1 at every preset control time. The position Vu_0(k) of the person to be followed U1 is detected as the relative position (position in the X-Y coordinate system) of the person to be followed U1 with respect to the moving object 100. In FIG. 13, Vu_1(k) indicates the position of the person to be followed U1 one control time before Vu_0(k), and Vu_2(k) indicates the position of the person to be followed U1 one control time before Vu_1(k). Vu_n ｕ (k) indicates the most recent position of the person to be followed U1.

[0103] The first detector 210 detects the past positions Vu_0(k), Vu_1(k), Vu_2(k), ..., Vu_n of the person U1 to be followed up. ｕ At this time, since the moving body 100 is actually moving, the previously detected position of the person U1 to be followed up must be corrected in accordance with the amount of movement of the moving body 100. This point will be described below.

[0104] FIG. 14 is a diagram showing an example of the movement amount of the moving body 100. In FIG. ０ indicates the position of the moving object 100 one control time before. ｍｖ is the time when the moving object 100 is at position Z ０ Δy indicates the distance traveled in the X direction by the moving body 100 when moving from the current position (the origin position in the XY coordinate system). ｍｖ is the time when the moving object 100 is at position Z ０ The control device 200 calculates the distance Δx in one control time based on the detection result of the sensor 150. ｍｖ and Δy ｍｖ Calculate.

[0105] Here, the position Vu_0(k) of the person to be followed U1 at the current time k is defined as shown in equation (13).

[0106] Vu_0(k) = [xu_0(k) yu_0(k)] …(13)

[0107] In this case, the control device 200 calculates the movement trajectories Vu_1(k), Vu_2(k), ..., Vu_m of the person to be followed U1. ｕ (k) is the movement amount Δx of the moving body 100 ｍｖ and Δy ｍｖ Based on this, the calculation is performed according to the following formulas (14) to (16). ｕ is n ｕ The following integers:

[0108] Vu_1(k) = [xu_1(k) yu_1(k)] = [xu_0(k-1)-Δx ｍｖ (k) yu_0(k-1)-Δy ｍｖ (k)] …(14)

[0109] Vu_2(k) = [xu_2(k) yu_2(k)] = [xu_1(k-1)-Δx ｍｖ (k) yu_1(k-1)-Δy ｍｖ (k)] …(15)

[0110] Vu_m ｕ (k) = [xu_m ｕ (k) yū_m ｕ (k)] = [xu_m ｕ −1(k−1)−Δx ｍｖ (k) yū_m ｕ −1(k−1)−Δy ｍｖ (k)] …(16)

[0111] FIG. 15 is a diagram illustrating the field of view of the user (person to be followed). When the moving body 100 follows the person to be followed U1, it is preferable that the moving body 100 travels while staying within the field of view of the person to be followed U1. This is to allow the person to be followed U1 to easily confirm the position of the moving body 100 without turning around. Therefore, the first determination unit 220 determines the following mode benefit function based on the position of the person to be followed U1 and information related to the field of view of the person to be followed U1. The information related to the field of view of the person to be followed U1 may be, for example, information related to the field of view of the person to be followed U1.

[0112] For example, the first determination unit 220 determines the relative position that the moving body 100 should follow with respect to the person to be followed U1 based on the position of the person to be followed U1 and information about the field of view of the person to be followed U1, and sets the following mode benefit function. Specifically, the first determination unit 220 calculates an offset amount Δxt in the X-axis direction and an offset amount Δyt in the Y-axis direction based on the information about the field of view of the person to be followed U1. Then, the first determination unit 220 calculates, as the target position Vt_0(k), a position that is offset by Δxt in the X-axis direction and by Δyt in the Y-axis direction from the position Vu_0(k) of the person to be followed U1 at the current time k.

[0113] 16 is a diagram illustrating an example of a plurality of target positions. As shown in FIG. 16, the first determination unit 220 determines the past positions Vu_0(k), ..., Vu_n of the person U1 to be followed up. ｕ (k), a plurality of target positions Vt_0(k), ..., Vt_n ｕ (k) is calculated from a plurality of target positions Vt_0(k), ..., Vt_n. ｕ (k) represents the past positions Vu_0(k), ..., Vu_n of the person U1 to be followed up. ｕ This is a position offset by Δxt in the X-axis direction and Δyt in the Y-axis direction from (k).

[0114] 17 is a diagram showing an example of a state in which the tracking mode benefit functions are set for a plurality of target positions. As shown in FIG. 17, the first determination unit 220 calculates the tracking mode benefit functions for the plurality of target positions Vt_0(k), ..., Vt_n ｕ A tracking mode benefit function is set for each position of the target positions Vt_0(k), ..., Vt_n. ｕ It is recommended to drive around (k).

[0115] The first determination unit 220 may calculate a congestion level based on the detection results of the detection device 120 and determine the relative position (target position) based on the calculated congestion level. For example, the first determination unit 220 may detect the number of interfering objects (e.g., pedestrians, obstacles, etc.) present around the target person U1 based on the detection results of the detection device 120 and calculate the congestion level based on the number of detected interfering objects. Specifically, the first determination unit 220 may reduce Δxt as the number of interfering objects present around the target person U1 increases. If the area around the target person U1 is crowded, a large Δxt will require other pedestrians, etc. to move significantly laterally to avoid the target person U1 and the moving body 100, which will cause stress to the other pedestrians, etc. However, by reducing Δxt as the area around the target person U1 becomes more crowded, the moving body 100 will automatically follow behind the target person U1. Therefore, the moving body 100 can be controlled so as not to interfere with the movement of other pedestrians, etc., and stress on other pedestrians, etc. can be reduced.

[0116] Note that the method for calculating the congestion degree is not limited to this. For example, the first determination unit 220 may detect the distance between multiple interfering objects (e.g., pedestrians, obstacles, etc.) present around the person to be followed U1 based on the detection results of the detection device 120, and calculate the congestion degree based on the detected distance. Specifically, the first determination unit 220 may reduce Δxt as the detected distance becomes shorter. This makes it possible to control the moving body 100 so as not to interfere with the movement of other pedestrians, etc., and reduce stress on other pedestrians, etc.

[0117] Furthermore, the first determination unit 220 may set Δxt to 0 when the congestion level is high (for example, when the congestion level is equal to or greater than the first predetermined value), and may decrease Δyt as the congestion level increases. As a result, when the area around the person U1 to be followed is too crowded, the moving body 100 can prevent contact with other pedestrians, etc. by hiding behind the person U1 to be followed so as not to get in the way of other pedestrians, etc.

[0118] Furthermore, when the degree of congestion is low (for example, when the degree of congestion is equal to or less than the second predetermined value), the first determination unit 220 may decrease Δyt. Note that the second predetermined value is less than the first predetermined value. This allows the person to be followed U1 to feel closer to the moving body 100 when the area around the person to be followed U1 is not crowded, and can give the person to be followed U1 the feeling that they are walking alongside a human.

[0119] Furthermore, the first determination unit 220 may reduce Δxt and Δyt when there is a conversation with the person to be followed U1 compared to when there is no conversation. For example, the moving body 100 may be provided with a microphone, and the first determination unit 220 may determine whether there is a conversation based on sound detected by the microphone. In this way, when the person to be followed U1 is conversing with the moving body 100, it is possible to give the person to be followed U1 the feeling that they are walking and conversing with a human.

[0120] As described above, the coordinates of the predicted position on the trajectory generated by the generation unit 260 are Z ｍｊ (X ｍｊ , Y ｍｊ ) and the coordinates of each of the multiple target positions and the coordinate Z of the predicted position. ｍｊ (X ｍｊ , Y ｍｊ ) and the benefit function value is calculated based on the above.

[0121] 18 is a diagram showing an example of a tracking mode benefit function in the X-axis direction. The horizontal axis Δxu_b in FIG. 18 represents the X coordinate of the target position and the X coordinate X of the predicted position on the trajectory generated by the generation unit 260. ｍｊ The first determination unit 220 defines the following mode benefit function Pu_b_x in the X-axis direction so that it has a gradient in the second sign direction (negative direction).

[0122] As shown in FIG. 18, the following mode benefit function Pu_b_x in the X-axis direction is a function whose benefit function value decreases as Δxu_b approaches 0.

[0123] 19 is a diagram showing an example of a tracking mode benefit function in the Y-axis direction. The horizontal axis Δyu_b in FIG. 19 represents the Y coordinate of the target position and the Y coordinate Y of the predicted position on the trajectory generated by the generation unit 260. ｍｊ The first determination unit 220 defines the following mode benefit function Pu_b_y in the Y-axis direction so that it has a gradient in the second sign direction (negative direction).

[0124] As shown in FIG. 19, the following mode benefit function Pu_b_y in the Y-axis direction is a function whose benefit function value decreases as Δyu_b approaches 0.

[0125] Next, a method for calculating the follow-up mode benefit function value will be described. First, a plurality of target positions Vt_l′ (l′=0, 1, . . . , n) at the current time k are calculated. ｕ ) are calculated based on equations (17) to (19).

[0126] Vt_0(k) = [xt_0(k) yt_0(k)] = [xu_0(k)+Δxt yu_0(k)+Δyt]…(17)

[0127] Vt_1(k) = [xt_1(k) yt_1(k)] = [xu_1(k)+Δxt yu_1(k)+Δyt]…(18)

[0128] Vt_n ｕ (k) = [xt_n ｕ (k) yt_n ｕ (k)] = [xu_n ｕ (k)+Δxt yu_n ｕ (k)+Δyt]…(19)

[0129] Next, the coordinates Z of the predicted positions on the trajectory generated by the generation unit 260 based on the plurality of target positions Vt_l′ are calculated. ｍｊ (X ｍｊ , Y ｍｊ ) (j=1, 2,..., n ｍ) to calculate the benefit function value.

[0130] Specifically, the X coordinate of the predicted position on the trajectory generated by the generating unit 260 is expressed as X ｍｊ (k) and the X coordinate of the target position is xt_l'(k), the difference Δxu_b_l'(k) therebetween is expressed as in equation (20).

[0131] Δxu_b_l'(k)=X ｍｊ (k)-xt_l'(k)...(20)

[0132] The first determination unit 220 calculates Δxu_b_l′(k) based on equation (20). Furthermore, the first determination unit 220 obtains a benefit function value Pu_b_x_j_l′(k) corresponding to the calculated Δxu_b_l′(k) from the following mode benefit function in the X-axis direction shown in FIG.

[0133] Also, the Y coordinate of the predicted position on the trajectory generated by the generation unit 260 is expressed as Y ｍｊ (k) and the Y coordinate of the target position is yt_l'(k), the difference Δyu_b_l'(k) therebetween is expressed as in equation (21).

[0134] Δyu_b_l'(k)=Y ｍｊ (k)-yt_l'(k)...(21)

[0135] The control device 200 calculates Δyu_b_l′(k) based on equation (21). The control device 200 also obtains a benefit function value Pu_b_y_j_l′(k) corresponding to the calculated Δyu_b_l′(k) from the following mode benefit function in the Y-axis direction shown in FIG.

[0136] Furthermore, the benefit function value Pu_b_j_l'(k) for each of the multiple target positions can be expressed as in equation (22). That is, Pu_b_j_l'(k) can be obtained by multiplying Pu_b_x_j_l'(k) and Pu_b_y_j_l'(k). Note that the benefit function value Pu_b_j_l'(k) may be signed so that it becomes a negative value.

[0137] Pu_b_j_l'(k)=Pu_b_x_j_l'(k)×Pu_b_y_j_l'(k)...(22)

[0138] The total benefit function value Pu_b_j(k) for the coordinates of the predicted positions on the predicted trajectory can be expressed as in equation (23). That is, the benefit function values ​​Pu_b_j_l′(k) (l′=0, 1, ..., n) for all target positions can be expressed as in equation (23). ｕ ) can be used to obtain Pu_b_j(k).

[0139] Pu_b_j(k) = Σ ｌ’＝０ ｎｕ (Pu_b_j_l'(k)) ...(23)

[0140] The following mode benefit function value Pu_b(k) can be expressed as in equation (24). That is, the total benefit function value Pu_b_j(k) (j=1, 2, ..., n) for the coordinates of all predicted positions on the predicted trajectory is expressed as ｍ ) to obtain the following mode benefit function value Pu_b(k).

[0141] Pu_b(k) = Σ ｊ＝１ ｎｍ (Pu_b_j(k)) …(24)

[0142] [Calculation Process of Evaluation Function, etc.] Next, the calculation process of the evaluation function, etc. will be described. The calculation unit 250 calculates the evaluation function J(k) based on the benefit function determined by the first determination unit 220 and the risk function determined by the second determination unit 240. Specifically, the calculation unit 250 calculates the evaluation function J(k) according to the following equation (25). That is, the calculation unit 250 calculates the evaluation function J(k) by adding together Pu_b(k), Pe_b(k), Ptp_r(k), and Pe_r(k).

[0143] J(k)=Pu_b(k)+Pe_b(k)+Ptp_r(k)+Pe_r(k)...(25)

[0144] The benefit functions Pu_b(k) and Pe_b(k) are negative values, and the smaller the benefit function value, the higher the degree of recommendation for moving the moving body 100. The risk functions Ptp_r(k) and Pe_r(k) are positive values, and the larger the risk function value, the higher the degree of risk of interference between the moving body 100 and the interference target. For this reason, the generation unit 260 needs to generate a target trajectory for the moving body 100 so that the evaluation value calculated based on the evaluation function J(k) is small.

[0145] The generation unit 260 generates a target trajectory by using an arc model to model the trajectory of the moving body 100 and calculating the curvature or radius of curvature of the arc so as to change the evaluation function J(k) in the second sign direction (negative direction), as shown in FIG. 4 above. As described above, the generation unit 260 calculates the rotation angle θ ｍ１ ~θ ｍ３ and the radius of curvature R ｍ１ ~R ｍ３ The generation unit 260 then calculates an evaluation value for each of the generated trajectories using the evaluation function J(k) calculated by the calculation unit 250, and generates the trajectory with the smallest evaluation value as the target trajectory.

[0146] The control unit 270 controls the moving body 100 based on the target trajectory generated by the generation unit 260. Specifically, the control unit 270 controls the movement mechanism 140 (drive motor, steering device, etc.) so that the moving body 100 travels along the target trajectory generated by the generation unit 260. This allows the control device 200 to control the moving body 100 so that it travels along a target trajectory that has a low risk of interference between the moving body 100 and an interfering object (pedestrian, obstacle, etc.) and is suitable for following the person U1 to be followed.

[0147] [Flowchart] FIG. 20 is a flowchart illustrating an example of processing executed by the control device 200. The processing according to this flowchart is executed in response to the determination of the person U1 to be followed. For example, the moving object 100 may be equipped with a biometric authentication unit, and a user who has undergone biometric authentication by the biometric authentication unit may be determined as the person U1 to be followed. Specifically, the biometric authentication unit may determine the person U1 to be followed by performing face authentication based on a facial image of the user captured by the camera 80. Note that the biometric authentication method is not limited to this. For example, the biometric authentication unit may be equipped with a vein sensor, and vein authentication may be performed based on output from the vein sensor. Furthermore, the control device 200 may identify the user through communication with a mobile device carried by the user and determine the identified user as the person U1 to be followed. Once the person U1 to be followed is determined, the processing according to the flowchart of FIG. 20 is executed.

[0148] First, the first detection unit 210 detects the position of the person U1 to be followed (step S100). Specifically, the first detection unit 210 detects the position of the person U1 to be followed based on information input from the detection device 120.

[0149] Next, the first determination unit 220 determines a benefit function indicating the degree to which it is recommended that the mobile object 100 continue traveling, based on the current and past positions of the person to be followed U1 (step S102). Specifically, the first determination unit 220 calculates a target position offset by Δxt in the X-axis direction and Δyt in the Y-axis direction from the current and past positions of the person to be followed U1, and determines the following mode benefit function Pu_b(k) based on the calculated target position. The first determination unit 220 also determines an environmental landmark-based benefit function Pe_b(k) based on the positions of environmental landmarks (e.g., walls, grass, etc.).

[0150] Next, the second detection unit 230 detects the position of the interfering object (for example, a pedestrian or an obstacle) (step S104). Specifically, the second detection unit 230 detects the position of the interfering object based on the information input from the detection device 120.

[0151] Next, the second determination unit 240 determines a risk function indicating the degree of risk of interference between the moving body 100 and the interference object based on the position of the interference object (step S106). Specifically, the second determination unit 240 determines an environmental object risk function Pe_r(k) based on the position of the environmental object. The second determination unit 240 also determines a traffic participant / obstacle risk function value Ptp_r(k) based on the positions of the traffic participants and obstacles.

[0152] Next, the calculation unit 250 calculates the evaluation function J(k) based on the benefit function determined by the first determination unit 220 and the risk function determined by the second determination unit 240 (step S108). Specifically, the calculation unit 250 calculates the evaluation function J(k) by adding together Pu_b(k), Pe_b(k), Ptp_r(k), and Pe_r(k).

[0153] Next, the generation unit 260 generates a target trajectory for the moving body 100 based on the evaluation function J(k) calculated by the calculation unit 250 (step S110). Specifically, the generation unit 260 models the trajectory of the moving body 100 using one or more arcs as shown in Fig. 4, and calculates the curvature or radius of curvature of the arc so as to change the evaluation function J(k) in the second sign direction (negative direction), thereby generating the target trajectory.

[0154] Next, the control unit 270 controls the moving body 100 based on the target trajectory generated by the generation unit 260 (step S112). Specifically, the control unit 270 controls the movement mechanism 140 (drive motor, steering device, etc.) so that the moving body 100 travels along the target trajectory generated by the generation unit 260. In this way, the control device 200 of this embodiment can cause the moving body 100 to travel along a path suitable for following when the moving body 100 operates in the following mode.

[0155] The control unit 270 controls the speed of the moving object 100 and the distance between the moving object 100 and the person to be followed U1. The target distance between the moving object 100 and the person to be followed U1 may be a predetermined distance, or the target distance may be calculated according to the degree of congestion. Specifically, the control unit 270 may calculate the target distance so that the distance between the moving object 100 and the person to be followed U1 is shortened when the degree of congestion is higher than a predetermined value. The control unit 270 may also change the target distance according to whether or not a conversation is taking place between the person to be followed U1 and the moving object 100. Specifically, when the control unit 270 determines that a conversation is taking place between the person to be followed U1 and the moving object 100, the control unit 270 may set the target distance longer than when it determines that no conversation is taking place. The control unit 270 may calculate the speed of the person to be followed U1 based on the trajectory of the person to be followed U1 from the past to the present, and may calculate the speed of the moving body 100 based on the calculated speed of the person to be followed U1, or may control the speed so as to maintain the target distance from the person to be followed U1. Specifically, the control unit 270 may use the calculated speed of the person to be followed U1 as the target speed of the moving body 100, or may use a speed obtained by subtracting a predetermined speed from the speed of the person to be followed U1 as the target speed of the moving body 100.

[0156] In a conventional method for controlling the moving body 100, when the person U1 to be followed turns a corner, the moving body 100 following the person U1 to be followed may take a shortcut around the corner and collide with a wall, etc. However, according to the control device 200 of the present embodiment, an appropriate target trajectory is generated according to the benefit function and the risk function, so that even when the person U1 to be followed turns a corner, the moving body 100 can be prevented from colliding with a wall, etc.

[0157] Furthermore, the control device 200 of this embodiment can cause the moving body 100 to follow the person U1 to an appropriate position, and can also control the moving body 100 to temporarily move away from the appropriate position when an object to be avoided (an interfering object) is present on the road, and automatically return to the appropriate position after avoiding the interfering object. Furthermore, the control device 200 of this embodiment does not require any additional special processing for avoiding the interfering object, so the continuity of the movement of the moving body 100 can be maintained, making it easier for other pedestrians to predict the behavior of the moving body 100, and effectively preventing contact between the moving body 100 and other pedestrians.

[0158] Furthermore, according to the control device 200 of this embodiment, the trajectory of the moving body 100 is modeled using an arc model to generate a target trajectory of the moving body 100, thereby smoothing the movement of the moving body 100. This makes it easier for traffic participants (pedestrians, etc.) to predict the behavior of the moving body 100, and makes it possible to effectively prevent contact between the moving body 100 and other traffic participants.

[0159] In this embodiment, when the moving body 100 travels on a road with a low risk of interfering with an interfering object (such as a pedestrian or an obstacle), the second determination unit 240 does not need to determine the risk function. In this case, the calculation unit 250 may calculate the evaluation function based only on the benefit function without considering the risk function.

[0160] As described above, the control device 200 of this embodiment is a device that operates the mobile body 100 in a following mode to follow the person U1 to be followed, and includes a first detection unit 210, a first determination unit 220, a calculation unit 250, a generation unit 260, and a control unit 270. The first detection unit 210 detects the position of the person U1 to be followed. The first determination unit 220 determines a benefit function that indicates a degree to which it is recommended that the mobile body 100 should travel, based on the current and past positions of the person U1 to be followed. The calculation unit 250 calculates an evaluation function for evaluating a route along which the mobile body 100 will travel, based on the benefit function. The generation unit 260 generates a target trajectory for the mobile body 100, based on the evaluation function. The control unit 270 controls the mobile body 100 based on the target trajectory. As a result, the control device 200 of this embodiment can cause the mobile body 100 to travel a route suitable for following when the mobile body 100 operates in the following mode.

[0161] The above-described embodiment can be expressed as follows: A control device comprising: a storage device storing a program; and a hardware processor, wherein the hardware processor executes the program stored in the storage device to perform the following processes when a moving object operates in a following mode to follow the moving object: detecting the position of the person to be followed, based on the detected position of the person to be followed, determining a benefit function indicating the degree to which it is recommended that the moving object should travel, based on the benefit function, generating a target trajectory for the moving object, and controlling the moving object based on the target trajectory.

[0162] The above describes the form for carrying out the present invention using an embodiment, but the present invention is not limited to such an embodiment, and various modifications and substitutions can be made within the scope that does not deviate from the gist of the present invention.

[0163] REFERENCE SIGNS LIST 100 Mobile object 200 Control device 210 First detection unit 220 First determination unit 230 Second detection unit 240 Second determination unit 250 Calculation unit 260 Generation unit 270 Control unit

Claims

1. A control device that operates a mobile body in a following mode to follow a person to be followed, comprising: a first detection unit that detects the position of the person to be followed; a first determination unit that determines a benefit function that indicates the degree to which it is recommended that the mobile body should travel, based on the detected position of the person to be followed; a generation unit that generates a target trajectory for the mobile body based on the benefit function; and a control unit that controls the mobile body based on the target trajectory.

2. The control device according to claim 1, wherein the first determination unit determines the benefit function based on past positions of the person to be followed.

3. The control device according to claim 1, further comprising a calculation unit that calculates an evaluation function for evaluating a route along which the moving body will travel based on the benefit function, and the generation unit generates a target trajectory for the moving body based on the evaluation function.

4. A control device as described in claim 3, further comprising: a second detection unit that detects the position of an interfering object around the moving body that the moving body should avoid; and a second determination unit that determines a risk function that indicates the degree of risk of interference between the moving body and the interfering object based on the position of the interfering object, wherein the calculation unit calculates the evaluation function based on the benefit function and the risk function.

5. The control device according to claim 4, wherein the second determination unit defines the risk function to have a gradient in a first sign direction, and the first determination unit defines the benefit function to have a gradient in a second sign direction opposite to the first sign direction.

6. The control device according to claim 5, wherein the generation unit generates the target trajectory using a model that represents a portion of the periphery of a geometric shape.

7. The control device according to claim 6, wherein the model is an arc model representing an arc, and the generation unit generates the target trajectory by using the arc model to model the trajectory of the moving body and calculating the curvature or radius of curvature of the arc so as to change the evaluation function in the second sign direction.

8. The control device according to claim 1, wherein the first determination unit determines the benefit function based on the position of the person to be followed and information about the field of view of the person to be followed.

9. The control device according to claim 8, wherein the first determination unit determines a relative position at which the moving body should follow the person to be followed based on information about the field of view of the person to be followed, and sets the benefit function at the relative position.

10. The control device according to claim 9, wherein the first determination unit detects a degree of congestion around the moving object, and determines the relative position based on the detected degree of congestion.

11. A control method in which a control device that operates a mobile body in a following mode to follow a person to be followed executes the following processes: a process of detecting the position of the person to be followed; a process of determining a benefit function indicating the degree to which it is recommended that the mobile body should move based on the detected position of the person to be followed; a process of generating a target trajectory for the mobile body based on the benefit function; and a process of controlling the mobile body based on the target trajectory.

12. A program for causing a processor of a control device that operates a mobile body in a following mode to follow a person to be followed to execute the following processes: detecting the position of the person to be followed; determining a benefit function that indicates the degree to which it is recommended that the mobile body should move based on the detected position of the person to be followed; generating a target trajectory for the mobile body based on the benefit function; and controlling the mobile body based on the target trajectory.

Images