Mobile body control system and control method
Patent Information
- Authority / Receiving Office
- WO · WO
- Patent Type
- Applications
- Current Assignee / Owner
- Filing Date
- 2025-02-07
- Publication Date
- 2026-08-13
Smart Images
Figure JP2025004256_13082026_PF_FP_ABST
Abstract
Description
Control System and Control Method for a Moving Body
[0001] The present invention relates to a control system and a control method for a moving body.
[0002] In recent years, the demand for ultra-small moving bodies (micromobility) to support people's movement within a small area has been increasing. Micromobility includes those with a seating capacity of about one person, or those that travel with people while carrying luggage instead of people boarding. Patent Document 1 describes a differential two-wheel drive type robot device.
[0003] International Publication No. 2022 / 075082
[0004] In a differential two-wheel drive type moving body, the wheel speeds of the left wheel and the right wheel are controlled individually. Therefore, when there is a failure in the sensors or drive unit of the moving body, unintended behavior may occur. Some aspects of the present invention aim to provide a technique for reducing the unintended behavior of a moving body.
[0005] According to some embodiments, there is provided a control system for a moving body having a drive unit that individually drives a left wheel and a right wheel, the control system including: trajectory determination means for determining a target trajectory of the moving body; target determination means for determining a target left wheel speed of the left wheel and a target right wheel speed of the right wheel for moving the moving body along the target trajectory, and a target value of a movement parameter related to the turning of the moving body; acquisition means for acquiring a measured value of the movement parameter based on an output from a sensor of the moving body; correction means for correcting the target left wheel speed and the target right wheel speed so that the difference between the target value and the measured value of the movement parameter is reduced; and control means for controlling the drive unit to drive the left wheel and the right wheel at the corrected target left wheel speed and the corrected target right wheel speed.
[0006] According to some embodiments, the unintended behavior of the moving body can be reduced.
[0007] Other features and advantages of the present invention will become apparent from the following description with reference to the accompanying drawings. In the accompanying drawings, the same or similar components are denoted by the same reference numerals.
[0008] The attached drawings are included in the specification and constitute a part thereof, illustrating embodiments of the present invention and are used to explain the principles of the present invention together with the description thereof. Schematic diagram showing an example of the configuration of a mobile body according to some embodiments. Schematic diagram showing an example of the configuration of a mobile body according to some embodiments. Block diagram showing an example of the configuration of the control system of a mobile body according to some embodiments. Block diagram showing an example of the functional configuration related to the control unit of a mobile body according to some embodiments. Block diagram showing an example of the configuration of a travel control unit according to some embodiments. Block diagram showing a detailed example of the configuration of a travel control unit according to some embodiments. Block diagram showing another detailed example of the configuration of a travel control unit according to some embodiments. Block diagram showing another detailed example of the configuration of a travel control unit according to some embodiments. Block diagram showing the unit angle of a mobile body according to some embodiments. Block diagram showing an example of a redundant sensor configuration according to some embodiments. Block diagram showing another example of the configuration of a travel control unit according to some embodiments. Block diagram showing another detailed example of the configuration of a travel control unit according to some embodiments. Flow diagram showing a control method for a mobile body according to some embodiments.
[0009] The embodiments will be described in detail below with reference to the attached drawings. Note that the following embodiments do not limit the invention as defined in the claims, and not all combinations of features described in the embodiments are essential to the invention. Two or more of the features described in the embodiments may be combined in any way. Furthermore, identical or similar configurations will be given the same reference numeral, and redundant descriptions will be omitted.
[0010] <Configuration of the Mobile Unit> The configuration of the mobile unit 100 will be described with reference to Figures 1A and 1B. In the following embodiment, as an example of the mobile unit 100, an ultra-small vehicle with a passenger capacity of about one person will be described. Such a vehicle may be called a micromobility. The vehicle may be an electric vehicle or a vehicle that runs on other power sources. The mobile unit 100 recognizes the driving area and generates a route using images taken by the mobile unit 100 itself, and autonomously drives according to the generated route. Furthermore, the mobile unit 100 generates a route using positioning results from a positioning sensor and map information. The mobile unit 100 may be capable of autonomous driving regardless of whether there is a person riding in the mobile unit 100. The mobile unit 100 may also be capable of driving according to the driving operations of an occupant.
[0011] Figure 1A shows a side view of the mobile body 100 according to this embodiment, and Figure 1B shows the internal structure of the mobile body 100. In the figures, arrow X indicates the front-to-back direction (length direction) of the mobile body 100, with F indicating the front and R indicating the rear. Arrows Y and Z indicate the width direction (left-to-right direction) and the up-and-down direction of the mobile body 100, respectively.
[0012] The mobile vehicle 100 is an electric autonomous vehicle equipped with a driving unit 112 and powered primarily by a battery 113. The battery 113 is a secondary battery such as a lithium-ion battery, and the mobile vehicle 100 is self-propelled by the driving unit 112 using the power supplied from the battery 113. The driving unit 112 takes the form of a tricycle, equipped with a left wheel 120L, a right wheel 120R, and a rear wheel 120S. The left wheel 120L and the right wheel 120R are drive wheels. The left wheel 120L and the right wheel 120R are sometimes collectively referred to as the drive wheels 120. The rear wheel 120S is a driven wheel. The driving unit 112 may also take the form of a four-wheeled vehicle or other forms. The mobile vehicle 100 is equipped with, for example, a single-person seat 111.
[0013] The driving unit 112 includes a drive unit 121 that drives the drive wheels 120. The drive unit 121 includes a left motor 122L, a right motor 122R, and a drive mechanism 123. The drive mechanism 123 is attached to the frame 117 of the mobile body 100. The drive mechanism 123 is a mechanism that rotates the drive wheels 120. The left motor 122L is a drive source for driving the left wheel 120L. When the left motor 122L drives the left wheel 120L, the left wheel 120L rotates. The right motor 122R is a drive source for driving the right wheel 120R. When the right motor 122R drives the right wheel 120R, the right wheel 120R rotates. The drive unit 121 can move the mobile body 100 forward or backward by individually driving the left wheel 120L and the right wheel 120R. The drive unit 121 can also change the direction of travel of the mobile body 100 by creating a difference in the rotational speed of the left wheel 120L and the right wheel 120R. The rotational speed of the wheels is called the wheel speed. The rear wheel 120S is rotatable with the Z direction as its axis of rotation.
[0014] The mobile body 100 is equipped with detection units 114 to 116 that detect targets in the vicinity of the mobile body 100. The detection units 114 to 116 are a group of external sensors that monitor the vicinity of the mobile body 100. In this embodiment, the detection units 114 to 116 are all imaging devices (cameras) that capture images of the vicinity of the mobile body 100, and for example, they include an optical system such as a lens and an image sensor. However, radar or lidar (Light Detection and Ranging) may be used instead of or in addition to the imaging device.
[0015] The detection units 114 are arranged, for example, in pairs at a distance in the Y direction from the front of the moving body 100, and are mainly used to detect targets in front of the moving body 100. The detection units 115 are arranged on the left and right sides of the moving body 100, respectively, and are mainly used to detect targets to the sides of the moving body 100. The detection unit 116 is arranged at the rear of the moving body 100 and is mainly used to detect targets behind the moving body 100.
[0016] <Control System Configuration> Figure 2 is a block diagram of the control system of the mobile unit 100. The mobile unit 100 is equipped with a control unit (ECU) 130. The control unit 130 includes one or more processors, such as a CPU, memory devices such as semiconductor memory, and interfaces with external devices. Therefore, the control unit 130 is a type of computer. The memory devices store programs executed by the processor and data used by the processor for processing. Multiple sets of processors, memory devices, and interfaces may be provided according to the function of the mobile unit 100 and configured to communicate with each other.
[0017] The control unit 130 acquires the outputs of the detection units 114 to 116 (e.g., image information), the input information of the operation unit 131, and the audio information input from the audio input device 133, and executes the corresponding processing. The control unit 130 controls the left motors 122L and 122R (driving control of the driving unit 112), controls the display of the display panel included in the operation unit 131, provides audio notifications to the occupants of the mobile body 100 via the speaker 132, and outputs information. Specifically, the control unit 130 controls the left motor 122L by supplying a control signal to the left motor driver 124L. The control unit 130 controls the right motor 122R by supplying a control signal to the right motor driver 124R. The control unit 130 can supply separate control signals to the left motor driver 124L and the right motor driver 124R. The left motor driver 124L and the right motor driver 124R are also included in the drive unit 121 of the mobile body 100.
[0018] The control unit 130 may perform processing on the outputs from the detection units 114 to 116 using a machine learning model for image recognition (e.g., a deep neural network). The control unit 130 may also perform processing on the output from the voice input device 133 using a machine learning model for speech recognition (e.g., a deep neural network). In this way, the control unit 130 controls the mobile body 100. Therefore, the control unit 130 may be considered a control device for the mobile body 100.
[0019] The voice input device 133 includes, for example, a microphone and captures the voices of the occupants of the mobile unit 100. The control unit 130 recognizes the input voice and can perform corresponding processing. The GNSS (Global Navigation Satellite system) sensor 134 is a positioning sensor that receives GNSS signals and detects the current position of the mobile unit 100.
[0020] The storage device 135 includes a recording medium for storing various data. The storage device 135 may also store programs executed by the processor and data used by the processor for processing. The storage device 135 may also store various parameters of machine learning models for speech recognition and image recognition executed by the control unit 130 (e.g., trained parameters and hyperparameters of a deep neural network). The storage device 135 may also store map information of the location where the mobile body 100 is moving. The communication device 136 is a communication device that can communicate with an external device (e.g., a communication terminal 140 owned by the user) via wireless communication such as Wi-Fi® or fifth-generation mobile communication. The sensor group 137 is one or more sensors for measuring the state of the components of the mobile body 100 and the state of the surroundings of the mobile body 100. Examples of sensors included in the sensor group 137 will be described later. The output from the sensor group 137 is supplied to the control unit 130.
[0021] <Functional Configuration of the Control Unit> Next, an example of the functional configuration of the control unit 130 will be described with reference to Figure 3. The user instruction acquisition unit 301 acquires user instructions input via the operation unit 131 or the voice input device 133. The user instructions may include specifying the destination location to which the mobile unit 100 should arrive. The destination location may be the location of an object recognized in the image output by the detection units 114 to 116, and specified by the spoken voice. Furthermore, the destination location may be a geographical location specified by the user using the mobile unit 100, or a geographical location that has been set in advance by the administrator of the mobile unit 100 and stored in the storage device 135. For example, the mobile unit 100 may be set to autonomously return to a pre-set geographical location after use by the user.
[0022] The image information processing unit 302 recognizes the position, shape, etc. of obstacles based on the output (e.g., image information) of the detection units 114 to 116. The recognition of the position, shape, etc. of obstacles in front of the moving body 100 is performed, for example, by determining the depth from the moving body 100 using stereo images obtained from the two detection units 114. To recognize obstacles, a pre-trained machine learning model for image recognition (e.g., a deep neural network) may be used with monocular or stereo images.
[0023] The map management unit 303 manages a map of the environment in which the mobile device 100 is used. This map may have restricted areas set. The map may be stored in the storage device 135 of the mobile device 100. The map management unit 303 may also receive maps from an external server and store them in the storage device 135.
[0024] The action planning unit 304 determines the trajectory that the moving body 100 should follow and the speed that the moving body 100 should take. The trajectory that the moving body 100 should follow is referred to as the target trajectory of the moving body 100. The speed that the moving body 100 should take is referred to as the target speed of the moving body 100. For example, if there are no obstacles in the straight-line direction from the current position to the target position, the action planning unit 304 generates a target trajectory along the straight-line direction. If there are obstacles in the straight-line direction from the current position to the target position, the action planning unit 304 generates a target trajectory such that the moving body approaches the target position while avoiding these obstacles. The action planning unit 304 may also generate a target trajectory such that the angular acceleration of the moving body 100 is below a threshold.
[0025] The driving control unit 305 controls the mobile body 100 according to the target trajectory and target speed supplied by the action planning unit 304. Specifically, the driving control unit 305 supplies control signals to the left motor driver 124L and the right motor driver 124R so that the mobile body 100 moves according to the target trajectory and target speed. If the mobile body 100 is a vehicle, the movement of the mobile body 100 can be expressed as the driving of the mobile body 100. If the driving control unit 305 receives an instruction from the user such as turning right, turning left, or stopping while the mobile body 100 is driving, it may control the mobile body 100 to drive according to this instruction.
[0026] <Configuration of the Driving Control Unit> Referring to Figure 4, the configuration of the driving control unit 305 will be described. The target value determination unit 401 determines the target values for each of the multiple movement parameters of the moving body 100 based on the target trajectory and target speed supplied from the action planning unit 304. Movement parameters are parameters related to the movement of the moving body 100. The movement parameters may be either parameters that define the operation of the actuator for moving the moving body 100 (for example, wheel speed) or parameters that represent the state of the moving body 100 as it moves due to the operation of the actuator (for example, the speed, acceleration, angular velocity, angular acceleration, yaw angle, difference in wheel speed of the moving body 100).
[0027] In some embodiments, the target value determination unit 401 determines target values for the wheel speed of the left wheel 120L and the wheel speed of the right wheel 120R in order to move the mobile body 100 along a target trajectory and at a target speed, and supplies these target values to the correction unit 403. The target value for the wheel speed of the left wheel 120L is expressed as the target left wheel speed, and the target value for the wheel speed of the right wheel 120R is expressed as the target right wheel speed. Furthermore, the target value determination unit 401 determines target values for movement parameters different from the target left wheel speed and target right wheel speed, and supplies these target values to the correction value determination unit 404. As will be described later, these movement parameters are used to stabilize the behavior of the mobile body 100. For this reason, these movement parameters are expressed as stabilization parameters, and their target values are expressed as stabilization target values. The stabilization parameters may be parameters related to the turning of the mobile body 100 (for example, the difference in wheel speeds, yaw angle, angular velocity, angular acceleration). The parameters related to the rotation of the mobile body 100 are parameters that may change while the mobile body 100 is rotating.
[0028] The measurement value acquisition unit 402 acquires measured values of multiple movement parameters based on the output from each sensor of the sensor group 137 of the moving body 100, and supplies these measured values to other components. In some embodiments, the measurement value acquisition unit 402 acquires measured values of the wheel speed of the left wheel 120L, the wheel speed of the right wheel 120R, and the stabilization parameters described above. The measured value of the wheel speed of the left wheel 120L is referred to as the measured left wheel speed, and the measured value of the wheel speed of the right wheel 120R is referred to as the measured right wheel speed. The measured value of the stabilization parameters is referred to as the stabilization measurement value.
[0029] The correction value determination unit 404 determines correction values for the target left wheel speed and target right wheel speed, respectively, based on the stabilized target value supplied from the target value determination unit 401 and the stabilized measured value supplied from the measured value acquisition unit 402, and supplies these values to the correction unit 403. The correction value for the target left wheel speed is referred to as the left wheel speed correction value, and the correction value for the target right wheel speed is referred to as the right wheel speed correction value. Specifically, the correction value determination unit 404 determines the left wheel speed correction value and the right wheel speed correction value in such a way as to reduce the difference between the stabilized target value and the stabilized measured value.
[0030] The correction unit 403 corrects the target left wheel speed supplied from the target value determination unit 401 based on the left wheel speed correction value, and supplies the corrected target left wheel speed to the drive control unit 405L. The correction unit 403 also corrects the target right wheel speed supplied from the target value determination unit 401 based on the right wheel speed correction value, and supplies the corrected target right wheel speed to the drive control unit 405R. Through these corrections, the target left wheel speed and target right wheel speed are corrected to reduce the difference between the stabilized target value and the stabilized measured value.
[0031] The drive control unit 405L controls the drive unit 121 of the mobile body 100 so as to drive the left wheel 120L at the target left wheel speed corrected by the correction unit 403 (i.e., so that the left wheel 120L rotates at the target left wheel speed corrected by the correction unit 403). Specifically, the drive control unit 405L supplies a control current to the left motor driver 124L that drives the left wheel 120L at the target left wheel speed corrected by the correction unit 403. The left motor driver 124L drives the left wheel 120L (i.e., rotates it) by supplying a drive current to the left motor 122L in accordance with the control current supplied from the drive control unit 405L. The drive control unit 405L may generate the control current by feedforward (FB) control, by feedback (FB) control using the measured left wheel speed, or by both of these controls. The drive control unit 405R controls the drive unit 121 of the mobile body 100 to drive the right wheel 120R at the target right wheel speed corrected by the correction unit 403, in the same manner as the drive control unit 405L.
[0032] If there are no malfunctions in the sensor group 137 or the drive unit 121, the behavior of the mobile body 100 can be stabilized by individually controlling the left wheel speed and the right wheel speed. However, if there are malfunctions in the sensor group 137 or the drive unit 121, the behavior of the mobile body 100 may not be stable even if the left wheel speed and the right wheel speed are controlled individually. For example, if the driving force of the left wheel speed is reduced due to a malfunction in the drive unit 121, and the right wheel speed is maintained at the target wheel speed, the mobile body 100 will turn to the left unintentionally. Conversely, if the driving force of the left wheel speed becomes excessive due to a malfunction in the drive unit 121, and the right wheel speed is maintained at the target right wheel speed, the mobile body 100 will turn to the right unintentionally. Similarly, if there is a malfunction in the sensor for measuring the left wheel speed and the right wheel speed is maintained at the target right wheel speed, the mobile body 100 will turn to the right or left unintentionally.
[0033] In some embodiments, the driving control unit 305 corrects the target left wheel speed and target right wheel speed based on parameters related to the turning of the moving body 100, as described above. Therefore, even if a malfunction occurs in the sensor group 137 or the drive unit 121, unintended behavior of the moving body 100 (for example, turning) can be mitigated.
[0034] <Specific Configuration of the Driving Control Unit> Referring to Figure 5, a specific configuration example of the sensor group 137, measurement value acquisition unit 402, correction unit 403, correction value determination unit 404, and drive control units 405L and 405R will be described. In the example in Figure 5, the difference between the wheel speed of the left wheel 120L and the wheel speed of the right wheel 120R is used as a stabilization parameter. The wheel speed of the left wheel 120L is expressed as the left wheel speed, and the wheel speed of the right wheel 120R is expressed as the right wheel speed. The difference between the left wheel speed and the right wheel speed is expressed as the wheel speed difference. The wheel speed difference is an example of a parameter related to the turning of the moving body 100. The target value determination unit 401 determines the target value of the wheel speed difference as the stabilization target value. The target value of the wheel speed difference is expressed as the target wheel speed difference. The measurement value acquisition unit 402 acquires the measured value of the wheel speed difference as the stabilization measured value. The measured value of the wheel speed difference is expressed as the measured wheel speed difference.
[0035] The sensor group 137 includes a left wheel speed sensor 501L for measuring the left wheel speed and a right wheel speed sensor 501R for measuring the right wheel speed. The measurement value acquisition unit 402 acquires the measured left wheel speed, the measured right wheel speed, and the measured wheel speed difference based on the outputs of the left wheel speed sensor 501L and the right wheel speed sensor 501R. Specifically, the measurement value acquisition unit 402 includes a left wheel speed acquisition unit 502L that receives the output from the left wheel speed sensor 501L and a right wheel speed acquisition unit 502R that receives the output from the right wheel speed sensor 501R. Furthermore, the measurement value acquisition unit 402 includes a subtraction unit 503 that calculates the difference between the measured left wheel speed and the measured right wheel speed. In the example in Figure 5, the wheel speed difference is the value obtained by subtracting the right wheel speed from the left wheel speed. Alternatively, the wheel speed difference may be the value obtained by subtracting the left wheel speed from the right wheel speed.
[0036] The correction value determination unit 404 includes a wheel speed difference FB controller 504 that determines left wheel speed correction values and right wheel speed correction values based on the target wheel speed difference supplied from the target value determination unit 401 and the measured wheel speed difference supplied from the measurement value acquisition unit 402. When the measured wheel speed difference is greater than the target wheel speed difference, the wheel speed difference FB controller 504 considers that the moving body 100 is moving to the right of the target trajectory, so it sets the left wheel speed correction value to a negative value and the right wheel speed correction value to a positive value. As a result, the actual trajectory of the moving body 100 approaches the target trajectory. On the other hand, when the measured wheel speed difference is smaller than the target wheel speed difference, the wheel speed difference FB controller 504 considers that the moving body 100 is moving to the left of the target trajectory, so it sets the left wheel speed correction value to a positive value and the right wheel speed correction value to a negative value. As a result, the actual trajectory of the moving body 100 approaches the target trajectory. The absolute values of the left wheel speed correction value and the right wheel speed correction value may each be half the difference between the measured wheel speed difference and the target wheel speed difference. When the measured wheel speed difference is equal to the target wheel speed difference, both the left wheel speed correction value and the right wheel speed correction value are set to 0.
[0037] The correction unit 403 includes adders 505L and 505R. Adder 505L corrects the target left wheel speed by adding a left wheel speed correction value supplied by the correction value determination unit 404 to the target left wheel speed supplied by the target value determination unit 401. As described above, the left wheel speed correction value can be a positive value, a negative value, or zero. Therefore, the target left wheel speed may increase, decrease, or remain constant depending on the correction of the target left wheel speed. Similarly, adder 505R corrects the target right wheel speed by adding a right wheel speed correction value supplied by the correction value determination unit 404 to the target right wheel speed supplied by the target value determination unit 401.
[0038] The drive control unit 405L includes a left wheel speed FF unit 506L, a left wheel speed FB controller 507L, and an adder unit 508L. The left wheel speed FF unit 506L calculates the FF current to be supplied to the left motor driver 124L in order to rotate the left wheel 120L at a corrected target left wheel speed, taking into consideration the acceleration of the moving body 100, the air resistance experienced by the moving body 100, and the frictional resistance experienced by the moving body 100 from the road surface, and supplies this FF current to the adder unit 508L. The left wheel speed FF unit 506L may also obtain the measured values necessary for calculating the FF current from the sensor group 137, and the sensor group 137 may include sensors for measuring these measured values. The left wheel speed FB controller 507L generates an FB current to reduce the difference between the corrected target left wheel speed and the measured left wheel speed, and supplies this FB current to the adder unit 508L. The FB current can be a positive value, a negative value, or zero. The left wheel speed FB controller 507L may include an integral term. The adder 508L adds the FF current and the FB current to generate a control current and supplies this control current to the left motor driver 124L. The drive control unit 405R, like the drive control unit 405L, includes the right wheel speed FF unit 506R and the right wheel speed FB controller 507R.
[0039] Referring to Figure 6, another specific configuration example of the sensor group 137, measurement value acquisition unit 402, and correction value determination unit 404 will be described. The configuration of the correction unit 403 and drive control unit 405 may be the same as in Figure 5. In the example in Figure 6, the yaw angle of the moving body 100 is used as the stabilization parameter. The yaw angle is an example of a parameter related to the rotation of the moving body 100. The yaw angle of the moving body 100 is the rotation angle around the Z direction in Figure 1A. In the following description, the yaw angle when the front of the moving body 100 is facing true north is set to 0 degrees, and clockwise rotation is considered positive. The target value determination unit 401 determines the target value of the yaw angle as the stabilization target value. The target value of the yaw angle is referred to as the target yaw angle. The measurement value acquisition unit 402 acquires the measured value of the yaw angle as the stabilization measured value. The measured value of the yaw angle is referred to as the measured yaw angle.
[0040] The sensor group 137 includes a left wheel speed sensor 501L and a right wheel speed sensor 501R, as well as a yaw angle sensor 601 for measuring the yaw angle of the moving body 100. The measurement value acquisition unit 402 acquires the measured yaw angle based on the output of the yaw angle sensor 601. Specifically, the measurement value acquisition unit 402 includes a left wheel speed acquisition unit 502L and a right wheel speed acquisition unit 502R, as well as a yaw angle acquisition unit 602 that receives the output from the yaw angle sensor 601.
[0041] The correction value determination unit 404 includes a yaw angle FB controller 603 that determines the left wheel speed correction value and the right wheel speed correction value based on the target yaw angle supplied from the target value determination unit 401 and the measured yaw angle supplied from the measurement value acquisition unit 402. When the measured yaw angle is greater than the target yaw angle, the yaw angle FB controller 603 considers that the moving body 100 is moving to the right of the target trajectory, so it sets the left wheel speed correction value to a negative value and the right wheel speed correction value to a positive value. This brings the actual trajectory of the moving body 100 closer to the target trajectory. On the other hand, when the measured yaw angle is smaller than the target yaw angle, the yaw angle FB controller 603 considers that the moving body 100 is moving to the left of the target trajectory, so it sets the left wheel speed correction value to a positive value and the right wheel speed correction value to a negative value. This brings the actual trajectory of the moving body 100 closer to the target trajectory. The absolute values of the left wheel speed correction value and the absolute values of the right wheel speed correction value may each be determined as a function of the difference between the measured yaw angle and the target yaw angle. This function may be determined by prior testing and stored in the memory of the control unit 130. When the measured yaw angle is equal to the target yaw angle, both the left wheel speed correction value and the right wheel speed correction value are set to 0.
[0042] Referring to Figure 7, another specific configuration example of the sensor group 137, measurement value acquisition unit 402, and correction value determination unit 404 will be described. The configuration of the correction unit 403 and drive control unit 405 may be the same as in Figure 5. In the example of Figure 7, the unit angle of the mobile body 100 is used as a stabilization parameter. The unit angle is an example of a parameter related to the rotation of the mobile body 100. The unit angle of the mobile body 100 is the angle of the drive mechanism 123 with respect to the frame 117 of the mobile body.
[0043] Referring to FIG. 8, the unit angle of the moving body 100 will be described. When the moving body 100 is moving straight, the drive mechanism 123 extends in the width direction (Y direction) of the moving body 100. This position is taken as the reference position of the drive mechanism 123, and the unit angle at the reference position is set to 0 degrees. When the moving body 100 turns left, the drive mechanism 123 rotates counterclockwise from the reference position. The unit angle when the drive mechanism 123 rotates counterclockwise from the reference position is set to be negative. When the moving body 100 turns right, the drive mechanism 123 rotates clockwise from the reference position. The unit angle when the drive mechanism 123 rotates clockwise from the reference position is set to be positive.
[0044] The target value determination unit 401 determines the target value of the unit angle as the stabilization target value. The target value of the unit angle is represented as the target unit angle. The measurement value acquisition unit 402 acquires the measured value of the unit angle as the stabilization measurement value. The measured value of the unit angle is represented as the measured unit angle.
[0045] The sensor group 137 includes a unit angle sensor 701 for measuring the unit angle of the moving body 100 in addition to the left wheel speed sensor 501L and the right wheel speed sensor 501R. The measurement value acquisition unit 402 acquires the measured unit angle based on the output of the unit angle sensor 701. Specifically, the measurement value acquisition unit 402 includes a unit angle acquisition unit 702 that receives the output from the unit angle sensor 701 in addition to the left wheel speed acquisition unit 502L and the right wheel speed acquisition unit 502R.
[0046] The correction value determination unit 404 includes a unit angle FB controller 703 that determines a left wheel speed correction value and a right wheel speed correction value based on the target unit angle supplied from the target value determination unit 401 and the measured unit angle supplied from the measurement value acquisition unit 402. When the measured unit angle is larger than the target unit angle, the unit angle FB controller 703 considers that the moving body 100 is moving to the right of the target trajectory, so it sets the left wheel speed correction value to a negative value and the right wheel speed correction value to a positive value. Thereby, the actual trajectory of the moving body 100 approaches the target trajectory. On the other hand, when the measured unit angle is smaller than the target unit angle, the unit angle FB controller 703 considers that the moving body 100 is moving to the left of the target trajectory, so it sets the left wheel speed correction value to a positive value and the right wheel speed correction value to a negative value. Thereby, the actual trajectory of the moving body 100 approaches the target trajectory. The absolute value of the left wheel speed correction value and the absolute value of the right wheel speed correction value may each be determined by a function of the difference between the measured unit angle and the target unit angle. This function may be determined by prior tests and stored in the memory of the control unit 130. When the measured unit angle is equal to the target unit angle, both the left wheel speed correction value and the right wheel speed correction value are set to 0.
[0047] In the above example, any one of the wheel speed difference, yaw angle, or unit angle is used as the stabilization parameter. Instead of this, different movement parameters (for example, angular velocity, angular acceleration, etc.) may be used as the stabilization parameter, or a combination of a plurality of movement parameters may be used as the stabilization parameter.
[0048] <Redundant Configuration of Sensor Group> Referring to FIG. 9, a modified example of the configuration of the moving body 100 described above will be described. As shown in FIG. 9, the sensors included in the sensor group 137 of the moving body 100 may be redundant. In FIG. 9, all the sensors described in the various configurations above are included in the sensor group 137.
[0049] The left wheel speed sensor 501L is redundant, consisting of left wheel speed sensors 501La and 501Lb, each of which measures the left wheel speed. The left wheel speed acquisition unit 502L is also redundant, consisting of left wheel speed acquisition units 502La and 502Lb. The left wheel speed acquisition unit 502La acquires output from the left wheel speed sensor 501La and outputs its measured value to the statistical processing unit 901. The statistical processing unit 901 determines a representative value of the measured left wheel speed supplied from the left wheel speed acquisition unit 502La and the measured left wheel speed supplied from the left wheel speed acquisition unit 502Lb, and supplies this representative value as the measured left wheel speed to other components. The representative value can be the maximum value, minimum value, average value, etc. For example, the statistical processing unit 901 may use the maximum value as the representative value. As a result, compared to when other values are used as representative values, if the two measured left wheel speeds have different values due to a malfunction in one of the left wheel speed sensors, the mobile body 100 is controlled so that the left wheel speed is reduced.
[0050] Similarly, the right wheel speed sensor 501R is redundant, consisting of a right wheel speed sensor 501Ra and a right wheel speed sensor 501Rb, and the right wheel speed acquisition unit 502R is also redundant, consisting of a right wheel speed acquisition unit 502Ra and a right wheel speed acquisition unit 502Rb. The statistical processing unit 902 determines a representative value of the measured right wheel speed supplied from the right wheel speed acquisition unit 502Ra and the measured right wheel speed supplied from the right wheel speed acquisition unit 502Rb, and supplies this representative value as the measured right wheel speed to the other components.
[0051] Similarly, the yaw angle sensor 601 is redundant, consisting of yaw angle sensor 601a and yaw angle sensor 601b, and the yaw angle acquisition unit 602 is also redundant, consisting of yaw angle acquisition unit 602a and yaw angle acquisition unit 602b. The statistical processing unit 903 determines a representative value of the measured yaw angle supplied from the yaw angle acquisition unit 602a and the measured yaw angle supplied from the yaw angle acquisition unit 602b, and supplies this representative value as the measured yaw angle to the other components. For example, the statistical processing unit 903 may use the average value as the representative value.
[0052] Similarly, the unit angle sensor 701 is redundant, consisting of unit angle sensor 701a and unit angle sensor 701b, and the unit angle acquisition unit 702 is also redundant, consisting of unit angle acquisition unit 702a and unit angle acquisition unit 702b. The statistical processing unit 904 determines a representative value of the measured unit angle supplied from the unit angle acquisition unit 702a and the measured unit angle supplied from the unit angle acquisition unit 702b, and supplies this representative value as the measured unit angle to the other components. For example, the statistical processing unit 904 may use the average value as the representative value.
[0053] <Fault Detection> Referring to Figure 10, a modified example of the configuration of the mobile body 100 described above will be explained. As shown in Figure 10, the travel control unit 305 further includes a fault detection unit 1001. The fault detection unit 1001 detects faults in the sensors included in the sensor group 137 based on the output from the sensor group 137. In response to the detection of a sensor fault, the fault detection unit 1001 controls the drive unit 121 to suppress the movement of the mobile body 100. Suppression of the movement of the mobile body 100 may be deceleration of the mobile body 100, or deceleration until the mobile body 100 stops. When the mobile body 100 is stopped, suppression of the movement of the mobile body 100 may be maintaining the stopped state. For example, in response to the detection of a sensor fault, the fault detection unit 1001 may supply stop signals to the left motor driver 124L and the right motor driver 124R to stop the operation of the left motor 122L and the right motor 122R. In response to the supply of a stop signal, the left motor driver 124L and the right motor driver 124R turn off the drive current, regardless of the control current value. This suppresses the rotation of both the left wheel 120L and the right wheel 120R.
[0054] The fault detection unit 1001 detects a fault in the left wheel drive unit based on the FB current supplied from the drive control unit 405L. The left wheel drive unit is the part of the drive unit 121 that drives the left wheel 120L. In response to the detection of a fault in the left wheel drive unit, the fault detection unit 1001 controls the drive unit 121 to suppress the movement of the mobile body 100. For example, in response to the detection of a fault in the left wheel drive unit, the fault detection unit 1001 may supply a stop signal to the left motor driver 124L to stop the operation of the left motor 122L. In response to the supply of the stop signal, the left motor driver 124L turns off the drive current regardless of the value of the control current. This suppresses the rotation of the left wheel 120L. Even if a fault in the left wheel drive unit is detected, the fault detection unit 1001 does not have to supply a stop signal to the right motor driver 124R to stop the operation of the right motor 122R. As described above, the driving control unit 305 corrects the target right wheel speed by feedback control of stabilization parameters. Therefore, when the rotation of the left wheel 120L is suppressed, the target right wheel speed is corrected so that the rotation of the right wheel 120R is also suppressed.
[0055] Similarly, the fault detection unit 1001 detects a fault in the right wheel drive unit based on the FB current supplied from the drive control unit 405R. The right wheel drive unit is the part of the drive unit 121 that drives the right wheel 120R. Similarly, the fault detection unit 1001 controls the drive unit 121 to suppress the movement of the mobile body 100 in response to the detection of a fault in the right wheel drive unit.
[0056] Referring to Figure 11, a specific configuration example of the fault detection unit 1001 will be described. The left wheel speed sensor fault detection unit 1101L detects a fault in the redundant left wheel speed sensor 501L when the difference between the measured left wheel speed supplied from the left wheel speed acquisition unit 502La and the measured left wheel speed supplied from the left wheel speed acquisition unit 502Lb is greater than a threshold. In response to detecting a fault in the left wheel speed sensor 501L, the left wheel speed sensor fault detection unit 1101L supplies a sensor fault signal to the OR unit 1105. Similarly, the right wheel speed sensor fault detection unit 1101R detects a fault in the right wheel speed sensor 501R, the yaw angle sensor fault detection unit 1102 detects a fault in the yaw angle sensor 601, and the unit angle sensor fault detection unit 1103 detects a fault in the unit angle sensor 701.
[0057] The OR unit 1105 supplies sensor fault signals to OR unit 1106L and OR unit 1106R respectively when a sensor fault signal is supplied from at least one of the left wheel speed sensor fault detection unit 1101L, the right wheel speed sensor fault detection unit 1101R, the yaw angle sensor fault detection unit 1102, and the unit angle sensor fault detection unit 1103.
[0058] The left drive unit fault detection unit 1104L detects a fault in the left wheel drive unit when the FB current supplied from the left wheel speed FB controller 507L is greater than a threshold. Upon detecting a fault in the left wheel drive unit, the left drive unit fault detection unit 1104L supplies a left drive unit fault signal to the OR unit 1106L. In this example, the left drive unit fault detection unit 1104L detects a fault in the left wheel drive unit based on the FB current. Alternatively, the left drive unit fault detection unit 1104L may detect a fault in the left wheel drive unit based on other information (e.g., a left wheel speed correction value). Similarly, the right drive unit fault detection unit 1104R detects a fault in the right wheel drive unit when the FB current supplied from the right wheel speed FB controller 507R is greater than a threshold.
[0059] The OR unit 1106L supplies a stop signal to the left motor driver 124L in response to the supply of at least one of the sensor fault signal and the left drive unit fault signal. As a result, the fault detection unit 1001 stops the rotation of the left wheel 120L when it detects a fault in the left wheel drive unit or when it detects a fault in at least one sensor. The OR unit 1106R supplies a stop signal to the right motor driver 124R in response to the supply of at least one of the sensor fault signal and the right drive unit fault signal. As a result, the fault detection unit 1001 stops the rotation of the right wheel 120R when it detects a fault in the right wheel drive unit or when it detects a fault in at least one sensor.
[0060] <Method for controlling the mobile body> Referring to Figure 12, a method by which the control unit 130 controls the mobile body 100 will be described. Each step of the method in Figure 12 may be executed by the processor of the control unit 130 executing a program stored in the memory of the control unit 130. Alternatively, at least some of the steps of the method in Figure 12 may be executed by a dedicated integrated circuit such as an ASIC (Application Specific Integrated Circuit). The method in Figure 12 may be started in response to the user instructing the movement of the mobile body 100, and may be terminated in response to the user instructing the user to terminate. The control unit 130 repeatedly executes the processes S1201 to S1205 (for example, with a 100ms cycle).
[0061] In S1201, the control unit 130 (for example, the action planning unit 304) determines the target trajectory and target speed as described above. In S1202, the control unit 130 (for example, the target value determination unit 401) determines the target values of the multiple movement parameters as described above. In S1203, the control unit 130 (for example, the measurement value acquisition unit 402) acquires the measured values of the multiple movement parameters as described above. In S1203, the control unit 130 (for example, the measurement value acquisition unit 402) acquires the measured values of the multiple movement parameters as described above. In S1204, the control unit 130 (for example, the correction unit 403 and the correction value determination unit 404) corrects the target left wheel speed and target right wheel speed, respectively, as described above. In S1205, the control unit 130 (for example, the drive control units 405L and 405R) controls the drive unit 121 using the corrected target left wheel speed and target right wheel speed as described above.
[0062] In the above-described embodiment, the method shown in Figure 12 is executed by the control unit 130. In this case, the control system for controlling the mobile body 100 is comprised of the control unit 130. Alternatively, at least one step of the method shown in Figure 12 may be executed by an external server connected to the mobile body 100 via a network. In other words, the method shown in Figure 12 may be executed by the cooperation of the external server and the control unit 130. In this case, the control system for controlling the mobile body 100 is comprised of this server and the control unit 130. Furthermore, when all steps of the method shown in Figure 12 are executed by the external server, the control system for controlling the mobile body 100 is comprised of the external server.
[0063] <Summary of Embodiments> (Item 1) A control system (130) for a mobile body (100) having a drive unit (121) that individually drives a left wheel (120L) and a right wheel (120R), comprising: a trajectory determination means (304) for determining a target trajectory of the mobile body; a target determination means (401) for determining a target left wheel speed for the left wheel and a target right wheel speed for the right wheel, and a target value of a movement parameter relating to the turning of the mobile body, for moving the mobile body along the target trajectory; an acquisition means (402) for acquiring measured values of the movement parameter based on the output from a sensor (137) of the mobile body; a correction means (403, 404) for correcting the target left wheel speed and the target right wheel speed so as to reduce the difference between the target value of the movement parameter and the measured value of the movement parameter; and a control means (405R, 405L) for controlling the drive unit to drive the left wheel and the right wheel at the corrected target left wheel speed and the target right wheel speed. According to this configuration, even if a sensor or drive unit malfunctions, unintended behavior of the moving body can be mitigated by individually correcting the target wheel speed using movement parameters related to the turning of the moving body. (Item 2) The control system according to Item 1, wherein the movement parameters include the difference between the left wheel speed of the left wheel and the right wheel speed of the right wheel. According to this configuration, the target wheel speed is corrected to reduce the difference between the measured value of the wheel speed difference and the target value. (Item 3) The control system according to Item 1 or 2, wherein the movement parameters include the yaw angle of the moving body. According to this configuration, the target wheel speed is corrected to reduce the difference between the measured value of the yaw angle and the target value. (Item 4) The control system according to any one of Items 1 to 3, wherein the drive unit includes a drive mechanism (123) that rotates the left wheel and the right wheel, and the movement parameters include the angle of the drive mechanism with respect to the frame (117) of the moving body. According to this configuration, the target wheel speed is corrected to reduce the difference between the measured value of the unit angle and the target value. (Item 5) The control system according to any one of items 1 to 4, wherein the sensors of the moving body are redundant, and the acquisition means acquires a representative value of the output from the redundant sensors as the measured value of the movement parameter.According to this configuration, even if a sensor fails, unintended behavior of the moving body can be mitigated. (Item 6) The control system further comprises a sensor failure detection means (1001) that detects a failure of the sensor based on the output from the redundant sensors, and the control means controls the drive unit to suppress the movement of the moving body in response to the detection of a sensor failure, as described in Item 5. According to this configuration, the moving body can be transitioned to a safe state if a sensor fails. (Item 7) The control system according to Item 6, where the control means controls the drive unit to suppress the rotation of both the left wheel and the right wheel in response to the detection of a sensor failure. According to this configuration, the moving body can be transitioned to a safe state if a sensor fails. (Item 8) The control system further comprises a drive unit failure detection means (1001) that detects a failure of the drive unit, and the drive unit failure detection means controls the drive unit to suppress the movement of the moving body in response to the detection of a drive unit failure, as described in any one of Items 1 to 7. According to this configuration, the moving body can be transitioned to a safe state if a drive unit fails. (Item 9) The control system according to Item 8, wherein the drive unit includes a left wheel drive unit that drives the left wheel and a right wheel drive unit that drives the right wheel, the drive unit failure detection means detects whether a failure has occurred in the left wheel drive unit or the right wheel drive unit, and the control means controls the drive unit to suppress the rotation of the wheel driven by the drive unit that has a failure detected, in response to the detection of a failure in the drive unit. With this configuration, if a failure occurs in the drive unit, the moving body can be transitioned to a safe state.(Item 10) A method for controlling a mobile body (100) having a drive unit (121) that drives a left wheel (120L) and a right wheel (120R) individually, comprising: determining a target trajectory of the mobile body (S1201); determining a target left wheel speed for the left wheel and a target right wheel speed for the right wheel, and a target value of a movement parameter relating to the turning of the mobile body, for moving the mobile body along the target trajectory (S1202); obtaining a measured value of the movement parameter based on the output from a sensor (137) of the mobile body (S1203); correcting the target left wheel speed and the target right wheel speed so as to reduce the difference between the target value of the movement parameter and the measured value of the movement parameter (S1204); and controlling the drive unit to drive the left wheel and the right wheel at the corrected target left wheel speed and the target right wheel speed (S1205). With this configuration, even if a sensor or drive unit malfunctions, unintended behavior of the moving body can be mitigated by individually correcting the target wheel speed using movement parameters related to the turning of the moving body.
[0064] The invention is not limited to the embodiments described above, and various modifications and changes are possible within the scope of the gist of the invention.
Claims
1. A control system for a mobile body having drive units that individually drive the left wheel and the right wheel, comprising: trajectory determination means for determining a target trajectory of the mobile body; target determination means for determining a target left wheel speed for the left wheel and a target right wheel speed for the right wheel, and a target value for a movement parameter relating to the turning of the mobile body, for moving the mobile body along the target trajectory; acquisition means for acquiring measured values of the movement parameter based on the output from a sensor of the mobile body; correction means for correcting the target left wheel speed and the target right wheel speed so as to reduce the difference between the target value of the movement parameter and the measured value of the movement parameter; and control means for controlling the drive unit to drive the left wheel and the right wheel at the corrected target left wheel speed and the target right wheel speed.
2. The control system according to claim 1, wherein the movement parameter includes the difference between the left wheel speed of the left wheel and the right wheel speed of the right wheel.
3. The control system according to claim 1 or 2, wherein the movement parameter includes the yaw angle of the moving body.
4. The control system according to any one of claims 1 to 3, wherein the drive unit includes a drive mechanism for rotating the left wheel and the right wheel, and the movement parameter includes the angle of the drive mechanism with respect to the frame of the moving body.
5. The control system according to any one of claims 1 to 4, wherein the sensors of the moving body are redundant, and the acquisition means acquires a representative value of the output from the redundant sensors as the measured value of the movement parameter.
6. The control system according to claim 5, further comprising a sensor failure detection means for detecting a failure of a sensor based on the output from the redundant sensors, wherein the control means controls the drive unit to suppress the movement of the moving body in response to the detection of a sensor failure.
7. The control system according to claim 6, wherein the control means controls the drive unit to suppress the rotation of both the left wheel and the right wheel in response to the detection of a malfunction in the sensor.
8. The control system according to any one of claims 1 to 7, further comprising a drive unit failure detection means for detecting a failure in the drive unit, wherein the drive unit failure detection means controls the drive unit to suppress the movement of the moving body in response to the detection of a failure in the drive unit.
9. The control system according to claim 8, wherein the drive unit includes a left wheel drive unit for driving the left wheel and a right wheel drive unit for driving the right wheel, the drive unit failure detection means detects whether a failure has occurred in the left wheel drive unit or the right wheel drive unit, and the control means controls the drive unit in response to the detection of a failure in the drive unit to suppress the rotation of the wheel driven by the drive unit that has a failure, whichever of the left wheel drive unit or the right wheel drive unit has a failure.
10. A method for controlling a mobile body having a drive unit that drives a left wheel and a right wheel individually, comprising: determining a target trajectory of the mobile body; determining a target left wheel speed for the left wheel and a target right wheel speed for the right wheel, and a target value of a movement parameter relating to the turning of the mobile body, for moving the mobile body along the target trajectory; obtaining a measured value of the movement parameter based on the output from a sensor of the mobile body; correcting the target left wheel speed and the target right wheel speed so as to reduce the difference between the target value of the movement parameter and the measured value of the movement parameter; and controlling the drive unit to drive the left wheel and the right wheel at the corrected target left wheel speed and the target right wheel speed.