Control device for a mobile body, method for controlling a mobile body, and program

Abstract

To stabilize the behavior of a moving object while improving its ability to follow a predetermined trajectory. [Solution] A control device for an autonomously moving body, comprising: an acquisition unit that acquires the current position of the body, the current state of the body, and the target trajectory of the body; a control point setting unit that sets a plurality of first control points on a predicted trajectory based on the current position and the current state, and sets a plurality of second control points on the target trajectory of the 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 performs an evaluation of the curve.

Description

Technical Field

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

Background Art

[0002] In recent years, research has been underway on automatically controlling the driving of vehicles (hereinafter referred to as "automatic driving"). In automatic driving, a target trajectory is set according to the current position of the vehicle and the surrounding situation, and the vehicle is controlled to travel along this target trajectory. In relation to such technology, a technology for controlling the running of a vehicle has been proposed in consideration of, in addition to the current position of the vehicle, the steering situation, the wheel situation, the azimuth situation, etc. (see, for example, Patent Document 1).

Prior Art Documents

Patent Documents

[0003]

Patent Document 1

Summary of the Invention

Problems to be Solved by the Invention

[0004] In a vehicle traveling in automatic driving, in a scene where steering control is required, such as a curved road or an intersection, it is required to smooth the trajectory traveled by the vehicle from the current position to the target trajectory while stabilizing the behavior of the moving body and improving the followability with respect to the determined target trajectory. However, in the conventional technology, sufficient consideration has not been given to the control of the behavior of the moving body in the above-described scenes.

[0005] The present invention has been made in consideration of such circumstances, and one of the objectives is to provide a control device for a moving body, a control method for a moving body, and a program that can improve the followability to a target trajectory while stabilizing the behavior of the moving body.

Means for Solving the Problems

[0006] The control device for a mobile body, the control method for a mobile body, and the program according to this invention employ the following configuration. (1) A control device for a mobile body according to one aspect of the present invention is a control device for a mobile body capable of autonomous movement, comprising: 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 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 performs an evaluation of the curve.

[0007] (2) In the embodiment of (1) above, the control point setting unit sets a plurality of sets of the plurality of second control points on the target trajectory, the curve generation unit generates a plurality of curves based on the plurality of first control points and each of the plurality of sets, and the evaluation unit selects the curve with the highest evaluation among the plurality of curves based on the evaluation results for the plurality of curves.

[0008] (3) In the embodiment of (1) above, the evaluation unit performs the evaluation based on at least one of the area of ​​the portion enclosed by the target trajectory and the curve, the ratio of the area to the length of the curve, the maximum curvature of the curve, and the rate of change of the maximum curvature of the curve.

[0009] (4): In the embodiment of (2) above, the evaluation unit calculates an indicated value for the curvature of the movement of the moving body by performing curvature fitting to the selected curve.

[0010] (5) The embodiment of (4) above further includes a movement control unit that moves the moving body according to the calculated instruction value.

[0011] (6) In the embodiment of (4) above, the predicted trajectory has a predetermined length, and the evaluation unit performs the curvature fitting on the portion of the selected curve that is the length from the current position to the predetermined length.

[0012] (7) In the embodiment of (6) above, the predetermined length is variable according to the current speed of the moving body.

[0013] (8): In any of the embodiments described in (1) to (7) above, the curve is a B-spline curve.

[0014] (9) In any embodiment of (1) to (7) above, the current state of the moving body includes at least an indicator value relating to the curvature of the current movement of the moving body.

[0015] (10): In the embodiment of (9) above, the indicated value is the current steering angle of the moving body.

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

[0017] (12): In any embodiment of (1) to (7) above, 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.

[0018] (13): Another aspect of the present invention relates to a method for controlling a mobile body capable of autonomous movement, wherein a computer acquires the current position of the mobile body, the current state of the mobile body, and the target trajectory of the mobile body, sets a plurality of first control points on a predicted trajectory based on the current position and the current state, sets a plurality of second control points on the target trajectory of the mobile body, generates a curve based on the plurality of first control points and the plurality of second control points, and performs an evaluation of the curve.

[0019] (14): A program according to another aspect of the present invention is a program for controlling a mobile body capable of autonomous movement, which causes a computer to acquire the current position of the mobile body, the current state of the mobile body, and the target trajectory of the mobile body, and to set a plurality of first control points on a predicted trajectory 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 evaluate the curve.

Effects of the Invention

[0020] According to the aspects (1) to (14) above, it is possible to improve the followability to the target trajectory while stabilizing the behavior of the mobile body.

Brief Description of the Drawings

[0021] [Figure 1] It is a diagram showing an example of the configuration of the mobile body 1 and the control device 100 according to the embodiment. [Figure 2] It is a perspective view of the mobile body 1 according to the embodiment as seen from above. [Figure 3A] It shows an example of a scene where the tracking control according to the embodiment is performed. [Figure 3B] It is a diagram for explaining a state in which control points are set on each of the predicted trajectory PT and the target trajectory TT according to the embodiment. [Figure 3C] It is a diagram for explaining a state in which the first B-spline curve BS1 according to the embodiment is generated. [Figure 3D] It is a diagram for explaining the first evaluation area EA1 of the first B-spline curve BS1 according to the embodiment. [Figure 3E] It is a diagram for explaining a state in which the second B-spline curve BS2 according to the embodiment is generated. [Figure 3F] It is a diagram for explaining the second evaluation area EA2 of the second B-spline curve BS2 according to the embodiment. [Figure 3G] It is a diagram for explaining the third evaluation area EA3 of the third B-spline curve BS3 according to the embodiment. [Figure 3H] This figure illustrates the nth evaluation area EAn of the nth spline curve BSn according to the embodiment. [Figure 4A] This diagram illustrates how one B-spline curve (selected spline curve SS) is selected from among multiple B-spline curves according to the embodiment. [Figure 4B] This figure illustrates how the extracted spline path EP is extracted from the selected spline curve SS according to the embodiment. [Figure 4C] This figure illustrates how the indicated curvature of the moving body 1 is calculated based on the predicted trajectory PT and extracted spline path EP according to the embodiment. [Figure 5] This flowchart shows an example of the processing flow for follow-up control performed by the control device 100 according to this embodiment. [Modes for carrying out the invention]

[0022] The following describes embodiments of the control device for a mobile body, the control method for a mobile body, and the program of the present invention with reference to the drawings. The control device for a mobile body according to the embodiment achieves both stability of the mobile body's behavior and tracking performance toward a target trajectory by performing tracking control toward a target trajectory while considering the current state of the mobile body. The mobile body is, for example, a vehicle (e.g., a four-wheeled vehicle, a three-wheeled vehicle), a micromobility, a wheeled robot, an electric wheelchair, etc., equipped with autonomous mobility functions. In the following description, the case in which the mobile body is a micromobility will be used as an example. This micromobility moves on both roadways and predetermined areas different from roadways (e.g., sidewalks).

[0023] Figure 1 shows an example of the configuration of a mobile body 1 and a control device 100 according to an embodiment. The mobile body 1 is equipped with, for example, an external environment detection device 10, a mobile body sensor 12, an operator 14, a positioning device 16, a communication device 18, a mode switching switch 20, an HMI (Human Machine Interface) 22, a mobile mechanism 30, a drive device 40, a storage device 50, and a control device 100. Some of these components that are not essential for realizing the functions of the present invention may be omitted.

[0024] The external environment detection device 10 detects the external conditions of the moving object 1. For example, the external environment detection device 10 is a device whose detection range covers at least a portion of the area around the moving object 1 (including the direction of travel). The external environment detection device 10 includes external cameras, radar devices, LIDAR (Light Detection and Ranging), sensor fusion devices, etc. The external environment detection device 10 outputs information indicating the detection result (images, object positions, etc.) to the control device 100.

[0025] The moving sensor 12 includes, for example, a speed sensor, an acceleration sensor, a yaw rate (angular velocity) sensor, a compass sensor, and a manipulated amount detection sensor attached to the operator 14.

[0026] The control element 14 receives driving operations from the occupant of the mobile vehicle. The control element 14 includes, for example, controls for instructing acceleration and deceleration (e.g., an accelerator pedal, a brake pedal, a dial switch or lever for speed adjustment) and controls for instructing steering (e.g., a steering wheel). In this case, the mobile vehicle sensor 12 may include an accelerator opening sensor, a brake pedal pressure sensor, a steering torque sensor, etc. The mobile vehicle 1 may also be equipped with controls other than those described above as the control element 14 (e.g., a non-annular rotary control, a joystick, a button, etc.).

[0027] The positioning device 16 is a device that determines the position of the mobile object 1. The positioning device 16 is, for example, a GNSS (Global Navigation Satellite System) receiver, which determines the position of the mobile object 1 based on signals received from GNSS satellites and outputs it as position information. The position information of the mobile object 1 may be estimated from the position of the Wi-Fi base station to which the communication device 18 is connected.

[0028] The communication device 18 communicates with other mobile devices in the vicinity using, for example, a cellular network, a Wi-Fi network, Bluetooth®, DSRC (Dedicated Short Range Communication), or with various external devices (for example, a management server) via a wireless base station.

[0029] The mode selector switch 20 is a switch operated by the occupant. The mode selector switch 20 may be a mechanical switch or a GUI (Graphical User Interface) switch set on the touch panel of the HMI 22. The mode selector switch 20 accepts an operation to switch the driving mode to one of Mode A, Mode B, or Mode C, for example. Mode A is an assist mode in which either steering or acceleration / deceleration control is performed by the occupant, and the other is performed automatically. Mode A may include Mode A-1 in which steering is performed by the occupant and acceleration / deceleration control is performed automatically, and Mode A-2 in which acceleration / deceleration is performed by the occupant and steering control is performed automatically. Mode B is a manual driving mode in which steering and acceleration / deceleration are performed by the occupant. Mode C is an automatic driving mode in which steering and acceleration / deceleration control are performed automatically.

[0030] The HMI22 presents (or informs, notifies) various information to the occupant of the mobile unit 1 and accepts input operations from the occupant. The HMI22 includes various display devices, speakers, microphones, buzzers, touch panels, switches, keys, lamps, etc. For example, the HMI22 informs the occupant of the driving status of the mobile unit 1, which is controlled by the control device 100, in different notification modes depending on the driving status. The HMI22 also presents information from the control device 100, or information acquired from external devices via the communication device 18.

[0031] The moving mechanism 30 is a mechanism for moving the mobile body 1 on a road. The moving mechanism 30 is, for example, a group of wheels including steering wheels and drive wheels.

[0032] The drive unit 40 outputs force to the moving mechanism 30 to move the moving body 1. For example, the drive unit 40 includes a motor that drives the drive wheels, a battery that stores the power 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 or a fuel cell as a means of outputting driving force or generating power. Furthermore, the drive unit 40 may also be equipped with a braking device that uses frictional force or air resistance.

[0033] Figure 2 is a perspective view of the mobile body 1 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 mechanism 40. 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-seater, and the occupant P is seated in the driver's seat DS wearing a seat belt SB. Arrow α1 indicates the direction of travel (velocity vector) of the mobile body 1. The external environment detection device 10 is located near the front end of the mobile body 1, and the mode switching switch 20 is located on the boss of the steering wheel WH. An HMI 22 display device is located in front of the occupant P inside the mobile body.

[0034] Returning to Figure 1, the storage device 50 is a non-transient storage device such as an HDD (Hard Disk Drive), flash memory, or RAM (Random Access Memory). The storage device 50 stores map information 52, a program 54 executed by the control device 100, and other data. In the figure, the storage device 50 is shown outside the control device 100, but the storage device 50 may be included within the control device 100.

[0035] [Control device] 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 trajectory 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 function of the control unit 110 is realized, for example, by a hardware processor such as a CPU (Central Processing Unit) executing a program (software) 54. Some or all of these components may be realized by hardware (including circuitry) such as an LSI (Large Scale Integration), ASIC (Application Specific Integrated Circuit), FPGA (Field-Programmable Gate Array), or GPU (Graphics Processing Unit), or by the cooperation of software and hardware. The program may be stored in the storage device 50 in advance, or it may be stored on a removable storage medium (non-transient storage medium) such as a DVD or CD-ROM and installed in the storage device 50 when the storage medium is mounted on a drive device.

[0036] The acquisition unit 111 acquires various information from various external devices (for example, the external environment detection device 10, the mobile sensor 12, the operator 14, the positioning device 16, the communication device 18, the mode switching switch 20, and the HMI 22). For example, the acquisition unit 111 acquires information regarding the current state of the mobile body 1 output from the mobile sensor 12 and information regarding the current position of the mobile body 1 output from the positioning device 16. The acquisition unit 111 also acquires information regarding the target trajectory from the storage device 50.

[0037] The object recognition unit 112 recognizes objects present around the moving object 1 (for example, within a predetermined distance from the moving object 1) based on the output of the external environment detection device 10. Objects include some or all of the following: moving objects such as vehicles, bicycles, and pedestrians; road boundaries such as road markings, steps, guardrails, road shoulders, and median strips; structures installed on the road such as road signs and billboards; and obstacles such as fallen objects present (lying on the road). For example, the object recognition unit 112 acquires information such as the presence, location, and type of other moving objects by inputting the image captured by the external camera of the external environment detection device 10 into a trained model that has been trained to output information such as the presence, location, and type of an object when the image captured by the external camera is input.

[0038] The trajectory generation unit 113 automatically generates a target trajectory for the moving body 1 to travel in the future (without driver operation) so as to avoid approaching objects recognized by the object recognition unit 112. For example, the object recognition unit 112 sets a risk area centered on the object whose state has been outputted, and within the risk area, the object recognition unit 112 sets a risk as an index value indicating the degree to which the moving body 1 should not approach. The trajectory generation unit 113 generates a target trajectory so that the moving body 1 does not pass through points where the risk is above a predetermined value, and travels within the recognized driving lane. For example, the target trajectory is expressed as a sequence of points (trajectory points) that the moving body 1 should reach. Trajectory points are points that the moving body 1 should reach at predetermined driving distances (e.g., a few meters), and separately, target speed and target acceleration at predetermined sampling times (e.g., a few tenths of a second) are generated as part of the target trajectory. Alternatively, the trajectory points may be the positions that the moving body 1 should reach at each sampling time. In this case, information about the target velocity and target acceleration is represented by the interval between trajectory points.

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

[0040] The curve generation unit 115 generates curves 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 curves are, for example, spline curves (B-spline curves). The curve generation unit 115 generates multiple curves based on each of the sets of the plurality of first-half control points and the plurality of second-half control points. Details of the processing of the curve generation unit 115 will be described later.

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

[0042] 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 path and object information based on the output of the object recognition unit 112 and controls the motor MT of the drive unit 40 to maintain a distance of at least a certain level from objects in front of the mobile body 1, and if the distance to objects in front of the mobile body 1 is sufficiently long, to move the mobile body 1 at a predetermined speed. The movement control unit 117 also controls the steering device SD to change the steering angle of the steering wheels based on the amount of operation of the control element 14 such as the steering wheel.

[0043] In Mode A-2, the movement control unit 117 controls the steering device SD of the drive unit 40 so that the moving body 1 moves along the target trajectory generated by the trajectory generation unit 113. The movement control unit 117 moves the moving body 1 according to the indicative curvature calculated by the evaluation unit 116. Regarding acceleration and deceleration, the movement 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.

[0044] In mode B, the movement 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. The movement control unit 117 also controls the steering unit SD to change the steering angle of the steering wheels based on the amount of operation of the control element 14 such as the steering wheel.

[0045] In mode C, the movement 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 trajectory generated by the trajectory generation unit 113. The movement control unit 117 moves the moving body 1 according to the indicative curvature calculated by the evaluation unit 116.

[0046] [Follow-up control] The following section specifically describes the tracking control of the mobile body 1 to a target trajectory when the mobile body 1 is moving under the control of automatic driving (for example, Mode A-2, Mode C). In tracking control, a spline curve based on the current state of the mobile body 1 (indicated curvature, steering angle) and the target trajectory is used to derive an indicated curvature (steering angle) that balances the stability of the mobile body 1's behavior with its tracking performance to the target trajectory. Since spline curves are known to be curvature continuous, by appropriately setting the evaluation function used to select the spline curve, it is possible to set an asymptotic trajectory that allows for smooth travel and closely follows the target trajectory.

[0047] Figure 3A shows an example of a scenario in which tracking control of the mobile body 1 is performed. In Figure 3A, the mobile body 1 is traveling from its current position CP toward its current travel trajectory CT, and a target trajectory TT (also called the "reference path") has been set as the trajectory that the mobile body 1 is about to travel. The current travel trajectory CT is calculated, for example, based on the steering angle of the mobile body 1's steering wheel. The X-axis direction is the forward direction (direction of travel) of the mobile body 1, and the Y-axis direction is the direction perpendicular to the X-axis direction (left-hand direction relative to the direction of travel of the mobile body 1). As shown in the figure, when viewed from the current position CP, in the XY plane, the target trajectory TT extends in a direction shifted from the X-axis direction in the Y direction, while the current travel trajectory CT extends in a direction shifted from the X direction in the -Y direction. In such a scenario, if the mobile body 1's travel trajectory (current travel trajectory CT) is suddenly tried to match the target trajectory TT, a sudden change in steering will occur. To avoid this sudden change, in this embodiment, steering is controlled based on the following procedure.

[0048] (Setting control points) Figure 3B illustrates how control points are set on both the predicted trajectory PT (also called the "predicted path") and the target trajectory TT. First, a predicted trajectory PT with a predetermined reference length is set, 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 end 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 second-half control point P11 and the second second-half control point P12 (the interval between the second second-half control point P12 and the third second-half control point P13) may be the same as the interval between the first first-half control point P1 and the second first-half control point P2 (the interval between the second first-half control point P2 and the third first-half 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 tracking control, etc. The reference length is variable according to the current speed of the moving body 1. The reference length is set to increase as the speed of the moving body 1 increases.

[0049] (Generation and evaluation of spline curves) Figure 3C illustrates how a B-spline curve is set based on control points set 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 set on the predicted trajectory PT and three second-half control points (P11, P12, P13) set on the target trajectory TT.

[0050] Figure 3D illustrates the first evaluation area EA1 of the first B spline curve BS1. In the example in Figure 3D, the first B spline curve BS1 is evaluated based on the size of the first evaluation area EA1, which is the area enclosed 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.

[0051] (Setting control points, generating spline curves, and iterating through evaluation) Figure 3E illustrates how additional control points are set on the target trajectory TT. After the evaluation of the first B spline curve BS1 as described above, three more second-half control points (P14, P15, P16) are set on the target trajectory TT. The spacing between the newly set second-half control points (P14, P15, P16) is the same as the spacing between the initially set second-half control points (P11, P12, P13). The second B spline curve BS2 is generated based on the three first-half control points (P1, P2, P3) set on the predicted trajectory PT and the three second-half control points (P14, P15, P16) set on the target trajectory TT.

[0052] Figure 3F illustrates the second evaluation area EA2 of the second B spline curve BS2. In the example in Figure 3F, the second B spline curve BS2 is evaluated based on the size of the second evaluation area EA2, which is the area enclosed by the predicted trajectory PT, the target trajectory TT, the first B spline curve BS1, and the line segment connecting the first half control point P1 and the first half control point P11.

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

[0054] As shown in Figures 3A to 3H above, multiple sets of second-half control points are set along the entire target trajectory TT, and a B-spline curve is generated and evaluated based on each of these sets of second-half control points and the first-half control points. For example, multiple candidate points for second-half control points are set at equal intervals in advance along the target trajectory TT, and by repeatedly shifting the position of the first control point of each set of second-half control points by a predetermined number (e.g., one by one) along the multiple candidate points, multiple sets of second-half control points are set along the entire target trajectory TT.

[0055] (Determination of indicated curvature) Next, based on the evaluation results for each of the multiple B-spline curves described above, an asymptotic trajectory that allows for smooth movement and closely follows the target trajectory is set. Figure 4A illustrates how one B-spline curve (selected spline curve SS) is selected from among multiple B-spline curves. The selection of a B-spline curve is performed using an evaluation function. The evaluation function is designed, for example, based on the evaluation area calculated for each B-spline curve as described above. For example, the evaluation function is designed so that the smaller the evaluation area, the higher the evaluation value. In addition, the evaluation function may 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, or the rate of change of the maximum curvature of the B-spline curve.

[0056] Figure 4B illustrates how, in a selected spline curve SS, an extracted spline path EP of a predetermined length is extracted from the starting point of the selected spline curve 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 (reference length).

[0057] Figure 4C illustrates how the target curvature of the moving body 1 is calculated based on the predicted trajectory PT and the extracted spline path EP. For example, the target curvature is determined by performing curvature (curve) fitting (curvature fitting to the selected spline curve SS) based on the extracted spline path EP. Based on this target curvature, a control value (steering wheel steering angle) is calculated for the steering device SD, which is associated with the steering angle of the steering wheels. By controlling the steering angle based on the control value calculated in this way, the moving body 1 travels along a trajectory that passes between the predicted trajectory PT and the extracted spline path EP.

[0058] [Processing flow] Next, the processing flow of the follow control performed by the control device 100 will be described. Figure 5 is a flowchart showing an example of the processing flow of the follow control performed by the control device 100. The series of processes shown in Figure 5 are repeatedly performed at predetermined intervals while the mobile body 1 is traveling under the control of automatic driving. It is also assumed that the target trajectory generated by the trajectory generation unit 113 is stored in the memory device 50.

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

[0060] 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).

[0061] 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).

[0062] Next, the evaluation unit 116 performs an evaluation of the spline curve generated by the curve generation unit 115 (step S109). The evaluation unit 116 performs the evaluation based on at least one of the area enclosed by the target trajectory and the spline curve, the ratio of this area to the length of the curve, the maximum curvature of the curve, and the maximum rate of change of curvature of the curve.

[0063] Next, the evaluation unit 116 determines whether the evaluation of all of the multiple second-half control points set on the target trajectory has been completed (step S111). If it is determined that the evaluation is not complete (step S111; NO), the process returns to step S105, and the control point setting unit 114 sets several more second-half control points on the target trajectory and repeats the subsequent process.

[0064] If it is determined that the evaluation is complete (step S111; YES), the evaluation unit 116 selects the spline curve with the highest evaluation from among the multiple spline curves generated by the curve generation unit 115 (step S113).

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

[0066] According to the embodiments described above, it is possible to stabilize the behavior of the moving body while improving its ability to follow a determined trajectory. By performing tracking control to the target trajectory while considering the current state of the moving body (indicated curvature), it is possible to achieve both stability in the behavior of the moving body and tracking performance to the target trajectory. In particular, it is possible to suppress sudden changes in steering when the target trajectory changes significantly in response to changes in the destination or surrounding environment.

[0067] Furthermore, since the lanes on the road traveled by the moving object are generally defined by straight sections and curved sections, when setting the target curvature of the moving object 1, it may be possible to determine whether the moving object 1 is on a straight section or a curved section. This determination allows for setting a longer target trajectory length used to calculate the target curvature depending on the situation, thereby improving the stability of the steering wheel's behavior and its ability to follow the target trajectory.

[0068] The embodiments described above can be expressed as follows. A control device for a mobile body capable of autonomous movement, A storage medium that stores computer-readable instructions, A processor connected to the storage medium, The processor executes the computer-readable instructions to: The current position of the moving object, the current state of the moving object, and the target trajectory of the moving object are obtained. Multiple first control points are set on a predicted trajectory based on the current position and the current state. Multiple second control points are set on the target trajectory of the moving body, A curve is generated based on the plurality of first control points and the plurality of second control points. To evaluate the aforementioned curve, A control device for mobile vehicles.

[0069] Although embodiments for carrying out the present invention have been described above using examples, the present invention is not limited in any way to these embodiments, and various modifications and substitutions can be made without departing from the spirit of the present invention. [Explanation of Symbols]

[0070] 1 Mobile Unit 10. External detection devices 12 Mobile Sensors 14 Operators 16 Positioning device 18. Communication equipment 20 Mode selector switch 30 Moving mechanism 40 Drive unit 50 Storage device 100 Control device 110 Control Unit 111 Acquisition Department 112 Object recognition section 113 Trajectory generation part 114 Control point setting unit 115 Curve generator 116 Evaluation Department 117 Movement Control Unit

Claims

1. A control device for a mobile body capable of autonomous movement, An acquisition unit that acquires the current position of the moving body, the current state of the moving body, and the target trajectory of the moving body, A control point setting unit sets a plurality of first control points on a predicted trajectory based on the current position and the current state, and sets a plurality of second control points on the target trajectory of the moving object. A curve generation unit that generates a curve based on the plurality of first control points and the plurality of second control points, An evaluation unit that performs an evaluation of the aforementioned curve, A control device for a mobile body, comprising the above.

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

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

4. The evaluation unit calculates an indicated value for the curvature of the movement of the moving body by performing curvature fitting to the selected curve. A control device for a mobile body according to claim 2.

5. The system further includes a movement control unit that moves the moving body according to the calculated instruction value. A control device for a mobile body according to claim 4.

6. The aforementioned predicted trajectory has a predetermined length, The evaluation unit performs the curvature fitting on the portion of the selected curve that is a length from the current position to a predetermined length. A control device for a mobile body according to claim 4.

7. The predetermined length is variable according to the current speed of the moving body. A control device for a mobile body according to claim 6.

8. The aforementioned curve is a B-spline curve. A control device for a mobile body according to any one of claims 1 to 7.

9. The current state of the moving body includes at least an indicator value relating to the curvature of the current movement of the moving body. A control device for a mobile body according to any one of claims 1 to 7.

10. The indicated value is the current steering angle of the moving body. A control device for a mobile body according to claim 9.

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

12. 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. A control device for a mobile body according to any one of claims 1 to 7.

13. A method for controlling a mobile body capable of autonomous movement, wherein a computer controls the system. The current position of the moving object, the current state of the moving object, and the target trajectory of the moving object are obtained. Multiple first control points are set on the predicted trajectory based on the current position and the current state. Multiple second control points are set on the target trajectory of the moving body, A curve is generated based on the plurality of first control points and the plurality of second control points. To evaluate the aforementioned curve, A method for controlling a moving object.

14. A program for controlling an autonomously moving object, which is used by a computer. The current position of the moving object, the current state of the moving object, and the target trajectory of the moving object are acquired. Multiple first control points are set on a predicted trajectory based on the current position and the current state. Multiple second control points are set on the target trajectory of the moving object. A curve is generated based on the plurality of first control points and the plurality of second control points. An evaluation of the aforementioned curve will be performed. program.

Images