Mobile body control device, mobile body control method, and storage medium

Abstract

The invention provides a control device for a moving body, a control method for the moving body, and a storage medium, which can stabilize the behavior of the moving body and improve the followability to a target trajectory. A control device for a moving body capable of autonomously moving according to an embodiment includes: an acquisition unit that acquires a current position of the moving body, a current state of the moving body, and a target trajectory of the moving body; a control point setting unit that sets a plurality of first control points on a predicted trajectory obtained on the basis of the current position and the current state, and sets a plurality of second control points on a target trajectory of the moving body; a curve generation unit that generates a curve on the basis of the plurality of first control points and the plurality of second control points; and an evaluation unit that evaluates the curve.

Description

Technical Field

[0001] This invention relates to a control device for a mobile body, a control method for a mobile body, and a storage medium. Background Technology

[0002] In recent years, research on automatically controlling vehicle driving (hereinafter referred to as "autonomous driving") has made continuous progress. In autonomous driving, a target trajectory is set based on the vehicle's current position and surrounding conditions, and the vehicle is controlled to travel along that target trajectory. Related to this technology, techniques have been proposed to control vehicle driving by considering not only the vehicle's current position but also steering conditions, wheel conditions, orientation conditions, etc. (for example, see Japanese Patent Application Publication No. 2023-90509). Summary of the Invention

[0003] [The problem the invention aims to solve]

[0004] In vehicles operating in autonomous driving mode, scenarios requiring steering control, such as curves and intersections, demand a smooth trajectory from the vehicle's current position to the target path to stabilize the vehicle's behavior and improve its ability to follow the determined target path. However, previous technologies have not adequately considered the control of the vehicle's behavior in such scenarios.

[0005] The present invention was made in consideration of such circumstances, and one of its objectives is to provide a control device, a control method for a moving body, and a storage medium that can stabilize the behavior of a moving body while improving its tracking ability toward a target trajectory.

[0006] [Solution to the problem]

[0007] The control device, control method, and storage medium for the mobile body involved in this invention adopt the following structure.

[0008] (1): One aspect of the present invention relates to a control device for a mobile body, which is a control device for a mobile body capable of autonomous movement, wherein the control device for the mobile body comprises: an acquisition unit that acquires the current position of the mobile body, the current state of the mobile body, and the target trajectory of the mobile body; a control point setting unit that sets a plurality of first control points on a predicted trajectory obtained based on the current position and the current state, and sets a plurality of second control points on the target trajectory of the mobile body; a curve generation unit that generates a curve based on the plurality of first control points and the plurality of second control points; and an evaluation unit that evaluates the curve.

[0009] (2): In the above (1) scheme, the control point setting unit sets a group of multiple second control points on the target track, the curve generation unit generates multiple curves based on the multiple first control points and each group of the multiple groups, and the evaluation unit selects the curve with the highest evaluation from the multiple curves based on the evaluation results of the multiple curves.

[0010] (3): In the above (1) scheme, the evaluation unit performs the evaluation based on at least one of the area of ​​the portion surrounded by the target track and the curve, the ratio of the area to the length of the curve, the maximum curvature of the curve, and the maximum rate of change of curvature of the curve.

[0011] (4): In the above (2) scheme, the evaluation unit calculates an indication value related to the curvature of the movement of the moving body by performing curvature fitting on the selected curve.

[0012] (5): In the above (4) scheme, the control device of the moving body further includes a movement control unit that moves the moving body according to the calculated indicated value.

[0013] (6): In the above scheme (4), the predicted trajectory has a specified length, and the evaluation unit performs curvature fitting on the portion of the selected curve that is a length from the current position based on the specified length.

[0014] (7): In the above scheme (6), the specified length is variable according to the current speed of the moving body.

[0015] (8): In any of the above schemes (1) to (7), the curve is a B-spline curve.

[0016] (9): In any of the above schemes (1) to (7), the current state of the moving body includes at least an indication value related to the curvature of the current movement of the moving body.

[0017] (10): In the above scheme (9), the indicated value is the current turning angle of the moving body.

[0018] (11): In any of the above schemes (1) to (7), the number of the plurality of first control points is 3 and the number of the plurality of second control points is 3.

[0019] (12): In any of the above schemes (1) to (7), the interval between each of the plurality of first control points is the same as the interval between each of the plurality of second control points.

[0020] (13): Another aspect of the present invention relates to a control method for a mobile body, which is a control method for a mobile body capable of autonomous movement, wherein the control method for the mobile body causes a computer to perform the following processing: obtaining the current position of the mobile body, the current state of the mobile body, and the target trajectory of the mobile body; setting a plurality of first control points on a predicted trajectory obtained based on the current position and the current state; setting a plurality of second control points on the target trajectory of the mobile body; generating a curve based on the plurality of first control points and the plurality of second control points; and evaluating the curve.

[0021] (14): Another aspect of the present invention relates to a storage medium storing a program for controlling a mobile body capable of autonomous movement, wherein the program causes a computer to perform the following processes: obtaining the current position of the mobile body, the current state of the mobile body, and the target trajectory of the mobile body; setting a plurality of first control points on a predicted trajectory obtained based on the current position and the current state; setting a plurality of second control points on the target trajectory of the mobile body; generating a curve based on the plurality of first control points and the plurality of second control points; and evaluating the curve.

[0022] [Invention Effects]

[0023] According to the above schemes (1) to (14), the behavior of the moving body can be stabilized while the following ability to the target trajectory can be improved. Attached Figure Description

[0024] Figure 1 This is a diagram illustrating an example of the structure of the mobile body 1 and the control device 100 according to the embodiment.

[0025] Figure 2 This is a perspective view obtained from above, showing the movable body 1 involved in the embodiment.

[0026] Figure 3A An example of a scenario involving follow control as described in the implementation is shown.

[0027] Figure 3B This diagram illustrates the scenario where control points are set for both the predicted trajectory PT and the target trajectory TT, according to the implementation method.

[0028] Figure 3C This is a diagram illustrating the generation of the first B-spline curve BS1 according to the implementation method.

[0029] Figure 3D This is a graph illustrating the first evaluation area EA1 of the first B-spline curve BS1 involved in the implementation method.

[0030] Figure 3EThis is a diagram illustrating the generation of the second B-spline curve BS2 according to the implementation method.

[0031] Figure 3F This is a graph illustrating the second evaluation area EA2 of the second B-spline curve BS2 involved in the implementation method.

[0032] Figure 3G This is a graph illustrating the third evaluation area EA3 of the third B-spline curve BS3 involved in the implementation method.

[0033] Figure 3H This is a graph illustrating the nth evaluation area EAn of the nth B-spline curve BSn involved in the implementation method.

[0034] Figure 4A This is a diagram illustrating the scenario of selecting one B-spline curve (selecting spline curve SS) from a plurality of B-spline curves according to the implementation method.

[0035] Figure 4B This is a diagram illustrating the case of extracting the extraction spline path EP from the selected spline curve SS according to the implementation method.

[0036] Figure 4C This is a diagram illustrating the case in which the indicated curvature of the moving body 1 is calculated based on the predicted trajectory PT and the extracted spline path EP according to the implementation method.

[0037] Figure 5 This is a flowchart illustrating an example of the process of follow-up control performed by the control device 100 according to an embodiment. Detailed Implementation

[0038] Hereinafter, embodiments of the control device, control method, and storage medium for the mobile body according to the present invention will be described with reference to the accompanying drawings. The control device for the mobile body according to the embodiments performs tracking control towards a target track while considering the current state of the mobile body, thus balancing the stability of the mobile body's behavior and its tracking performance towards the target track. The mobile body may be, for example, a vehicle (e.g., a four-wheeled vehicle, a three-wheeled vehicle), a micro-mobile body, a wheeled robot, an electric wheelchair, or other mobile body capable of autonomous movement. Hereinafter, an example of a micro-mobile body will be described. This micro-mobile body can move both in a lane and in a designated area different from the lane (e.g., a sidewalk).

[0039] Figure 1This diagram illustrates an example of the structure of the mobile body 1 and control device 100 according to the embodiment. The mobile body 1 includes, for example, an external detection device 10, a mobile body sensor 12, an operating element 14, a positioning device 16, a communication device 18, a mode switching switch 20, an HMI (Human Machine Interface) 22, a movement mechanism 30, a drive device 40, a storage device 50, and a control device 100. It should be noted that structures that are not essential for achieving the functions of this invention may be omitted.

[0040] The external detection device 10 detects the external conditions of the moving body 1. For example, the external detection device 10 includes various devices that define at least a portion of the area surrounding the moving body 1 (including the direction of travel) as the detection range. The external detection device 10 includes external cameras, radar devices, LIDAR (Light Detection and Ranging) devices, sensor fusion devices, etc. The external detection device 10 outputs information showing the detection results (images, object positions, etc.) to the control device 100.

[0041] The moving body sensor 12 includes, for example, a speed sensor, an acceleration sensor, a yaw rate (angular velocity) sensor, an orientation sensor, and an operation quantity detection sensor mounted on the operating member 14.

[0042] The control element 14 accepts driving operations performed by the occupants of the moving body. The control element 14 may include, for example, controls for indicating acceleration and deceleration (e.g., accelerator pedal, brake pedal, speed adjustment DIP switch, lever) and controls for indicating steering (e.g., steering wheel). In this case, the moving body sensor 12 may also include a throttle opening sensor, a brake pedal position sensor, a steering torque sensor, etc. The moving body 1, as the control element 14, may also have control elements of other forms than those described above (e.g., non-ring-shaped rotary controls, joysticks, buttons, etc.).

[0043] Positioning device 16 is a device for detecting the position of mobile body 1. Positioning device 16 is, for example, a GNSS (Global Navigation Satellite System) receiver, which determines the position of mobile body 1 based on signals received from GNSS satellites and outputs it as position information. It should be noted that the position information of mobile body 1 can also be estimated based on the location of the Wi-Fi base station to which communication device 18 is connected.

[0044] The communication device 18 communicates with other mobile bodies in the vicinity of the mobile body, such as using cellular networks, Wi-Fi networks, Bluetooth (registered trademark), DSRC (Dedicated Short Range Communication), etc., or communicates with various external devices (e.g., management servers) via wireless base stations.

[0045] The mode switch 20 is an occupant-operated switch. The mode switch 20 can be a mechanical switch or a GUI (Graphical User Interface) switch located on the touch panel of the HMI 22. The mode switch 20, for example, accepts the operation of switching the driving mode to any of Mode A, Mode B, and Mode C. Mode A is an auxiliary mode where the occupant performs steering operation and acceleration / deceleration control, while the other is performed automatically. Mode A may also include Mode A-1, where the occupant performs steering operation and automatic acceleration / deceleration control, and Mode A-2, where the occupant performs acceleration / deceleration operation and automatic steering control. Mode B is a manual driving mode where the occupant performs steering operation and acceleration / deceleration control. Mode C is an automatic driving mode where steering control and acceleration / deceleration control are performed automatically.

[0046] HMI22 provides various information to the occupants of mobile vehicle 1 (either by prompting or reporting, notifying them) and accepts input operations performed by the occupants. HMI22 includes various display devices, speakers, microphones, buzzers, touch panels, switches, buttons, lights, etc. For example, HMI22 reports the driving status of mobile vehicle 1 controlled by control device 100 to the occupants in different reporting formats depending on the driving status. Additionally, HMI22 prompts, for example, information from control device 100 and information obtained from external devices by communication device 18.

[0047] The moving mechanism 30 is a mechanism for moving the moving body 1 on a road. The moving mechanism 30 is, for example, a wheel assembly including steering wheels and drive wheels.

[0048] The drive unit 40 outputs force to the moving mechanism 30, causing the moving body 1 to move. For example, the drive unit 40 includes a motor that drives the drive wheels, a battery that stores the electricity supplied to the motor, and a steering device that adjusts the steering angle of the steering wheels. The drive unit 40 may also be equipped with an internal combustion engine, fuel cell, etc., as a drive force output mechanism or a power generation mechanism. In addition, the drive unit 40 may also be equipped with a braking device that brakes by friction or air resistance.

[0049] Figure 2This is a perspective view of the mobile body 1 taken from above. In the figure, FW is the steering wheel, RW is the drive wheel, SD is the steering mechanism, MT is the motor, and BT is the battery. The steering mechanism SD, motor MT, and battery BT are included in the drive unit 40. Additionally, AP is the accelerator pedal, BP is the brake pedal, WH is the steering wheel, SP is the speaker, and MC is the microphone. The mobile body 1 shown is a single-occupant vehicle; the occupant P sits in the driver's seat DS and wears the seatbelt SB. Arrow α1 indicates the direction of travel (velocity vector) of the mobile body 1. An external detection device 10 is located near the front end of the mobile body 1, and a mode switch 20 is located at the hub of the steering wheel WH. Furthermore, an HMI 22 serving as a display device is located in front of the occupant P inside the mobile body.

[0050] return Figure 1 The storage device 50 is a non-temporary storage device such as an HDD (Hard Disk Drive), flash memory, or RAM (Random Access Memory). Map information 52 and programs 54 executed by the control device 100 are stored in the storage device 50. In the diagram, the storage device 50 is shown outside the frame of the control device 100, but it may be included within the control device 100.

[0051] [Control device]

[0052] The control device 100 includes, for example, a control unit 110. The control unit 110 includes, for example, an acquisition unit 111, an object recognition unit 112, a track generation unit 113, a control point setting unit 114, a curve generation unit 115, an evaluation unit 116, and a movement control unit 117. Each functional unit of the control unit 110 is implemented, for example, by executing a program (software) 54 using a hardware processor such as a CPU (Central Processing Unit). Some or all of these components can also be implemented using hardware (including circuitry) such as LSI (Large Scale Integration) circuits, ASIC (Application Specific Integrated Circuit), FPGA (Field-Programmable Gate Array), and GPU (Graphics Processing Unit), or through the coordinated operation of software and hardware. The program can be pre-saved in the storage device 50, or stored in a removable storage medium (non-temporary storage medium) such as a DVD or CD-ROM, and installed in the storage device 50 by mounting the storage medium onto a drive device.

[0053] The acquisition unit 111 acquires various information from various external devices (e.g., external detection device 10, moving body sensor 12, operating element 14, positioning device 16, communication device 18, mode switch 20, and HMI 22). For example, the acquisition unit 111 acquires information related to the current state of the moving body 1 output from the moving body sensor 12, and information related to the current position of the moving body 1 output from the positioning device 16. In addition, the acquisition unit 111 acquires information related to the target trajectory from the storage device 50.

[0054] The object recognition unit 112 identifies one or more objects existing around the moving body 1 (e.g., within a predetermined distance of the moving body 1) based on the output of the external detection device 10. An object includes moving bodies such as vehicles, bicycles, and pedestrians; road boundaries such as road markings, steps, guardrails, shoulders, and median strips; road signs and notice boards; and obstacles such as objects that have fallen onto the road. The object recognition unit 112 obtains information such as the presence, location, and category of other moving bodies by inputting images captured by an external camera into a learned model that outputs information such as the presence, location, and category of objects when images captured by the external camera are input to the external detection device 10.

[0055] The track generation unit 113 generates a target track for the mobile body 1 to travel automatically (independent of driver operation) in order to avoid approaching objects identified by the object recognition unit 112. For example, the object recognition unit 112 sets a risk zone centered on the object whose status has been output. Within the risk zone, the object recognition unit 112 sets a risk level as an indicator value showing the degree to which the mobile body 1 should not approach the object. The track generation unit 13 generates the target track in a way that the mobile body 1 will not pass through locations where the risk level is above a predetermined value, and will instead travel within the identified driving lane. For example, the target track is represented by a track obtained by sequentially arranging the locations (track points) that the mobile body 1 should reach. The track points are the locations that the mobile body 1 should reach every predetermined distance (e.g., several meters) along the route. In contrast, the target speed and target acceleration are generated as part of the target track at predetermined sampling times (e.g., a fraction of a second). Alternatively, the track points can also be the positions that the mobile body 1 should reach at the sampling time at predetermined sampling times. In this case, the target velocity and target acceleration information are represented by the intervals of the orbital points.

[0056] The control point setting unit 114 sets multiple first control points (hereinafter referred to as "first half control points") on the predicted trajectory obtained based on the current position and state of the moving body 1, and sets multiple second control points (hereinafter referred to as "second half control points") on the target track of the moving body. The control point setting unit 114 sets multiple groups of the multiple second half control points on the target track. The predicted trajectory has a predetermined length. Details regarding the processing of the control point setting unit 114 will be described later.

[0057] The curve generation unit 115 generates a curve based on a plurality of first half control points and a plurality of second half control points set by the control point setting unit 114. The generated curve is, for example, a spline curve (B-spline curve). The curve generation unit 115 generates multiple curves based on each group of the plurality of first half control points and the plurality of second half control points. Details regarding the processing of the curve generation unit 115 will be described later.

[0058] Evaluation unit 116 evaluates the curve generated by curve generation unit 115. Evaluation unit 116 selects the curve with the highest evaluation from among the multiple curves generated by curve generation unit 115. Evaluation unit 116 evaluates based on at least one of the following: the area enclosed by the target track and the curve, the ratio of that area to the length of the curve, the maximum curvature of the curve, and the maximum rate of change of curvature of the curve. Evaluation unit 116 calculates an indication value (hereinafter referred to as "indication curvature") related to the curvature of the movement of the moving body 1 by performing curvature fitting on the selected curve. Indication curvature is, for example, a control value of the steering device SD associated with the steering angle of the steering wheel. Evaluation unit 116 performs curvature fitting on the portion of the selected curve that is a length based on a predetermined length from the current position of the moving body 1. Details regarding the processing of evaluation unit 116 will be described later.

[0059] The movement control unit 117 controls the movement of the mobile body 1. In mode A-1, the movement control unit 117 refers to the information about the driving path and objects obtained from the output of the object recognition unit 112, maintains the distance between itself and objects in front of the mobile body 1 at a certain level or higher, and controls the motor MT of the drive unit 40 to move the mobile body 1 at a predetermined speed when the distance between itself and objects in front of the mobile body 1 is sufficiently long. In addition, the movement control unit 117 controls the steering device SD by changing the steering angle of the steering wheel based on the amount of operation of the steering wheel or other operating components 14.

[0060] In mode A-2, the motion control unit 117 controls the steering device SD of the drive unit 40 so that the moving body 1 moves along the target track generated by the track generation unit 113. The motion control unit 117 moves the moving body 1 according to the indicated curvature calculated by the evaluation unit 116. Regarding acceleration and deceleration, the motion control unit 117 controls the motor MT of the drive unit 40 based on the speed of the moving body 1 and the amount of operation of the accelerator pedal or brake pedal.

[0061] In Mode B, the motion control unit 117 controls the motor MT of the drive unit 40 based on the speed of the moving body 1 and the amount of operation of the accelerator pedal or brake pedal. Furthermore, the motion control unit 117 controls the steering device SD by changing the steering wheel angle based on the amount of operation of the steering wheel or other operating components 14.

[0062] In mode C, the motion control unit 117 controls the steering device SD and motor MT of the drive unit 40 so that the moving body 1 moves along the target track generated by the track generation unit 113. The motion control unit 117 moves the moving body 1 according to the indicated curvature calculated by the evaluation unit 116.

[0063] [Follower Control]

[0064] The following describes in detail the following follow-up control of mobile body 1 when it is moving under the control of autonomous driving (e.g., mode A-2, mode C) and following a target track. In the follow-up control, a spline curve based on the current state of mobile body 1 (indicating curvature, steering angle) and the target track is used to derive the indicating curvature (steering angle), which balances the stability of mobile body 1's behavior and its following performance on the target track. Regarding the spline curve, the curvature is known to be continuous; therefore, by appropriately setting the evaluation function used for selecting the spline curve, smooth driving and setting an asymptotic track that highly follows the target track can be achieved.

[0065] Figure 3A This illustrates an example of a scenario involving the follow-up control of a moving body 1. Figure 3AThe following example illustrates a scenario where a mobile body 1 is traveling from its current position CP toward its current travel track CT, and a target track TT (also called a "reference path") is set as the path from which the mobile body 1 will move. The current travel track CT is calculated, for example, based on the steering angle of the steering wheel of the mobile body 1. The X-axis direction is the forward direction (travel direction) of the mobile body 1, and the Y-axis direction is set as the direction orthogonal to the X-axis direction (left-hand direction relative to the travel direction of the mobile body 1). As shown, when viewed from the current position CP, in the XY plane, the target track TT extends in a direction offset from the X-axis direction to the Y-axis direction, while the current travel track CT extends in a direction offset from the X-axis direction to the -Y-axis direction. In such a scenario, when the travel track of the mobile body 1 (the current travel track CT) is suddenly aligned with the target track TT, a sharp change in steering occurs. To avoid this sharp change, the steering is controlled in this embodiment based on the following steps.

[0066] (Control point setting)

[0067] Figure 3B This diagram illustrates the setup of control points on both the predicted trajectory PT (also known as the "predicted path") and the target trajectory TT. First, a predicted trajectory PT with a specified reference length is established, starting from the current position CP. Three equally spaced first-half control points (P1, P2, P3) are set on this predicted trajectory PT. The first first-half control point P1 coincides with the current position CP (the starting point of the predicted trajectory PT), the third first-half control point P3 coincides with the ending point of the predicted trajectory PT, and the second first-half control point P2 is the midpoint between the first first-half control point P1 and the third first-half control point P3. Next, the first second-half control point P11, the second second-half control point P12, and the third second-half control point P13 are set on the target trajectory TT. The interval between the first rear control point P11 and the second rear control point P12 (and the interval between the second rear control point P12 and the third rear control point P13) can also be the same as the interval between the first front control point P1 and the second front control point P2 (and the interval between the second front control point P2 and the third front control point P3). The reference length can be arbitrarily set based on the speed of the moving body 1, the length of the time interval for follow control, etc. The reference length is variable according to the current speed of the moving body 1. It is set such that the faster the speed of the moving body 1, the longer the reference length.

[0068] (Generation and evaluation of spline curves)

[0069] Figure 3CThis diagram illustrates the scenario where a B-spline curve is set based on control points established on the predicted trajectory PT and the target trajectory TT. The first B-spline curve BS1 is generated based on three first-half control points (P1, P2, P3) set on the predicted trajectory PT and three second-half control points (P11, P12, P13) set on the target trajectory TT.

[0070] Figure 3D This is a graph illustrating the first evaluated area EA1 of the first B-spline curve BS1. Figure 3D In the example, the first B-spline curve BS1 is evaluated based on the size of the first evaluation area EA1, which is the portion surrounded by the predicted trajectory PT, the target trajectory TT, the first B-spline curve BS1, and the line segment connecting the first first half control point P1 and the first second half control point P11.

[0071] (Setting control points, generating spline curves, and iterating the evaluation)

[0072] Figure 3E This diagram illustrates the scenario where additional control points are set on the target trajectory TT. After evaluating the first B-spline curve BS1 as described above, three additional rear control points (P14, P15, P16) are set on the target trajectory TT. The intervals of the newly set rear control points (P14, P15, P16) are the same as the intervals of the initially set rear control points (P11, P12, P13). The second B-spline curve BS2 is generated based on the three front control points (P1, P2, P3) set on the predicted trajectory PT and the three rear control points (P14, P15, P16) set on the target trajectory TT.

[0073] Figure 3F This is a graph illustrating the second evaluation area EA2 of the second B-spline curve BS2. Figure 3F In the example, the second B-spline curve BS2 is evaluated based on the size of the second evaluation area EA2, which is the portion surrounded by the predicted trajectory PT, the target trajectory TT, the first B-spline curve BS1, and the line segment connecting the first first half control point P1 and the first second half control point P11.

[0074] Subsequently, the setting of additional control points on the target trajectory TT, the generation and evaluation of B-spline curves were repeatedly performed. Figure 3G In the process, a third B-spline curve BS3 is generated based on other rear control points (P17, P18, P19) set on the target trajectory TT, and the third evaluation area EA3 is evaluated. Figure 3H In the process, the nth B-spline curve BSn is generated based on the other rear control points (Pa, Pa+1, Pa+2) set on the target trajectory TT, and the nth evaluation area EAN is evaluated.

[0075] As mentioned above Figures 3A to 3H As shown, multiple groups of rear control points are set over the entire range of the target orbit TT, and B-spline curves are generated and evaluated based on each group of these multiple rear control points and the front control points. For example, the process of pre-setting multiple candidate points of rear control points at equal intervals on the target orbit TT and staggering the positions of the initial control points of each group of rear control points by a specified number of times along the multiple candidate points (e.g., one by one) is repeated, thereby setting multiple groups of rear control points over the entire range of the target orbit TT.

[0076] (Determination of curvature)

[0077] Next, based on the evaluation results of the multiple B-spline curves as described above, an asymptotic trajectory that can travel smoothly and closely follow the target trajectory is set. Figure 4A This diagram illustrates the case of selecting one B-spline curve from multiple B-spline curves (selecting spline curve SS). The selection of the B-spline curve is performed using an evaluation function. This evaluation function is designed, for example, based on the evaluation area calculated for each B-spline curve, as described above. For instance, the evaluation function is designed such that the smaller the evaluation area, the higher the evaluation value. Alternatively, the evaluation function can also be based on at least one of the following: the ratio of the evaluation area to the length of the B-spline curve, the maximum curvature of the B-spline curve, and the rate of change of the maximum curvature of the B-spline curve.

[0078] Figure 4B This diagram illustrates the process of extracting a spline path EP from the selected spline curve SS, with a specified length from the starting point of SS (e.g., the current position CP). The length of the extracted spline path EP is, for example, twice the length of the predicted trajectory PT (the baseline length).

[0079] Figure 4C This diagram illustrates the calculation of the indicated curvature of the moving body 1 based on the predicted trajectory PT and the extracted spline path EP. For example, curvature (curve) fitting (curvature fitting of the selected spline curve SS) is performed based on the extracted spline path EP, thereby determining the indicated curvature. Based on this indicated curvature, a control value (steering wheel angle) for the steering device SD, which is associated with the steering angle of the steering wheels, is calculated. The steering angle is controlled based on this calculated control value, thereby allowing the moving body 1 to travel on a track between the predicted trajectory PT and the extracted spline path EP.

[0080] [Processing Flow]

[0081] Next, the process of the follow-up control executed by the control device 100 will be explained. Figure 5This is a flowchart illustrating an example of the process of follow-up control performed by the control device 100. Figure 5 The series of processes shown are repeatedly executed at a predetermined cycle during the period when the mobile body 1 is traveling under the control of automatic driving. In addition, the target track generated by the track generation unit 113 is stored in the storage device 50.

[0082] First, the acquisition unit 111 acquires the current state of the moving body 1 output from the moving body sensor 12, the current position of the moving body 1 output from the positioning device 16, and the target track stored in the storage device 50 (step S101). The current state of the moving body 1 includes at least the current indicated curvature (steering angle) of the moving body 1.

[0083] Next, the control point setting unit 114 sets multiple first-half control points on the predicted trajectory of the moving body 1 (step S103). Furthermore, the control point setting unit 114 sets multiple second-half control points on the target trajectory (step S105).

[0084] Next, the curve generation unit 115 generates a spline curve (B-spline curve) based on the multiple first half control points and multiple second half control points set by the control point setting unit 114 (step S107).

[0085] Next, the evaluation unit 116 evaluates the spline curve generated by the curve generation unit 115 (step S109). The evaluation unit 116 evaluates the curve based on at least one of the following: the area enclosed by the target track and the spline curve, the ratio of the area to the length of the curve, the maximum curvature of the curve, and the maximum rate of change of curvature of the curve.

[0086] Next, the evaluation unit 116 determines whether the evaluation of all the multiple rear control points set on the target track has been completed (step S111). If the evaluation is determined to be incomplete (step S111: "No"), the process returns to step S105, and the control point setting unit 114 further sets other multiple rear control points on the target track and repeats the subsequent processing.

[0087] If the evaluation is deemed complete (step S111: "Yes"), the evaluation unit 116 selects the spline curve with the highest evaluation from the multiple spline curves generated by the curve generation unit 115 (step S113).

[0088] Next, the evaluation unit 116 calculates the indicated curvature (steering angle) of the moving body 1 by performing curvature fitting on the selected spline curve (step S115). Then, the movement control unit 117 performs steering control of the moving body 1 based on the indicated curvature calculated by the evaluation unit 116 (step S117).

[0089] According to the implementation method described above, the behavior of the moving body can be stabilized while improving its tracking performance on the determined trajectory. By considering the current state of the moving body (indicating curvature) while performing tracking control on the target trajectory, both the stability of the moving body's behavior and the tracking performance on the target trajectory can be balanced. In particular, in cases where the target trajectory changes significantly due to changes in the destination or surrounding environment, abrupt changes in turning can be suppressed.

[0090] It should be noted that the travel path of the moving body is defined by approximately straight sections and curved sections (curves). Therefore, when setting the indicated curvature of moving body 1, it is possible to determine whether moving body 1 is in a straight section or a curved section. Through this determination, the length of the calculated target track used for indicating curvature can be set longer according to the situation, thereby improving the stability of the steering wheel's behavior and its ability to follow the target track.

[0091] The implementation methods described above can be performed as follows.

[0092] A control device for a mobile body, wherein the mobile body is capable of autonomous movement, wherein...

[0093] The control device for the moving body includes:

[0094] Storage medium, which stores computer-readable instructions; and

[0095] The processor, which is connected to the storage medium,

[0096] The processor performs the following processing by executing computer-readable instructions:

[0097] Obtain the current position of the moving object, the current state of the moving object, and the target trajectory of the moving object;

[0098] Multiple first control points are set on the predicted trajectory obtained based on the current position and the current state;

[0099] Multiple second control points are set on the target track of the moving body;

[0100] A curve is generated based on the plurality of first control points and the plurality of second control points; and

[0101] The curve is then evaluated.

[0102] The above describes specific embodiments of the present invention, but the present invention is not limited to such embodiments in any way. Various modifications and substitutions can be made without departing from the spirit of the present invention.

Claims

1. A mobile body control device that is a control device of a mobile body capable of autonomous movement, wherein the mobile body control device comprises: an acquisition unit that acquires a current position of the mobile body, a current state of the mobile body, and a target trajectory of the mobile body; a control point setting unit that sets a plurality of first control points on a predicted trajectory obtained based on the current position and the current state, and sets a plurality of second control points on the target trajectory of the mobile body; a curve generating unit that generates a curve based on the plurality of first control points and the plurality of second control points; and an evaluation unit that performs evaluation of the curve.

2. The mobile body control device according to claim 1, wherein the control point setting unit sets a plurality of groups of the plurality of second control points on the target trajectory, the curve generating unit generates a plurality of the curves based on the plurality of first control points and each of the plurality of groups, and the evaluation unit selects a curve with the highest evaluation from among the plurality of curves based on evaluation results of the plurality of curves.

3. The mobile body control device according to claim 1, wherein the evaluation unit performs the evaluation based on at least one of an area of a portion enclosed by the target trajectory and the curve, a ratio of the area to a length of the curve, a maximum curvature of the curve, and a maximum curvature change rate of the curve.

4. The mobile body control device according to claim 2, wherein the evaluation unit calculates an index value related to a curvature of movement of the mobile body by performing curvature fitting on the selected curve.

5. The mobile body control device according to claim 4, wherein the mobile body control device further comprises a movement control unit that moves the mobile body in accordance with the calculated index value.

6. The mobile body control device according to claim 4, wherein the predicted trajectory has a prescribed length, and the evaluation unit performs the curvature fitting on a portion of the selected curve having a length from the current position by the prescribed length.

7. The mobile body control device according to claim 6, wherein the prescribed length is variable in accordance with a current speed of the mobile body.

8. The mobile body control device according to any one of claims 1 to 7, wherein the curve is a B-spline curve.

9. The mobile body control device according to any one of claims 1 to 7, wherein the current state of the mobile body includes at least an index value related to a curvature of current movement of the mobile body.

10. The mobile body control device according to claim 9, wherein the index value is a current steering angle of the mobile body.

11. The mobile body control device according to any one of claims 1 to 7, wherein the number of the plurality of first control points is three, and the number of the plurality of second control points is three.

12. The mobile body control device according to any one of claims 1 to 7, wherein the interval of each of the plurality of first control points is the same as the interval of each of the plurality of second control points. ​ ​ ​ ​ ​ ​ ​ ​ ​ ​ ​ ​ ​ ​ ​ ​ ​ ​ ​ ​ 13. A control method of a mobile body, which is a control method of a mobile body capable of autonomous movement, in which the control method of the mobile body causes a computer to perform the following processing: acquire a current position of the mobile body, a current state of the mobile body, and a target trajectory of the mobile body; set a plurality of first control points on a predicted trajectory obtained based on the current position and the current state; set a plurality of second control points on the target trajectory of the mobile body; generate a curve based on the plurality of first control points and the plurality of second control points; and perform evaluation of the curve.

14. A storage medium storing a program for controlling a mobile body capable of autonomous movement, in which the program causes a computer to perform the following processing: acquire a current position of the mobile body, a current state of the mobile body, and a target trajectory of the mobile body; set a plurality of first control points on a predicted trajectory obtained based on the current position and the current state; set a plurality of second control points on the target trajectory of the mobile body; generate a curve based on the plurality of first control points and the plurality of second control points; and perform evaluation of the curve. ​ ​

Images