WO2024034579A1 - 制御装置 - Google Patents

制御装置 Download PDF

Info

Publication number
WO2024034579A1
WO2024034579A1 PCT/JP2023/028799 JP2023028799W WO2024034579A1 WO 2024034579 A1 WO2024034579 A1 WO 2024034579A1 JP 2023028799 W JP2023028799 W JP 2023028799W WO 2024034579 A1 WO2024034579 A1 WO 2024034579A1
Authority
WO
WIPO (PCT)
Prior art keywords
control device
command value
command
limiter
speed
Prior art date
Application number
PCT/JP2023/028799
Other languages
English (en)
French (fr)
Inventor
智史 大月
健一 中島
Original Assignee
川崎重工業株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 川崎重工業株式会社 filed Critical 川崎重工業株式会社
Publication of WO2024034579A1 publication Critical patent/WO2024034579A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B11/00—Automatic controllers
    • G05B11/01—Automatic controllers electric
    • G05B11/36—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/40—Control within particular dimensions
    • G05D1/43—Control of position or course in two dimensions
    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G1/00—Traffic control systems for road vehicles
    • G08G1/16—Anti-collision systems

Definitions

  • the technology disclosed herein relates to a control device.
  • Patent Document 1 discloses a technique that assumes a route connecting a plurality of nodes in a straight line and calculates an optimal route by moving the nodes so as to avoid obstacles.
  • the technology of the present disclosure is directed to a control device that calculates the amount of operation of an actuator for moving a moving body.
  • the control device includes a setting device that sets a command value for moving the movable body to a target position, a limiter that sets a constraint condition for the command value to avoid a failure, and a control device that sets the command value so that the command value does not meet the constraint condition.
  • the control device includes a corrector that corrects the command value to fall within the limit range when the command value is outside the limit range determined by the command value, and a manipulated variable calculator that calculates the manipulated variable based on the command value.
  • control device it is possible to instantly respond to a failure and avoid the failure.
  • FIG. 1 is a schematic diagram of a driving support system.
  • FIG. 2 is a diagram showing a schematic hardware configuration of a mobile object.
  • FIG. 3 is a diagram showing the hardware configuration of the control device.
  • FIG. 4 is a functional block diagram of the controller.
  • FIG. 5 is a detailed functional block diagram of the control calculator.
  • FIG. 6 is an image diagram showing an example of correction of a command value by a corrector.
  • FIG. 7 is an image diagram showing an example of correction of a manipulated variable by a corrector without considering constraints on output characteristics.
  • FIG. 8 is an image diagram showing an example of correction of the manipulated variable by the corrector in consideration of the constraint conditions on the output characteristics.
  • FIG. 1 is a schematic diagram of a driving support system.
  • FIG. 2 is a diagram showing a schematic hardware configuration of a mobile object.
  • FIG. 3 is a diagram showing the hardware configuration of the control device.
  • FIG. 4 is a functional block diagram of the controller.
  • FIG. 5
  • FIG. 9 is an image diagram showing an example of correction of a command value by a corrector when a fixed constraint condition and a variable constraint condition are included.
  • FIG. 10 is an image diagram showing an example of correction of a command value by a corrector when changing constraint conditions when a deadlock occurs.
  • FIG. 11 is an image diagram showing an example of correction of a command value by a corrector when preliminary correction is performed when a deadlock occurs.
  • FIG. 12 is a flowchart of the command value restriction process.
  • FIG. 13 is an explanatory diagram showing a model of a ship.
  • FIG. 14 is an explanatory diagram showing a vehicle model.
  • FIG. 15 is a detailed functional block diagram of a control calculator according to another embodiment.
  • FIG. 1 is a schematic diagram of a driving support system 1000.
  • the control device 100 is mounted on the mobile body 1 and executes automatic operation of the mobile body 1.
  • Mobile object 1 is included in driving support system 1000.
  • Driving support system 1000 may include a plurality of moving bodies 1.
  • the driving support system 1000 includes, in addition to the mobile object 1, an support device 200 that supports automatic driving.
  • the support device 200 is communicably connected to the mobile body 1.
  • the support device 200 transmits map information to the mobile body 1 to support driving of the mobile body 1 .
  • the control device 100 generates a route based on map information and executes automatic driving. Communication between support device 200 and mobile object 1 is performed via relay device 121 .
  • the relay device 121 is installed in the control tower 120, for example.
  • the support device 200 and the relay device 121 are connected via a communication network N such as the Internet. Relay device 121 and mobile object 1 perform wireless communication.
  • Map information includes geographic information.
  • the geographical information includes at least one of roads, passages, routes, and features (objects on land or sea, whether natural or artificial, such as buildings, trees, and rocks).
  • Geographical information may be information related to general maps such as road maps, facility maps, route maps, and the like.
  • the map information may include information regarding obstacles in addition to geographical information.
  • the information regarding the failure includes at least one of the position, speed, and type of the failure.
  • the moving body 1 is, for example, a vehicle.
  • the mobile object 1 may be a ship or an autonomous robot.
  • FIG. 2 is a diagram showing a schematic hardware configuration of the mobile object 1. As shown in FIG.
  • the mobile body 1 includes a monitoring sensor 11, an actuator 12, a communication device 13, a position detector 14, an input device 15, a display device 16, an alarm device 17, and a control device 100.
  • the monitoring sensor 11 acquires monitoring information in a predetermined range within the environment in which the mobile object 1 moves.
  • the monitoring information may include terrain, buildings, features, equipment, moving objects, and the like.
  • the surveillance sensor includes at least one of a camera, a LiDAR (Light Detection And Ranging), an infrared sensor, a laser range finder, and a Doppler LiDAR.
  • the camera captures still images or videos.
  • the laser distance meter may employ a green laser.
  • the monitoring sensor 11 is a LiDAR.
  • the monitoring sensor 11 acquires point cloud data of objects within a predetermined monitoring area as monitoring information.
  • the actuator 12 is a driving source for the moving body 1.
  • the actuator 12 may include a first actuator 12A and a second actuator 12B.
  • the first actuator 12A is an electric motor or an engine
  • the second actuator 12B is a steering device (specifically, an electric motor that adjusts the steering angle).
  • the communication device 13 performs wireless communication with an external device.
  • the position detector 14 detects the position of the moving body 1.
  • the position detector 14 is a GNSS (Global Navigation Satellite System) receiver or an IMU (Inertial Measurement Unit).
  • the input device 15 is a device for a user to input various information.
  • the input device 15 is a touch panel of a car navigation system.
  • the display device 16 is, for example, a car navigation display.
  • the display device 16 may display a map or route generated from map information.
  • the alarm 17 is a buzzer or a lamp. The alarm device 17 emits sound or light in response to a command from the control device 100.
  • FIG. 3 is a diagram showing the hardware configuration of the control device 100.
  • Control device 100 controls the entire mobile body 1 .
  • the control device 100 executes automatic operation of the mobile body 1. That is, the control device 100 operates the actuator 12 to move the movable body 1.
  • the control device 100 includes a controller 18, a storage device 19, and a memory 110.
  • the controller 18 performs various calculation processes.
  • the controller 18 is formed of a processor such as a CPU (Central Processing Unit).
  • the controller 18 may be formed of an MCU (Micro Controller Unit), an MPU (Micro Processor Unit), an FPGA (Field Programmable Gate Array), a PLC (Programmable Logic Controller), a system LSI, or the like.
  • the storage device 19 stores programs executed by the controller 18 and various data.
  • the storage device 19 stores a control program.
  • the storage device 19 is formed of a nonvolatile memory, an HDD (Hard Disc Drive), an SSD (Solid State Drive), or the like.
  • the memory 110 temporarily stores data and the like.
  • memory 110 is formed of volatile memory.
  • the moving body 1 performs autonomous operation by the controller 18 operating the actuator 12.
  • the controller 18 generates a route based on map information.
  • the controller 18 operates the actuator 12 based on the detection results by the monitoring sensor 11 and the position detector 14 so that the moving body 1 moves along the generated route. At this time, the controller 18 operates the actuator 12 to avoid interference between the moving body 1 and the obstacle.
  • FIG. 4 is a functional block diagram of the controller 18.
  • the controller 18 implements various functions by reading programs from the storage device 19 into the memory 110 and developing them. Specifically, the controller 18 functions as a state estimator 111 that estimates the state of the moving body 1 and the state of the environment, and a control calculator 112 that calculates the command operation amount of the actuator 12.
  • Map information, the detection results of the monitoring sensor 11, and the detection results of the position detector 14 are input to the state estimator 111.
  • the state estimator 111 estimates the state of the mobile body 1 using a Kalman filter.
  • the state estimator 111 also estimates the state of a fault in the environment.
  • the state estimator 111 may perform self-position estimation and environmental map generation using SLAM (Simultaneous Localization and Mapping) technology.
  • the state estimator 111 includes map information including the location of the obstacle, information about the moving object such as the current position and speed of the moving object 1, information about the obstacle such as the relative position and speed of the obstacle with respect to the moving object 1, and information about the disturbance. Output information.
  • obstacles include features, buildings, other moving objects other than the moving object 1, construction sites, uneven ground, shallow waters, fish farms, etc.
  • FIG. 5 is a detailed functional block diagram of the control calculator 112.
  • the control calculator 112 includes a route generator 21 that generates a route for the moving object 1, a setting device 22 that sets the commanded speed of the moving object 1, a limiter 25 that sets the limiting conditions for the commanded speed, and a limiter 25 that sets the commanded speed.
  • a corrector 26 that performs correction
  • a servo controller 27 that calculates a command force that causes the actual speed of the moving body 1 to follow the command speed
  • a manipulated variable calculator 28 that calculates the manipulated variable of the actuator 12 based on the command force.
  • a notification controller 29 that notifies the user of correction of the command speed via the display device 16 or the alarm 17.
  • the route generator 21 reads the destination stored in the memory 19.
  • the destination is stored in the storage device 19 in advance.
  • the storage device 19 may also store transit points. In that case, the route generator 21 reads the waypoints from the memory 19. Map information and information regarding the moving object are input to the route generator 21 from the state estimator 111 .
  • the route generator 21 generates a route from the current position of the mobile object 1 to the destination based on map information.
  • the route generator 21 refers to map information and generates a route that does not collide with terrestrial objects. If a road, passage, or sea route is set in the environment, the route generator 21 generates a route along the road, passage, or sea route. For example, the route generator 21 generates a route using the A-star search algorithm or the RRT-star algorithm.
  • the route generator 21 sets a waypoint on the route, that is, a target position relatively close to the current position on the route, and outputs the target position to the command calculator 23 .
  • the setter 22 sets either the command speed calculated based on the route or the command speed input via the input device 15 as the command speed.
  • the setter 22 includes a command calculator 23 that calculates the command speed based on the route generated by the route generator 21, and a command calculator 23 that calculates the command speed from the input device 15 and the command speed from the command calculator 23. It has a switch 24 that switches depending on the command speed.
  • the command speed from the input device 15 is a command speed input by a user's manual operation.
  • the command calculator 23 calculates a command speed for moving the moving body 1 to a target position (ie, a waypoint). For example, the command calculator 23 calculates the command speed using PID control. That is, the command calculator 23 calculates the command speed based on the difference between the current position and the target position of the moving body 1.
  • the command calculator 23 may calculate the command speed using model predictive control (MPC). If the route generated by the route generator 21 is a basic route, then in model predictive control, the command calculator 23 generates a local route included in the basic route, and when moving along the local route. Calculate the command speed of The local route is a route from the current position of the moving body 1 to the target position. The command calculator 23 predicts the future state quantities of the moving body 1 and the obstacle from the current state quantities of the moving body 1 and the obstacle, and determines the optimal route from the current position of the moving body 1 to the target position and the route corresponding thereto. Calculate the command speed. In this case, information regarding the failure is also input to the command calculator 23 from the state estimator 111.
  • MPC model predictive control
  • the switch 24 switches the command speed input to the corrector 26 between the command speed from the input device 15 and the command speed from the command calculator 23. During automatic operation, the switch 24 basically inputs the command speed from the command calculator 23 to the corrector 26. On the other hand, when the command speed is input from the input device 15 by manual operation, the switch 24 inputs the command speed from the input device 15 to the corrector 26 .
  • the limiter 25 sets constraint conditions that limit the command speed. Specifically, the limiter 25 receives information about the moving object, information about the obstacle, and information about the disturbance from the state estimator 111. The limiter 25 sets constraints on the commanded speed for the moving body 1 to avoid obstacles. For example, the limiter 25 sets constraints using a kinematics model of the moving body 1, that is, a kinematics model. Specifically, the limiter 25 uses a kinematic model of the moving object 1 to issue a command to ensure a predetermined distance (hereinafter also referred to as "safety distance") between the moving object 1 and the obstacle. Set speed constraints.
  • the limiter 25 can set multiple constraint conditions. For example, if there are a plurality of obstacles that should be avoided at the same time, the limiter 25 can set a plurality of constraint conditions corresponding to each obstacle. Furthermore, if a plurality of protection parts of the mobile body 1 that should be considered for avoidance at the same time are set, the limiter 25 can set a plurality of constraint conditions corresponding to each protection part. Therefore, when a plurality of failures and a plurality of protection parts are to be considered, the number of constraint conditions can be set according to the combination of failures and protection parts.
  • the limiter 25 sets constraint conditions regarding the output characteristics with respect to the command speed. For example, the limiter 25 sets constraints regarding the control characteristics of the servo controller 27. Therefore, the control state quantity is also input to the limiter 25 from the servo controller 27 . Further, the limiter 25 sets constraints regarding the actuator 12 in addition to avoiding obstacles.
  • the constraint conditions related to the control characteristics and the constraint conditions related to the actuator 12 are examples of the constraint conditions related to the output characteristics.
  • the limiter 25 sequentially calculates the constraint conditions and updates the constraint conditions. In other words, the situation of the mobile object 1 and the obstacle may change over time.
  • the limiter 25 can update the constraint conditions in response to changes over time in the conditions of the mobile object 1 and obstacles.
  • the limiter 25 outputs the constraint conditions to the corrector 26.
  • the limiter 25 may set adjustable constraint conditions. For example, constraints that can be relaxed depending on at least one of the protection part of the mobile body 1 and the type of obstacle may be set to be adjustable. If the command speed is not properly corrected, the limiter 25 changes the adjustable constraints.
  • the corrector 26 compares the command speed with the limit range defined by the constraint conditions to determine the final command speed. Specifically, if the commanded speed is within the limit range, the corrector 26 determines the commanded speed as it is as the final commanded speed. If the command speed is outside the limit range, the corrector 26 corrects the command speed to within the limit range, and sets it as the final command speed.
  • the restricted range does not consist of a finite number of options, for example, only a few options, but has a continuous width, that is, an expansion. In other words, the corrector 26 corrects the command speed to an appropriate speed within a continuously expanding range. For example, the corrector 26 uses quadratic programming or the like to optimize the command speed so that it falls within a limited range.
  • the corrector 26 outputs the final command speed to the servo controller 27.
  • the servo controller 27 receives information regarding the moving object and disturbance information from the state estimator 111.
  • the servo controller 27 calculates a command force for causing the actual speed of the moving body 1 to follow the command speed.
  • the servo controller 27 performs, for example, PID control. Note that the control of the servo controller 27 is not limited to PID control, and may be robust control or the like.
  • the servo controller 27 outputs the command force to the manipulated variable calculator 28 .
  • the operation amount calculator 28 distributes the command force to the plurality of actuators 12 and calculates the command operation amount of each of the plurality of actuators 12. If the actuator 12 is an electric motor, the manipulated variable is, for example, the rotational speed or torque of the electric motor. When the actuator 12 is an engine, the manipulated variables are, for example, an accelerator opening degree and a fuel injection amount.
  • Actuator 12 operates according to the command operation amount.
  • Actuator 12 may be provided with its own controller for operating actuator 12 .
  • the actuator 12 when the actuator 12 is an electric motor, the actuator 12 further includes a servo amplifier. In that case, the servo amplifier operates the electric motor according to the command operation amount. As a result, the moving body 1 exerts thrust and moves toward the target position.
  • the route and target position are input to the notification controller 29 from the route generator 21. Furthermore, the final command speed u and the presence/absence of correction are inputted to the notification controller 29 from the corrector 26 .
  • the notification controller 29 notifies the user via the display device 16 and the alarm 17 that the command speed is corrected.
  • the notification controller 29 causes the display device 16 to display the route, target position, and final command speed u.
  • the display device 16 displays the route and target position superimposed on the map, and further displays the final command speed u as a vector.
  • the display device 16 changes the display form of the final command speed u depending on whether the command speed has been corrected. This allows the user to know the progress status of the moving object 1 and whether or not the commanded speed has been corrected.
  • the display device 16 may display a message indicating that the command speed is being corrected, such as "under correction” or "under restriction.” In this case, the display device 16 does not need to change the display form of the final command speed u.
  • the notification controller 29 outputs a command to the alarm 17 to notify the correction of the commanded speed.
  • the alarm device 17 issues an alarm in response to a command.
  • the alarm device 17 emits sound or light as an alarm. This allows the user to know that the command speed has been corrected.
  • the notification controller 29 may notify the user of the command speed correction via only one of the display device 16 and the alarm 17.
  • the limiter 25 and the corrector 26 limit the command speed using, for example, a control barrier function (CBF). Specifically, the command speed is limited based on the evaluation function J shown in equation (1) below and the constraint condition shown in equation (2).
  • CBF control barrier function
  • u ref is an input command value, that is, a control input. That is, the input command value u ref is a provisional command value before correction.
  • u is the corrected command value.
  • x is the current state quantity of the controlled object (here, the moving body 1), and x j is the current state quantity of the fault.
  • ⁇ is a functional (class ⁇ function), and h i is a barrier function expressed by the following formula (3). i is a number for identifying a constraint condition. For example, h i is the i-th barrier function.
  • the barrier function h i is a function representing a safe state.
  • the barrier function h i is set so that the controlled object is safe when h i ⁇ 0.
  • Equation (2) is an inequality constraint condition for converging the controlled object to a range of h i ⁇ 0.
  • the constraint condition can be transformed into a linear equation (5).
  • the limiter 25 sets a constraint condition as shown in equation (6).
  • the corrector 26 determines a command value u that minimizes the evaluation function J while satisfying the constraint conditions. As a result, the corrector 26 determines a command value u that has the minimum amount of change from the input command value u ref and satisfies the constraint conditions. For example, the corrector 26 determines the command value u using a convex optimization calculation method, such as an interior point method or a quadratic programming method. The corrector 26 corrects the input command value u ref to a command value u, and outputs it as the final command value u. Note that when the input command value u ref is within the limit range, the evaluation function J becomes 0.
  • the corrector 26 determines whether the input command value u ref is within the limit range based on whether the evaluation function J is 0 or not. When the input command value u ref is within the limit range, the corrector 26 outputs the input command value u ref as it is as the final command value u.
  • FIG. 6 is an image diagram showing an example of correction of the command value u by the corrector 26.
  • the command value u is a command speed, and is expressed as a vector (u, v) in a coordinate system consisting of orthogonal X and Y axes.
  • the hatched area is a restricted range that satisfies the constraint conditions.
  • the broken line indicates the constraint R1 and serves as the boundary of the limit range.
  • the corrector 26 determines the final command speed u that is within the limit range and has the minimum amount of change from the input command speed u ref . demand. As can be seen from FIG. 6, among the points on the boundary B, the point closest to the input command speed u ref becomes the final command speed u.
  • the corrector 26 corrects the input command speed u ref to the final command speed u included within the limit range.
  • the moving body 1 to be controlled is maintained safely.
  • Safety is defined by the barrier function h i .
  • the limiter 25 and the corrector 26 can quickly obtain the optimal final command value u. For example, when an obstacle suddenly appears in the vicinity of the moving body 1, or when the obstacle moves rapidly, the limiter 25 and the corrector 26 instantaneously determine the optimal final command value u and 1 can instantly avoid interference with obstacles.
  • constraints other than safety include constraints on control characteristics and constraints on output characteristics, such as constraints on actuator characteristics.
  • the command force output from the servo controller 27 may be subject to constraints on the control characteristics of the servo controller 27.
  • restrictions may be intentionally set on the control characteristics of the servo controller 27, such as setting an upper limit on the command force so that the moving body 1 does not accelerate excessively.
  • the actuator 12 has characteristics such as maximum output. The limiter 25 may consider at least one of these constraints other than safety.
  • the command force output from the servo controller 27 is influenced by control characteristics such as responsiveness and followability of the servo controller 27, that is, is subject to restrictions.
  • the transfer function of the servo controller 27 represents the control characteristics of the servo controller 27.
  • the limiter 25 converts constraints on the control characteristics into constraints on the command speed based on the inverse transfer function of the servo controller 27. Specifically, the state quantity of the servo controller 27 is input to the limiter 25 .
  • the limiter 25 determines an inverse transfer function of the servo controller 27 based on the state quantity of the servo controller 27.
  • the limiter 25 uses an inverse transfer function to find constraints on control characteristics for the command speed.
  • the range of thrust that the actuator 12 can exert as a whole, that is, the limit is determined according to actuator characteristics such as the arrangement of the actuator 12, the output upper limit and output lower limit, and the distribution logic of the manipulated variable calculator 28. Therefore, the limit of the thrust that the actuator 12 can exert is determined in advance based on the actuator characteristics such as the arrangement of the actuator 12, and is stored in the memory 19.
  • the limit of the thrust by the actuator 12 can be regarded as a restriction on the command force output from the servo controller 27 to the manipulated variable calculator 28.
  • the constraints on the command force can be converted into constraints on the command speed using the inverse transfer function of the servo controller 27.
  • the limit of the thrust by the actuator 12 can be converted into a constraint on the command speed.
  • the limiter 25 uses the inverse transfer function of the servo controller 27 from the thrust limit of the actuator 12 stored in the memory 19 to determine a constraint on the actuator characteristics for the command speed.
  • FIG. 7 is an image diagram showing an example of correction of the command value u by the corrector 26 without considering constraints on output characteristics.
  • FIG. 8 is an image diagram showing an example of correction of the command value u by the corrector 26 in consideration of the constraint conditions on the output characteristics.
  • the hatched area is the restricted range, and the broken line indicates the restrictive condition R1 and serves as the boundary of the restricted range.
  • the dashed line indicates a command value constraint corresponding to a constraint on the output characteristics.
  • the hatched area is the restricted range, and the broken lines indicate the restrictive conditions R1 and R2, and are the boundaries of the restricted range.
  • the constraint R1 is a safety constraint
  • the constraint R2 is a constraint on output characteristics.
  • the corrector 26 corrects the input command speed u ref based only on the safety constraint R1.
  • the final command speed u is located on the constraint condition R1, that is, on the boundary of the limit condition.
  • the servo controller 27 calculates the command force based on the final command speed u
  • the operation amount calculator 28 calculates the command operation amount of each of the first actuator 12A and the second actuator 12B based on the command force. do.
  • the command force is influenced by the control characteristics of the servo controller 27.
  • the overall thrust by the actuator 12 is influenced by the actuator characteristics of the first actuator 12A and the second actuator 12B.
  • the thrust given to the moving body 1 is not a thrust that appropriately corresponds to the final command speed u, but a thrust that is subject to constraints on control characteristics and actuator characteristics.
  • a restriction on thrust is converted into a restriction on command speed u, a chain line in FIG. 7 is obtained.
  • a thrust equal to the commanded speed u further corrected so as to satisfy the constraint indicated by the dash-dotted line will be exerted.
  • the actual commanded speed u' is farther inside the limit range than the constraint condition R1. This means driving at a safer operating point, and can also be considered to be driving with the commanded speed u more limited than necessary.
  • the limiter 25 sets both safety constraints R1 and output characteristics constraints R2, as shown in FIG.
  • the corrector 26 corrects the input command speed u ref to within the limit range determined by the constraint conditions R1 and R2.
  • the corrector 26 corrects the input command speed u ref to the point closest to the input command speed u ref on the boundary of the limit range.
  • the input command speed u ref is corrected at the intersection of constraint condition R1 and constraint condition R2.
  • the limiter 25 is adjustable, ie, can set variable constraints.
  • obstacles include obstacles that are passable but preferably avoided, such as uneven ground or areas with strong air currents.
  • the limiter 25 may switch whether the constraint condition is variable or fixed, depending on the type of failure that is the target of the constraint condition.
  • the restrictor 25 may have adjustable or relaxed constraints. For example, when the constraint conditions are severe, there may be cases where there is no command value u included within the limit range, or there may be a case where the command value u included within the limit range is extremely small. As a result, it becomes difficult to move the moving body 1 appropriately. In such a case, the limiter 25 changes the variable constraint condition to relax it and expand the limit range. As a result, it becomes possible to set the command value u included within the limit range, or there is a possibility that the command value u included within the limit range increases.
  • the limiter 25 when setting constraints so that a safe distance is ensured between the protected part and the obstacle, the limiter 25 sets the constraints so that they can be adjusted, that is, relaxed, so that the safe distance is reduced. You may.
  • the safety distance is set by adding an allowance to the required minimum distance so that interference between the protected part and an obstacle can be reliably avoided.
  • the limiter 25 may be able to adjust the constraint condition within a margin of the safety distance. For example, when a plurality of protection parts are set for the mobile body 1 and protection priorities are set for the plurality of protection parts, the limiter 25 sets the constraint condition corresponding to the protection part with a relatively lower priority. may be made variable.
  • the restrictor 25 sets the constraint condition corresponding to the first protected area. It may be fixed, and the constraint conditions corresponding to the second protected region may be variable. Alternatively, the limiter 25 may make the constraint conditions corresponding to the protected parts variable and gradually relax them in descending priority order until an appropriate command value u is set.
  • the limiter 25 introduces a slack variable ⁇ i as shown in equation (7) for variable constraint conditions.
  • the limiter 25 outputs the constraint condition including the slack variable to the corrector 26.
  • the corrector 26 sets the evaluation function J as shown in equation (8) and adjusts the weight of the slack variable ⁇ i . Since the fixed constraint condition does not include the slack variable ⁇ i , the corrector 26 sets the weight of the slack variable ⁇ i of the fixed constraint condition to 0 in the evaluation function J. The limiter 25 and the corrector 26 can thus adjust or expand the limit range.
  • FIG. 9 is an image diagram showing an example of correction of the command value u by the corrector 26 in a case where fixed constraint conditions and variable constraint conditions are included.
  • the plurality of constraints include a fixed first constraint R1 and a variable second constraint R2 .
  • the limiter 25 can change the second constraint condition R 2 by adjusting the slack variable ⁇ 2 and, as a result, change the restriction range.
  • the limiter 25 may correct the input command value u ref again by adjusting variable constraint conditions.
  • the corrected command value u may change compared to before the constraint condition is changed. Thereby, the corrected command value u may be improved.
  • the deadlock countermeasures As a result of the input command speed u ref being corrected within the limit range, the final command speed u may become approximately zero. In other words, the moving body 1 substantially stops. This state is called deadlock. If deadlock occurs, the deadlock is resolved by changing the constraint conditions or changing the command value u before correction.
  • the corrector 26 causes the limiter 25 to change the variable constraint condition. For example, if there is a possibility of deadlock, the corrector 26 notifies the limiter 25 of this fact.
  • the limiter 25 changes the variable constraints and outputs the changed constraints to the corrector 26. Since the restriction range changes due to a change in the constraint conditions, the corrected command value u may deviate from 0, that is, it may take a value other than 0.
  • FIG. 10 is an image diagram showing an example of correction of the command value u by the corrector 26 when changing the constraint conditions when a deadlock occurs.
  • the corrector 26 causes the limiter 25 to change the constraint conditions.
  • the limiter 25 changes the variable constraint conditions.
  • the limiter 25 changes the second constraint R2 .
  • the corrector 26 corrects the input command speed u ref to within the limit range after changing the second constraint condition R 2 . Since the limit range is changed, the final command speed u deviates from 0 and takes on a value other than 0. As a result, the mobile body 1 can move without falling into a deadlock.
  • the corrector 26 pre-corrects the input command value u ref and then corrects it to within the limit range.
  • the corrector 26 preliminarily corrects the input command value u ref by a predetermined amount.
  • the predetermined amount of preliminary correction may be changed each time preliminary correction is performed. Alternatively, the predetermined amount may be constant.
  • Preliminary correction of the input command value u ref changes the relationship between the input command value u ref and the limit range, so the command value u within the limit range where the amount of correction is minimum from the input command value u ref may also change. .
  • the corrected command value u may deviate from 0, that is, it may take a value other than 0.
  • FIG. 11 is an image diagram showing an example of correction of the command value u by the corrector 26 when preliminary correction is performed when a deadlock occurs.
  • the corrector 26 when there is no preliminary correction, when the corrector 26 corrects the input command speed u ref to within the limit range, the final command speed u becomes 0, leading to a deadlock (see the two-dot chain line).
  • the corrector 26 first pre-corrects the input command speed u ref .
  • the corrector 26 corrects the pre-corrected input command speed u ref ' to within a limited range.
  • the final command speed u deviates from 0 and takes a value other than 0. As a result, the mobile body 1 can move without falling into a deadlock.
  • the corrector 26 may wait in the deadlock state. Over time, the condition of the fault may change and the constraints may change accordingly. In that case, the deadlock will resolve itself. For example, if a deadlock occurs due to constraints corresponding to other moving objects, when the other moving objects move, the constraint conditions change and the possibility of interference with the other moving objects is reduced. obtain. In that case, the limit range is expanded and the deadlock is resolved. Alternatively, if the deadlock is not resolved even after taking measures such as changing the constraint conditions, preliminary correction of the input command value u ref , and waiting as described above, the route generator 21 generates the route again. It's okay.
  • FIG. 12 is a flowchart of the command value restriction process.
  • FIG. 12 is an example of automatic operation in which the command speed is automatically set.
  • the control device 100 generates a route in step S101. Specifically, the route generator 21 generates a route from the current position of the mobile body 1 to the destination via intermediate points based on map information, information regarding the mobile body, and the like. Furthermore, the route generator 21 determines a waypoint on the route, that is, the nearest target position on the route from the current position.
  • control device 100 calculates the input command speed u ref in step S102.
  • the command calculator 23 calculates an input command speed u ref for moving the moving body 1 to the target position.
  • step S103 the control device 100 sets a constraint condition for the command speed u.
  • the limiter 25 calculates the constraint condition based on information regarding the moving object and information regarding the obstacle.
  • the limiter 25 also sets constraints on the output characteristics.
  • step S104 the control device 100 determines whether the input command speed u ref is within the limit range.
  • the corrector 26 uses the evaluation function J and the constraint conditions to determine whether or not the evaluation function J becomes 0 for the input command speed u ref .
  • the corrector 26 determines that the input command speed u ref is within the limit range.
  • the control device 100 determines the input command speed u ref as the final command speed u in step S105. If the input command speed u ref is outside the limit range, the control device 100 corrects the input command speed u ref to be within the limit range in step S107. Specifically, the corrector 26 corrects the input command speed u ref to within the limit range using the evaluation function J and the constraint conditions as described above.
  • step S108 the control device 100 determines whether a deadlock occurs due to the correction of the input command speed u ref . Specifically, the corrector 26 determines whether the corrected command speed u is less than a predetermined threshold. The threshold value is a small value close to 0. If the corrected command speed u does not become less than the threshold value, the control device 100 determines the corrected command speed u as the final command speed u in step S105.
  • the limiter 25 determines whether the constraint condition can be relaxed in step S109. For example, the restrictor 25 determines whether the constraint conditions can be relaxed depending on the type of protection site or failure.
  • step S110 If the constraint condition can be relaxed, the corrector 26 relaxes the constraint condition in step S110. Thereafter, in step S107, the corrector 26 corrects the input command speed u ref to within the limit range, and determines deadlock in step S108. Since the limit range has been changed by changing the constraint conditions, the input command speed u ref can be corrected to a final command speed u that is different from the previous correction. As a result, deadlocks often do not occur.
  • step S111 the corrector 26 pre-corrects the input command speed u ref in step S111. Thereafter, in step S107, the corrector 26 corrects the pre-corrected input command speed u ref ' to within the limit range, and determines deadlock in step S108. Since the input command speed u ref has been pre-corrected, it can be corrected to a final command speed u that is different from the previous correction. As a result, deadlocks often do not occur.
  • step S110 or S111 is executed until the deadlock is resolved.
  • the final command speed u is eventually corrected to a value larger than the threshold value.
  • the control device 100 operates the actuator 12 according to the final command speed u to move the moving body 1 in step S106.
  • the servo controller 27 calculates the command force according to the final command speed u
  • the operation amount calculator 28 calculates the command operation amount of each of the plurality of actuators 12 based on the command force.
  • the plurality of actuators 12 operate based on the respective command operation amounts. As a result, the moving body 1 moves toward the target position.
  • the control device 100 specifically, the notification controller 29 causes the display device 16 to display the generated route.
  • the control device 100 causes the display device 16 to display the final command speed u in the form of a vector or the like.
  • the control device 100 displays the input command speed u ref via at least one of the display device 16 and the alarm 17. Notify the user of the ref correction.
  • the control device 100 notifies specific correction contents such as the presence or absence of a deadlock, relaxation of restriction conditions, or preliminary correction of the input command speed u ref via at least one of the display device 16 and the alarm 17. It's okay.
  • the moving body 1 moves along the route. At this time, if there is a possibility of interference between the moving body 1 and an obstacle, the command speed u is instantly limited to ensure safety.
  • step S103 when the user inputs the input command speed u ref via the input device 15, manual input via the input device 15 is performed in place of steps S101 and S102.
  • step S103 it is determined whether the input command speed u ref input manually is within the limit range (step S104). , if the input command speed u ref is outside the limit range, the input command speed u ref is corrected to fall within the limit range (step S105).
  • step S105 the same applies to other processes. In other words, even with manual input, if there is a possibility of interference between the moving body 1 and an obstacle, the command speed u is instantly limited, and safety is ensured.
  • FIG. 13 is an explanatory diagram showing a model of a ship.
  • the moving body 1 is a ship.
  • k is a number for identifying a part in the mobile body 1.
  • FIG. 14 is an explanatory diagram showing a vehicle model.
  • the moving body 1 is a vehicle including a towing vehicle 1A and a towed vehicle 1B.
  • the towed vehicle 1B is swingably connected to the tow vehicle 1A.
  • U is the forward speed given to the towing vehicle 1A
  • ⁇ is the yaw rate of the towing vehicle 1A
  • ⁇ is the angle of the towed vehicle 1B in the map coordinate system
  • ⁇ is the relative angle of the towed vehicle 1A with respect to the towed vehicle 1B.
  • (x t , y t ) are the position coordinates of the protected part of the towed vehicle 1B in the map coordinate system.
  • L 1 is the distance from the connection point of the towed vehicle 1B to the protected area in the towing vehicle 1A.
  • L 2 is the distance of the towed vehicle 1B from the protected part of the towed vehicle 1B to the connection point with the towed vehicle 1A.
  • M is the distance between the connection point of the towing vehicle 1A and the connection point of the towed vehicle 1B.
  • the limiter 25 models the moving body 1 using such a kinematics model, that is, a kinematics model, and sets constraint conditions.
  • the limiter 25 determines that the distance between the protected part of the mobile body 1 and the obstacle is a safe distance (i.e., the necessary minimum distance plus an allowance).
  • the constraint condition is set to be larger than the added distance).
  • the constraint condition for maintaining a safe distance between the bow and the quay is set as shown in the following equation.
  • Equation (12) the right side of equation (12) is 2 x (unit vector in the quay direction) x (Jacobian of own ship kinematics) x (relative speed between own ship and quay).
  • the corrector 26 can calculate the command speed u for moving the mobile object 1 closer to the quay while maintaining a safe distance between the bow and the quay.
  • the moving body 1 reaches its destination, ie, the position where it docks.
  • a distance equal to the required minimum distance plus an allowance is secured between the bow and the quay.
  • the restrictor 25 sets the constraint conditions for the other vessel, for example, the constraint condition between the bow and the other vessel, and the constraint condition between the stern and the other vessel. By setting , it is possible to ensure a safe distance between the moving body 1 and other ships.
  • the restrictor 25 may change the constraint conditions according to the movement status of the mobile object 1. For example, in the above-mentioned example, when the distance between the bow and the quay becomes approximately equal to the safe distance, the moving body 1 does not approach the quay any further. Therefore, the limiter 25 relaxes the constraint conditions. For example, the restrictor 25 reduces at least one of the protected area radius r k and the quay margin ⁇ s under the constraint condition. In other words, at least one of the required minimum distance and the margin decreases. As a result, the moving body 1 can get closer to the quay. The restrictor 25 can allow the moving object 1 to reach the destination by relaxing the constraint conditions until the moving object 1 reaches the destination.
  • the corrector 26 may change the weighting of the corresponding slack variable ⁇ i in the evaluation function J including the above-mentioned variable constraints. For example, this can also be achieved by decreasing it.
  • constraint conditions for command values to avoid failures are set, and when the command value deviates from the limit range defined by the constraint conditions, the command value falls within the limit range. Corrected. In other words, the command value is limited so that it satisfies the constraint conditions. Thereby, the moving body 1 moves to the target position while avoiding obstacles. In this way, since obstacles are avoided by limiting the command value, there is no need to regenerate the target position or route. The amount of calculation required to limit the command value is smaller than that required to recalculate the target position or route. In other words, the command value can be quickly limited, and failures can be responded to instantly.
  • the options for the command value to be corrected are wide. That is, unlike a response such as simply stopping the moving body 1 or uniformly changing the direction of the moving body 1 in a fixed direction (for example, to the left), the moving body 1 can take a flexible response. In other words, the moving body 1 can continue moving toward the target position even though the command value is limited.
  • the constraint conditions are updated sequentially, the constraint conditions are set in accordance with the ever-changing moving body 1 and its surrounding environment. That is, the command value is not always uniformly limited, but is limited by constraint conditions according to changes in the moving body 1 and its surrounding environment.
  • control device 100 limits the command speed as a command value. Therefore, the control device 100 can realize the computation of the constraint conditions and the computation for correcting the command speed to within the limit range using the kinematics model of the moving body 1, that is, the kinematics model. This simplifies the calculation formula and facilitates the user's design compared to the case where the calculation is performed using the dynamics model of the moving body 1.
  • control device 100 limits the command value at a stage before the servo controller 27, the influence of disturbance and model error can be reduced by servo control. For example, if the command value is limited at the subsequent stage of the servo controller 27, the command value that has been adjusted to reduce the influence of disturbance etc. will be changed. As a result, the effect of reducing the influence of disturbance and model error due to servo control becomes small. By limiting the command value at a stage before the servo controller 27, it is possible to appropriately reduce the influence of disturbances and model errors while ensuring safety.
  • control device 100 can adjust the constraint conditions so as to realize appropriate movement of the mobile body 1. For example, if smooth movement of the mobile body 1 cannot be realized, the movement of the mobile body 1 may be made smooth by relaxing the constraint conditions.
  • control device 100 can resolve the deadlock by changing the constraint conditions or preliminary correction of the input command value.
  • the control device 100 when the command value is outside the limited range, the control device 100 notifies the user of this fact via at least one of the display device 16 and the alarm 17. This allows the user to know that the failure is being avoided. Further, the control device 100 causes the display device 16 to display at least one of the route and the command value, regardless of whether the command value is outside the limit range. Thereby, the user can know at least one of the route of the mobile object 1 and the command value, regardless of whether or not the user is avoiding the obstacle.
  • the mobile object 1 in the driving support system 1000 is not limited to the above-mentioned example.
  • the moving object 1 is not limited to a vehicle or a ship, and any moving object can be employed as long as it is capable of autonomous driving.
  • the mobile object 1 may be an autonomous robot (for example, a carrier pallet), a flying device (for example, a drone), or the like.
  • the number of moving objects 1 included in the driving support system 1000 is not limited to the example in FIG. 1, and is arbitrary.
  • the actuator 12 is an actuator related to the movement of the moving body 1.
  • the actuator 12 is not limited to an electric motor, an engine, and a steering device.
  • the actuators include propellers, thrusters, and the like.
  • the propeller is the actuator.
  • the object restricted by the constraint conditions is not limited to the commanded speed. Any command value is subject to restriction as long as it is a command value at a stage before the actuator 12. That is, in the above description, "command speed” can be read as "command value.”
  • the command force may be limited.
  • the detailed functional block of the control calculator 212 is as shown in FIG. In the control calculator 212 of FIG. 15, the command speed from the setting device 22 is directly input to the servo controller 27. A command force output from the servo controller 27 is input to the corrector 226. The corrector 226 corrects the command force within a limit range determined by the constraint conditions from the limiter 225.
  • the corrector 226 also functions as a manipulated variable calculator, and also calculates the command manipulated variable of the actuator 12.
  • the limiter 225 uses a dynamic model of the moving object 1, that is, a dynamics model, and uses a force command to set constraint conditions for the command force so that a predetermined distance is secured between the moving object 1 and the obstacle. Set. For example, when the influence of disturbance and model error are small, the command value may be limited in the latter stage of servo control in this way.
  • the limiter 25 sets constraints on the control characteristics and constraints on the actuator characteristics, but is not limited thereto. Constraints other than safety constraints are not mandatory.
  • step S111 preliminary correction of the command value
  • step S109 and S110 the constraint conditions
  • steps S108, S109, S110, and S111 after deadlock determination may be omitted. That is, after the command value is corrected in step S107, the command value is determined to be the corrected command value (step S105), and the moving body 1 may be driven (step S106). If a deadlock occurs at this time, the moving body 1 may stop. However, over time, the surrounding environment including the failure changes, and the deadlock may resolve itself.
  • the functions achieved by the components described herein include general-purpose processors, special-purpose processors, integrated circuits, and ASICs (Application Specific Integrated Circuits) programmed to perform the described functions. It may be implemented in circuitry or processing circuitry, including a Central Processing Unit (CPU), conventional circuitry, and/or combinations thereof.
  • a processor includes transistors and other circuits and is considered a circuit or an operational circuit.
  • the processor may be a programmed processor that executes a program stored in memory.
  • a circuit, unit or means is hardware that is programmed to perform or performs the functions described.
  • the hardware may be any hardware disclosed herein or programmed to perform or known to perform the described functions. good.
  • the circuit, means or unit is a combination of hardware and software used to configure the hardware and/or processor. be.
  • the control device 100 that calculates the operation amount of the actuator 12 for moving the movable body 1 includes a setting device 22 that sets a command value for moving the movable body 1 to a target position, and a setting device 22 that operates to avoid obstacles. a limiter 25 that sets constraint conditions for the command value for the control; a corrector 26 that corrects the command value to within the limit range when the command value is outside the limit range defined by the constraint; It also includes a manipulated variable calculator 28 that calculates the manipulated variable based on the command value.
  • the command value for moving the moving body 1 to the target position is provisionally set.
  • constraint conditions are set to limit the command value so as to avoid failures.
  • the constraint conditions define the limit range of the command value. If the provisional command value is outside the limit range, the command value is corrected to fall within the limit range.
  • the moving object moves toward the target position while avoiding obstacles.
  • the control device 100 does not need to change the target position or change the route to avoid the obstacle. In other words, the control device 100 only limits the command value and does not need to perform calculations to change the target position or route. As a result, the calculation load on the control device 100 is reduced, so the command value can be quickly limited and a failure can be instantaneously responded to.
  • the limiter 25 sequentially calculates the constraint conditions to update the constraint conditions.
  • the constraint conditions are not always constant, but are updated in response to the ever-changing moving body 1 and its surrounding environment.
  • a restricted range is set according to the situation, and the safety of the moving body 1 is appropriately ensured.
  • the control device 100 further includes a servo controller 27 that executes servo control based on the commanded speed of the moving body 1 as the command value, and the corrector 26 corrects the command speed to within the limit range and outputs a final command speed to the servo controller 27, and the servo controller 27 outputs a command force based on the final command speed.
  • the operation amount calculator 28 calculates the operation amount based on the command force.
  • the command value is limited at the stage before the servo controller 27, so the command force adjusted by the servo control by the servo controller 27 is output without being restricted.
  • a command force that reduces the influence of disturbance or model error is output. Since the operation amount is calculated based on this command force, the movable body 1 can be moved stably and accurately.
  • the limiter 25 sets constraints regarding the control characteristics of the servo controller 27 in addition to avoiding obstacles.
  • the servo controller 27 has unique control characteristics and does not output the expected command force for all command speeds. Due to restrictions on the control characteristics of the servo controller 27, there may be cases where the commanded force is not output as per the commanded speed.
  • the command speed can be corrected to a range that is not easily influenced by the control characteristics while ensuring safety. As a result, a command force as expected from the command speed is output, and as a result, a manipulated variable that appropriately corresponds to the command speed is calculated.
  • the limiter 25 sets constraint conditions regarding the actuator 12 in addition to avoiding obstacles.
  • an appropriate thrust force can be exerted in consideration of the restrictions on the characteristics of the actuator 12.
  • the actuator 12 has unique actuator characteristics, and does not exert the expected thrust in response to all command forces.
  • the thrust force as per the command speed may not be output.
  • the command speed can be corrected to a range that is not easily affected by the actuator characteristics while ensuring safety. As a result, a thrust force that appropriately corresponds to the commanded speed is exerted.
  • the limiter 25 sets the constraint so that a predetermined distance is secured between the mobile object 1 and the obstacle. Set conditions.
  • the moving body 1 can move toward the target position while ensuring a predetermined distance between the moving body 1 and the obstacle.
  • the restrictor 25 corresponds to the plurality of protected parts of the mobile body 1 so that the plurality of protected parts avoids obstacles.
  • a plurality of the constraint conditions are set.
  • the safety of the plurality of protected parts can be ensured when the moving body 1 moves toward the target position.
  • the limiter 25 sets the constraint condition in an adjustable manner.
  • the limiter 25 can flexibly change the constraint conditions. For example, as a result of correcting the command value to within the limited range, there may be cases where appropriate movement of the mobile body 1 cannot be realized. In such a case, the moving body 1 can be moved appropriately by adjusting the constraint conditions and changing the limit range.
  • the constraint condition when the corrected command value becomes too small to realize appropriate movement of the moving body 1, the constraint condition is changed.
  • the restriction range is changed, and the corrected command value may also change and become equal to or greater than the threshold value. That is, by changing the constraint conditions, a situation where the corrected command value is less than the threshold value can be resolved.
  • the corrector 26 uses the command value as a reserve.
  • the pre-corrected command value is corrected to fall within the limited range.
  • the command value when the corrected command value becomes too small to realize appropriate movement of the moving body 1, the command value is preliminarily corrected.
  • the amount of correction for correcting the command value to within the limit range also changes, so there is a possibility that the command value after correction will exceed the threshold value. That is, by pre-correcting the command value, a situation where the corrected command value is less than the threshold value can be resolved.
  • the setting device 22 sets a command speed as the command value
  • the limiter 25 sets a command speed of the moving body 1.
  • the limit range is calculated based on a kinematic model.
  • the moving body 1 is modeled using so-called kinematics. This simplifies the calculation formula and facilitates design by the user.
  • control device 100 In the control device 100 according to any one of [1] to [11], a notification for notifying the user of the correction of the command value via the display device 16 or alarm device 17 provided in the mobile body 1. It further includes a controller 29.
  • the user can know that the command value has been corrected.Thereby, the user can accurately know the operating status of the mobile body 1. For example, when the user executes an override during automatic operation of the mobile body 1, it is possible to refer to whether or not the command value is corrected.

Landscapes

  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Feedback Control In General (AREA)
  • Traffic Control Systems (AREA)

Abstract

移動体１を移動させるためのアクチュエータ１２の操作量を演算する制御装置１００は、移動体１を目標位置へ移動させるための指令値を設定する設定器２２と、障害を回避するための指令値の制約条件を設定する制限器２５と、指令値が制約条件によって定められる制限範囲外である場合に、指令値を制限範囲内へ補正する補正器２６と、指令値に基づいて操作量を演算する操作量演算器２８とを備える。

Description

制御装置
　ここに開示された技術は、制御装置に関する。
　従来より、移動体の経路を生成する技術に関し、移動体と障害との衝突を回避するように経路を生成して自動運転を行う技術が知られている。例えば、特許文献１には、複数のノードを直線で連結した経路を想定し、障害を回避するようにノードを移動させることによって最適な経路を演算する技術が開示されている。
米国特許第１１１６１５０１号明細書
　ところで、前述のように障害を回避する自動運転においては、障害に衝突しない将来の目標位置及び最適な時系列操作量が逐次演算される。そのような演算は複雑であり、演算負荷が大きい。そのため、瞬時の演算が難しく、急に出現した障害又は動きが急峻な障害等に瞬時に対応することが困難である。
　ここに開示された技術は、かかる点に鑑みてなされたものであり、その目的とするところは、瞬時に障害に対応して、障害を回避することにある。
　本開示の技術は、移動体を移動させるためのアクチュエータの操作量を演算する制御装置が対象である。制御装置は、前記移動体を目標位置へ移動させるための指令値を設定する設定器と、障害を回避するための前記指令値の制約条件を設定する制限器と、前記指令値が前記制約条件によって定められる制限範囲外である場合に、前記指令値を前記制限範囲内へ補正する補正器と、前記指令値に基づいて前記操作量を演算する操作量演算器とを備える。
　前記制御装置によれば、瞬時に障害に対応して、障害を回避することができる。
図１は、運転支援システムの概要図である。 図２は、移動体の概略的なハードウェア構成を示す図である。 図３は、制御装置のハードウェア構成を示す図である。 図４は、制御器の機能ブロック図である。 図５は、制御演算器の詳細な機能ブロック図である。 図６は、補正器による指令値の補正の一例を示すイメージ図である。 図７は、出力特性上の制約条件を考慮しない、補正器による操作量の補正の一例を示すイメージ図である。 図８は、出力特性上の制約条件を考慮した、補正器による操作量の補正の一例を示すイメージ図である。 図９は、固定の制約条件と可変の制約条件とを含む場合の補正器による指令値の補正の一例を示すイメージ図である。 図１０は、デッドロック発生時に制約条件を変更する場合の補正器による指令値の補正の一例を示すイメージ図である。 図１１は、デッドロック発生時に予備補正を行う場合の補正器による指令値の補正の一例を示すイメージ図である。 図１２は、指令値の制限処理のフローチャートである。 図１３は、船舶のモデルを示す説明図である。 図１４は、車両のモデルを示す説明図である。 図１５は、その他の実施形態に係る制御演算器の詳細な機能ブロック図である。
　以下、例示的な実施形態を図面に基づいて詳細に説明する。図１は、運転支援システム１０００の概要図である。制御装置１００は、移動体１に搭載され、移動体１の自動運転を実行する。移動体１は、運転支援システム１０００に含まれる。運転支援システム１０００は、複数の移動体１を含んでいてもよい。運転支援システム１０００は、移動体１の他に、自動運転を支援する支援装置２００を備える。支援装置２００は、移動体１と通信可能に接続されている。支援装置２００は、移動体１の運転を支援するための地図情報を移動体１へ送信する。制御装置１００は、地図情報に基づいて経路を生成し、自動運転を実行する。支援装置２００と移動体１との通信は、中継装置１２１を介して行われる。中継装置１２１は、例えば、管制塔１２０に設置されている。支援装置２００と中継装置１２１とは、インターネット等の通信ネットワークＮを介して接続されている。中継装置１２１と移動体１とは、無線通信を行う。
　地図情報には、地理的情報が含まれる。地理的情報は、道路、通路、航路及び地物（建物、樹木及び岩石など、自然及び人工に関わらず地上又は海上にある物）の少なくとも１つを含む。地理的情報は、道路図、施設図、航路図等の一般的な地図に関する情報であり得る。地図情報は、地理的情報の他に、障害に関する情報を含んでいてもよい。障害に関する情報は、障害の位置、速度及び種別の少なくとも１つを含む。
　－移動体－
　移動体１は、例えば、車両である。移動体１は、船舶又は自律ロボットであってもよい。図２は、移動体１の概略的なハードウェア構成を示す図である。移動体１は、監視センサ１１とアクチュエータ１２と通信器１３と位置検出器１４と入力装置１５と表示装置１６と警報器１７と制御装置１００とを有している。
　監視センサ１１は、移動体１が移動する環境内において所定範囲の監視情報を取得する。監視情報には、地形、建物、地物、設備又は移動体等が含まれ得る。監視センサは、カメラ、ＬｉＤＡＲ（Light Detection And Ranging）、赤外線センサ、レーザ距離計及びドップラＬｉＤＡＲの少なくとも１つを含む。例えば、カメラは、静止画又は動画を撮影する。レーザ距離計は、グリーンレーザを採用していてもよい。この例では、監視センサ１１は、ＬｉＤＡＲである。監視センサ１１は、所定の監視領域内の物体の点群データを監視情報として取得する。
　アクチュエータ１２は、移動体１の駆動源である。アクチュエータ１２は、第１アクチュエータ１２Ａと第２アクチュエータ１２Ｂとを含んでいてもよい。例えば、移動体１が車両の場合、第１アクチュエータ１２Ａは、電動モータ又はエンジンであり、第２アクチュエータ１２Ｂは、ステアリング装置（詳しくは、操舵角を調節する電動モータ）である。
　通信器１３は、外部装置と無線通信を行う。位置検出器１４は、移動体１の位置を検出する。例えば、位置検出器１４は、ＧＮＳＳ（Global Navigation Satellite System）受信機又はＩＭＵ（慣性計測ユニット：Inertial Measurement Unit）である。入力装置１５は、ユーザが各種情報を入力するための装置である。例えば、移動体１が車両である場合、入力装置１５は、カーナビのタッチパネルである。移動体１が車両である場合、表示装置１６は、例えばカーナビのディスプレイである。表示装置１６は、地図情報から生成される地図又は経路を表示してもよい。警報器１７は、ブザー又はランプである。警報器１７は、制御装置１００からの指令に応じて、音又は光を発する。
　図３は、制御装置１００のハードウェア構成を示す図である。制御装置１００は、移動体１の全体を制御する。制御装置１００は、移動体１の自動運転を実行する。つまり、制御装置１００は、アクチュエータ１２を動作させて移動体１を移動させる。制御装置１００は、制御器１８と記憶器１９とメモリ１１０とを有している。
　制御器１８は、各種の演算処理を行う。例えば、制御器１８は、ＣＰＵ（Central Processing Unit）等のプロセッサで形成されている。制御器１８は、ＭＣＵ（Micro Controller Unit）、ＭＰＵ（Micro Processor Unit）、ＦＰＧＡ（Field Programmable Gate Array）、ＰＬＣ（Programmable Logic Controller）、システムＬＳＩ等で形成されていてもよい。
　記憶器１９は、制御器１８で実行されるプログラム及び各種データを格納している。例えば、記憶器１９は、制御プログラムを格納している。記憶器１９は、不揮発性メモリ、ＨＤＤ（Hard Disc Drive）又はＳＳＤ（Solid State Drive）等で形成される。メモリ１１０は、データ等を一時的に格納する。例えば、メモリ１１０は、揮発性メモリで形成される。
　制御器１８がアクチュエータ１２を操作することによって、移動体１は自律運転を行う。制御器１８は、地図情報に基づいて経路を生成する。制御器１８は、生成された経路に沿って移動体１が移動するように、監視センサ１１及び位置検出器１４による検出結果に基づいてアクチュエータ１２を操作する。このとき、制御器１８は、移動体１と障害との干渉を回避するようにアクチュエータ１２を操作する。
　図４は、制御器１８の機能ブロック図である。制御器１８は、記憶器１９からプログラムをメモリ１１０に読み出して展開することによって、各種機能を実現する。具体的には、制御器１８は、移動体１の状態及び環境の状態を推定する状態推定器１１１と、アクチュエータ１２の指令操作量を演算する制御演算器１１２として機能する。
　状態推定器１１１には、地図情報、監視センサ１１の検出結果及び位置検出器１４の検出結果が入力される。例えば、状態推定器１１１は、カルマンフィルタを用いて、移動体１の状態推定を行う。状態推定器１１１は、環境中の障害の状態推定も行う。状態推定器１１１は、ＳＬＡＭ（Simultaneous Localization and Mapping）技術を用いて、自己位置推定と環境地図生成とを実行してもよい。状態推定器１１１は、障害の位置等を含む地図情報、移動体１の現在位置及び速度等の移動体に関する情報、障害の移動体１に対する相対位置及び速度等の障害に関する情報、並びに、外乱の情報を出力する。
　尚、障害には、地物、建物、移動体１以外の他の移動体、工事現場、不整地、浅瀬、養殖場等が含まれる。
　図５は、制御演算器１１２の詳細な機能ブロック図である。制御演算器１１２は、移動体１の経路を生成する経路生成器２１と、移動体１の指令速度を設定する設定器２２と、指令速度の制限条件を設定する制限器２５と、指令速度を補正する補正器２６と、移動体１の実速度を指令速度に追従させるような指令力を演算するサーボ制御器２７と、指令力に基づいてアクチュエータ１２の操作量を演算する操作量演算器２８と、指令速度の補正を表示装置１６又は警報器１７を介してユーザに報知する報知制御器２９とを有している。
　経路生成器２１は、記憶器１９に保存された目的地を読み込む。記憶器１９には、目的地が予め保存されている。記憶器１９には経由地が保存されている場合もある。その場合、経路生成器２１は、経由地を記憶器１９から読み込む。経路生成器２１には、状態推定器１１１から地図情報及び移動体に関する情報が入力される。
　経路生成器２１は、地図情報に基づいて移動体１の現在位置から目的地までの経路を生成する。経路生成器２１は、地図情報を参照して、地物等に衝突しないような経路を生成する。環境内に道路、通路又は航路が設定されている場合には、経路生成器２１は、道路、通路又は航路に沿った経路を生成する。例えば、経路生成器２１は、Ａ－ｓｔａｒ探索アルゴリズム又はＲＲＴ－ｓｔａｒアルゴリズムを活用して経路を生成する。経路生成器２１は、経路上のウェイポイント、即ち、経路における現在位置から比較的近い目標位置を設定し、目標位置を指令演算器２３へ出力する。
　設定器２２は、経路に基づいて演算された指令速度と入力装置１５を介して入力された指令速度との何れか一方を指令速度として設定する。詳しくは、設定器２２は、経路生成器２１によって生成された経路に基づいて指令速度を演算する指令演算器２３と、出力する指令速度を入力装置１５からの指令速度と指令演算器２３からの指令速度とで切り替える切替器２４とを有している。入力装置１５からの指令速度は、ユーザのマニュアル操作によって入力される指令速度である。
　指令演算器２３には、状態推定器１１１から移動体に関する情報及び外乱の情報が入力される。指令演算器２３は、移動体１が目標位置（即ち、ウェイポイント）に移動するための指令速度を演算する。例えば、指令演算器２３は、ＰＩＤ制御によって指令速度を演算する。つまり、指令演算器２３は、移動体１の現在位置と目標位置との差分に基づいて指令速度を演算する。
　あるいは、指令演算器２３は、モデル予測制御（ＭＰＣ：Model Predictive Control）によって指令速度を演算してもよい。経路生成器２１によって生成された経路を基本経路とすると、指令演算器２３は、モデル予測制御では、基本経路に含まれる局所的な経路を生成しつつ、局所的な経路に沿って移動する際の指令速度を演算する。局所的な経路は、移動体１の現在位置から目標位置までの経路である。指令演算器２３は、移動体１及び障害の現在の状態量から移動体１及び障害の将来の状態量を予測して、移動体１の現在位置から目標位置までの最適な経路及びそれに応じた指令速度を算出する。この場合、指令演算器２３には、状態推定器１１１から障害に関する情報も入力される。
　切替器２４は、補正器２６に入力する指令速度を入力装置１５からの指令速度と指令演算器２３からの指令速度とで切り替える。自動運転の実行中は、基本的には、切替器２４は、指令演算器２３からの指令速度を補正器２６へ入力する。一方、マニュアル操作によって入力装置１５から指令速度が入力された場合には、切替器２４は、入力装置１５からの指令速度を補正器２６へ入力する。
　制限器２５は、指令速度を制約する制約条件を設定する。詳しくは、制限器２５には、状態推定器１１１から移動体に関する情報、障害に関する情報及び外乱の情報が入力される。制限器２５は、移動体１が障害を回避するための指令速度の制約条件を設定する。例えば、制限器２５は、移動体１の運動学モデル、即ち、キネマティクスモデルを用いて制約条件を設定する。具体的には、制限器２５は、移動体１の運動学モデルを用いて、移動体１と障害との間に所定の距離（以下、「安全距離」ともいう）が確保されるように指令速度の制約条件を設定する。
　制限器２５は、複数の制約条件を設定し得る。例えば、回避を同時に考慮すべき障害が複数存在する場合には、制限器２５は、それぞれの障害に対応する複数の制約条件を設定し得る。さらに、回避を同時に考慮すべき移動体１の保護部位が複数設定されている場合には、制限器２５は、それぞれの保護部位に対応する複数の制約条件を設定し得る。そのため、複数の障害及び複数の保護部位を考慮すべき場合には、障害と保護部位との組み合わせに応じた数の制約条件が設定され得る。
　制限器２５は、障害の回避に加え、指令速度に対する出力特性に関する制約条件を設定する。例えば、制限器２５は、サーボ制御器２７の制御特性に関する制約条件を設定する。そのため、制限器２５には、サーボ制御器２７から制御状態量も入力される。さらに、制限器２５は、障害の回避に加え、アクチュエータ１２に関する制約条件を設定する。制御特性に関する制約条件及びアクチュエータ１２に関する制約条件は、出力特性に関する制約条件の一例である。
　制限器２５は、制約条件を逐次演算して、制約条件を更新する。つまり、移動体１及び障害の状況は、経時的に変化し得る。制限器２５は、移動体１及び障害の状況の経時的変化に対応して、制約条件を更新することができる。制限器２５は、制約条件を補正器２６へ出力する。
　また、制限器２５は、調節可能な制約条件を設定してもよい。例えば、移動体１の保護部位及び障害の種別の少なくとも一方に応じて緩和可能な制約条件については調節可能に設定してもよい。指令速度が適切に補正されない場合には、制限器２５は、調節可能な制約条件を変更する。
　補正器２６は、制約条件によって定められる制限範囲と指令速度とを照らし合わせて、最終的な指令速度を決定する。詳しくは、指令速度が制限範囲内であれば、補正器２６は、指令速度をそのまま最終的な指令速度として決定する。指令速度が制限範囲外であれば、補正器２６は、指令速度を制限範囲内に補正して、最終的な指令速度とする。制限範囲は、有限個、例えば、数個の選択肢のみで構成されるわけではなく、連続的な幅、即ち、拡がりを有している。つまり、補正器２６は、連続的な拡がりを有する範囲の中から適切な速度に指令速度を補正する。例えば、補正器２６は、二次計画法等によって指令速度を制限範囲内に入るように最適化する。補正器２６は、最終的な指令速度をサーボ制御器２７へ出力する。
　サーボ制御器２７には、状態推定器１１１から移動体に関する情報及び外乱の情報が入力される。サーボ制御器２７は、移動体１の実速度を指令速度に追従させるための指令力を演算する。サーボ制御器２７は、例えば、ＰＩＤ制御を行う。尚、サーボ制御器２７の制御は、ＰＩＤ制御に限定されず、ロバスト制御等であってもよい。サーボ制御器２７は、指令力を操作量演算器２８へ出力する。
　操作量演算器２８は、指令力を複数のアクチュエータ１２に分配して、複数のアクチュエータ１２のそれぞれの指令操作量を演算する。アクチュエータ１２が電動モータの場合には、例えば、操作量は、電動モータの回転速度又はトルクである。アクチュエータ１２がエンジンの場合には、例えば、操作量は、アクセル開度及び燃料噴射量である。
　それぞれのアクチュエータ１２は、指令操作量に応じて動作する。アクチュエータ１２には、アクチュエータ１２を動作させるための固有の制御器が設けられている場合がある。例えば、アクチュエータ１２が電動モータである場合には、アクチュエータ１２は、サーボアンプをさらに有している。その場合、サーボアンプが指令操作量に応じて電動モータを動作させる。その結果、移動体１は、推力を発揮し、目標位置へ向かって移動する。
　報知制御器２９には、経路生成器２１から経路及び目標位置が入力される。また、報知制御器２９には、補正器２６から最終指令速度u及び補正の有無が入力される。報知制御器２９は、指令速度が補正される旨を表示装置１６及び警報器１７を介してユーザに報知する。
　具体的には、報知制御器２９は、経路、目標位置及び最終指令速度uを表示装置１６に表示させる。例えば、表示装置１６は、地図上に経路及び目標位置を重畳させて表示し、さらには、最終指令速度uをベクトル表示する。このとき、表示装置１６は、指令速度が補正されているか否かによって最終指令速度uの表示形態を変更する。これにより、ユーザは、移動体１の進行状況及び指令速度の補正の有無を知ることができる。尚、表示装置１６は、「補正中」又は「制限中」のような、指令速度が補正されている旨を表示してもよい。この場合、表示装置１６は、最終指令速度uの表示形態を変更しなくてもよい。
　さらに、指令速度が補正される場合には、報知制御器２９は、指令速度の補正を報知するための指令を警報器１７へ出力する。警報器１７は、指令に応じて警報を発報する。例えば、警報器１７は、警報としての音又は光を発する。これにより、ユーザは、指令速度が補正されていることを知ることができる。
　尚、報知制御器２９は、表示装置１６及び警報器１７の何れか一方だけを介して指令速度の補正をユーザに報知してもよい。
　－制約条件及び制限範囲－
　続いて、制約条件及び制限範囲について詳細に説明する。
　制限器２５及び補正器２６は、例えば、制御バリア関数（ＣＢＦ：Control Barrier Function）を用いて、指令速度を制限する。具体的には、以下の式（１）に示す評価関数Ｊ及び式（２）に示す制約条件に基づいて指令速度を制限する。
Figure JPOXMLDOC01-appb-M000001
 　ここで、urefは、入力指令値、即ち、制御入力である。すなわち、入力指令値urefは、補正前の暫定的な指令値である。uは、補正後の指令値である。xは、制御対象（ここでは、移動体１）の現在の状態量であり、xjは、障害の現在の状態量である。αは、汎関数（ｃｌａｓｓ　κ関数）であり、hiは、下記式（３）で示されるバリア関数である。iは、制約条件を識別するための番号である。例えば、hiは、i番目のバリア関数である。
Figure JPOXMLDOC01-appb-M000002
　バリア関数hiは、安全状態を表す関数である。バリア関数hiは、hi≧０を満たすときに、制御対象が安全となるように設定されている。式（２）は、制御対象をhi≧０の範囲に収束させるための不等式制約条件である。
　移動体１の運動方程式は、式（４）のように、指令値uに対してＡｆｆｉｎｅな形式をとる場合が多いため、制約条件は、線形な式（５）のように変換できる。制限器２５は、式（６）のような制約条件を設定する。
Figure JPOXMLDOC01-appb-M000003
　補正器２６は、制約条件を満たしつつ、評価関数Jが最小になる指令値uを求める。その結果、補正器２６は、入力指令値urefからの変化量が最小で且つ制約条件を満足する指令値uを求める。例えば、補正器２６は、凸最適化の計算手法、例えば、内点法又は二次計画法によって指令値uを求める。補正器２６は、入力指令値urefを指令値uに補正し、最終的な指令値uとして出力する。尚、入力指令値urefが制限範囲内の場合には、評価関数Jが０となる。つまり、補正器２６は、評価関数Jが０か否かによって、入力指令値urefが制限範囲内か否かを判定する。入力指令値urefが制限範囲内の場合には、補正器２６は、入力指令値urefをそのまま最終的な指令値ｕとして出力する。
　図６は、補正器２６による指令値uの補正の一例を示すイメージ図である。この例では、指令値uは指令速度であり、直交するＸ軸及びＹ軸からなる座標系におけるベクトル（u, v）で表される。ハッチングが付された領域は、制約条件を満たす制限範囲である。破線は、制約条件Ｒ１を示し、制限範囲の境界となる。補正器２６に入力された入力指令速度urefが制限範囲外である場合、補正器２６は、制限範囲内に含まれ且つ入力指令速度urefからの変化量が最小となる最終指令速度uを求める。図６からわかるように、境界Ｂ上の点のうち入力指令速度urefから最も近い点が最終指令速度uとなる。
　こうして、補正器２６は、入力指令速度urefを制限範囲内に含まれる最終指令速度uに補正する。最終指令速度uを制限範囲内に制限することによって、制御対象である移動体１が安全に維持される。安全は、バリア関数hiによって定義される。
　このような制約条件の演算及び指令値uを制限範囲内へ補正する演算は、目標位置を再設定する演算や経路（局所的な経路を含む）を再設定する演算に比べて計算負荷が小さい。そのため、制限器２５及び補正器２６は、最適な最終指令値uを早急に求めることができる。例えば、移動体１の近傍に障害が急に現れた場合、又は、障害が急速に移動した場合に、制限器２５及び補正器２６は、最適な最終指令値uを瞬時に求めて、移動体１に障害との干渉を瞬時に回避させることができる。
　＝追加的な制約＝
　以上の説明の制約条件は、移動体１が障害を回避するための制約条件であり、安全上の制約条件である。制限器２５は、安全上の制約条件に加えて、安全性以外の制約条件を併せて考慮してもよい。例えば、安全性以外の制約条件としては、制御特性上の制約条件及びアクチュエータ特性上の制約条件等の出力特性上の制約条件がある。サーボ制御器２７から出力される指令力は、サーボ制御器２７の制御特性上の制約条件を受け得る。あるいは、移動体１が過度に加速しないように指令力に上限が設定される等、サーボ制御器２７の制御特性に意図的に制約が設定される場合もあり得る。さらに、アクチュエータ１２は、最大出力等の特性を有する。制限器２５は、これら安全性以外の制約条件の少なくとも１つを考慮してもよい。
　まず、制御特性上の制約条件について説明する。サーボ制御器２７から出力される指令力は、サーボ制御器２７の応答性及び追従性等の制御特性の影響、即ち、制約を受ける。サーボ制御器２７の伝達関数は、サーボ制御器２７の制御特性を表している。制限器２５は、サーボ制御器２７の逆伝達関数に基づいて、制御特性上の制約条件を指令速度に対する制約条件に変換する。具体的には、制限器２５には、サーボ制御器２７の状態量が入力される。制限器２５は、サーボ制御器２７の状態量に基づいてサーボ制御器２７の逆伝達関数を求める。制限器２５は、逆伝達関数を用いて、指令速度に対する、制御特性上の制約条件を求める。
　次に、アクチュエータ特性上の制約条件について説明する。アクチュエータ１２が全体として発揮できる推力の範囲、即ち、限界は、アクチュエータ１２の配置、出力上限及び出力下限並びに操作量演算器２８の分配ロジック等のアクチュエータ特性に応じて決まっている。そのため、アクチュエータ１２の発揮できる推力の限界がアクチュエータ１２の配置等のアクチュエータ特性に基づいて予め求められ、記憶器１９に保存されている。アクチュエータ１２による推力の限界は、サーボ制御器２７から操作量演算器２８へ出力される指令力への制約とみなすことができる。指令力への制約は、前述の如く、サーボ制御器２７の逆伝達関数を用いて、指令速度に対する制約条件に変換することができる。つまり、アクチュエータ１２による推力の限界は、指令速度に対する制約条件に変換することができる。制限器２５は、記憶器１９に保存されたアクチュエータ１２による推力の限界からサーボ制御器２７の逆伝達関数を用いて、指令速度に対する、アクチュエータ特性上の制約条件を求める。
　制限器２５は、制御特性上の制約及びアクチュエータ特性上の制約条件の両方を考慮する場合には、両方の制約条件を合算して、出力特性上の制約条件を求める。図７は、出力特性上の制約条件を考慮しない、補正器２６による指令値uの補正の一例を示すイメージ図である。図８は、出力特性上の制約条件を考慮した、補正器２６による指令値uの補正の一例を示すイメージ図である。図７において、ハッチングが付された領域は、制限範囲であり、破線は、制約条件Ｒ１を示し、制限範囲の境界となる。一点鎖線は、出力特性上の制約に相当する指令値の制約ある。図８において、ハッチングが付された領域は、制限範囲であり、破線は、制約条件Ｒ１及びＲ２を示し、制限範囲の境界となる。制約条件Ｒ１は、安全上の制約条件であり、制約条件Ｒ２は、出力特性上の制約条件である。
　出力特性上の制約条件を考慮しない場合、補正器２６は、入力指令速度urefを、安全上の制約条件Ｒ１のみに基づいて補正する。最終指令速度uは、制約条件Ｒ１上、即ち、制限条件の境界上に位置する。その後、サーボ制御器２７は、最終指令速度uに基づいて指令力を演算し、操作量演算器２８は、指令力に基づいて第１アクチュエータ１２Ａ及び第２アクチュエータ１２Ｂのそれぞれの指令操作量を演算する。このとき、指令力は、サーボ制御器２７の制御特性の影響を受ける。また、アクチュエータ１２よる総合的な推力は、第１アクチュエータ１２Ａ及び第２アクチュエータ１２Ｂのアクチュエータ特性の影響を受ける。その結果、移動体１に与えられる推力は、最終指令速度uに適切に対応する推力ではなく、制御特性上の制約及びアクチュエータ特性上の制約を受けた推力となる。このような推力の制約を、指令速度uの制約に変換すると、図７の一点鎖線となる。つまり、指令速度uが一点鎖線の制約を満たすようにさらに補正されたのと等しい推力が発揮されることになる。図７の例では、実質的な指令速度u’は、制約条件Ｒ１よりも制限範囲の内側へ離れている。これは、より安全な動作点での運転を意味し、必要以上に指令速度uを制限した運転とみなすこともできる。
　一方、出力特性上の制約条件を考慮する場合、制限器２５は、図８に示すように、安全上の制約条件Ｒ１と出力特性上の制約条件Ｒ２との両方を設定する。補正器２６は、制約条件Ｒ１及びＲ２によって定められる制限範囲内へ入力指令速度urefを補正する。その結果、補正器２６は、制限範囲の境界上における、入力指令速度urefから最も近い点に入力指令速度urefを補正する。この例では、制約条件Ｒ１と制約条件Ｒ２との交点に入力指令速度urefが補正される。こうして求められた最終指令速度uは、図７と比較すると、ｕ軸方向の速度は同じであるが、ｖ軸方向への速度は増大している。その結果、移動体１は、安全を確保しつつ、出力特性上の制約内でできる限り大きな推力を発揮する。
　＝可変の制約条件＝
　制限器２５は、調節可能、即ち、可変の制約条件を設定することができる。例えば、障害には、不整地又は気流が激しい場所等のように、通行可能ではあるが、通行を回避することが好ましい障害がある。制限器２５は、制約条件の対象となる障害の種別に応じて、制約条件を可変とするか固定とするかを切り替えてもよい。通行可能な障害に対しては、制限器２５は、制約条件を調節可能、即ち、緩和可能としてもよい。例えば、制約条件が厳しい場合には、制限範囲内に含まれる指令値uが存在しない場合や、制限範囲内に含まれる指令値uが微小になる場合があり得る。その結果、移動体１を適切に移動させることが困難となる。そのような場合、制限器２５は、可変の制約条件を緩和するように変更して、制限範囲を拡大する。その結果、制限範囲内に含まれる指令値uを設定することが可能となる、又は、制限範囲内に含まれる指令値uが増大する可能性がある。
　あるいは、制限器２５は、保護部位と障害との間に安全距離が確保されるように制約条件を設定する場合、安全距離が低減されるように制約条件を調節可能、即ち、緩和可能に設定してもよい。安全距離は、保護部位と障害との干渉が確実に回避されるように、必要最小距離に余裕分を付加して設定されている。制限器２５は、安全距離のうち余裕分の範囲で制約条件を調節可能としてもよい。例えば、移動体１に複数の保護部位が設定され且つ、複数の保護部位に保護の優先順位が設定されている場合、制限器２５は、優先順位が相対的に低い保護部位に対応する制約条件を可変としてもよい。具体的には、保護部位は第１保護部位と第１保護部位よりも優先順位の低い第２保護部位とが含まれている場合、制限器２５は、第１保護部位に対応する制約条件を固定し、第２保護部位に対応する制約条件を可変としてもよい。あるいは、制限器２５は、適切な指令値uが設定されるまで、優先順位の低い順に、保護部位に対応する制約条件を可変として順次緩和してもよい。
　制限器２５は、可変の制約条件については、式（７）のようにスラック変数σiを導入する。
Figure JPOXMLDOC01-appb-M000004
　この場合、制限器２５は、スラック変数を含む制約条件を補正器２６へ出力する。補正器２６は、評価関数Jを式（８）のように設定し、スラック変数σiの重みを調節する。固定の制約条件はスラック変数σiを含んでいないので、補正器２６は、評価関数Jにおいて固定の制約条件のスラック変数σiの重みを０とする。制限器２５及び補正器２６は、このようにして、制限範囲を調節、即ち、拡大することができる。
Figure JPOXMLDOC01-appb-M000005
　図９は、固定の制約条件と可変の制約条件とを含む場合の補正器２６による指令値uの補正の一例を示すイメージ図である。図９では、複数の制約条件として、固定の第１制約条件R1と可変の第２制約条件R2とが含まれている。制限器２５は、スラック変数σ2を調整することによって第２制約条件R2を変更し、結果として、制限範囲を変更することができる。
　例えば、補正後の指令値uが適切でない場合などには、制限器２５は、可変の制約条件を調節することによって、入力指令値urefの補正をやり直してもよい。制約条件が変更されることによって、補正後の指令値uが制約条件の変更前と比較して変化し得る。これにより、補正後の指令値uが改善される可能性がある。
　＝デッドロック対策＝
　入力指令速度urefが制限範囲内に補正された結果、最終指令速度uが略０になる場合がある。つまり、移動体１が略停止する。この状態をデットロックという。デッドロックに陥る場合には、制約条件を変更するか又は補正前の指令値uを変更して、デッドロックを解消させる。
　具体的には、制約条件が調節可能な場合には、補正器２６は、制限器２５に可変の制約条件を変更させる。例えば、デッドロックの可能性がある場合、補正器２６は、その旨を制限器２５へ通知する。制限器２５は、可変の制約条件を変更し、変更後の制約条件を補正器２６へ出力する。制約条件の変更によって制限範囲が変化するので、補正後の指令値uは、０からずれる、即ち、０以外の値となる可能性がある。
　図１０は、デッドロック発生時に制約条件を変更する場合の補正器２６による指令値uの補正の一例を示すイメージ図である。図１０の例では、制約条件を変更しない場合、補正器２６が入力指令速度urefを制限範囲内に補正すると、最終指令速度uが０になり、デッドロックに陥る（二点鎖線参照）。この場合には、補正器２６は、制限器２５に制約条件を変更させる。例えば、前述の図９のように、可変の制約条件が設定されている場合には、制限器２５は、可変の制約条件を変更する。図１０の例では、制限器２５は、第２制約条件R2を変更する。補正器２６は、第２制約条件R2の変更後に、入力指令速度urefを制限範囲内に補正する。制限範囲が変更されるので、最終指令速度uは、０からずれ、０以外の値となる。その結果、移動体１は、デッドロックに陥ることなく、移動することができる。
　あるいは、補正器２６は、入力指令値urefを予備補正してから、制限範囲内へ補正する。補正器２６は、入力指令値urefを所定量だけ予備的に補正する。予備補正の所定量は、予備補正のたびに変更されてもよい。あるいは、所定量は、一定であってもよい。入力指令値urefの予備補正によって、入力指令値urefと制限範囲との関係性が変化するため、入力指令値urefから補正量が最小となる制限範囲内の指令値uも変化し得る。その結果、補正後の指令値uは、０からずれる、即ち、０以外の値となる可能性がある。
　図１１は、デッドロック発生時に予備補正を行う場合の補正器２６による指令値uの補正の一例を示すイメージ図である。図１１の例では、予備補正がない場合、補正器２６が入力指令速度urefを制限範囲内に補正すると、最終指令速度uが０になり、デッドロックに陥る（二点鎖線参照）。この場合には、補正器２６は、入力指令速度urefをまず予備補正する。補正器２６は、予備補正後の入力指令速度uref’を制限範囲内に補正する。最終指令速度uは、０からずれ、０以外の値となる。その結果、移動体１は、デッドロックに陥ることなく、移動することができる。
　尚、デッドロックに陥る場合は、補正器２６は、デッドロックの状態で待機してもよい。時間の経過と共に障害の状態が変化し、それにより制約条件が変化する可能性がある。その場合、デッドロックが自然と解消する。例えば、他の移動体に対応する制約条件に起因してデッドロックが発生した場合、他の移動体が移動すると、制約条件が変化して、他の移動体との干渉の可能性が低減し得る。その場合、制限範囲が拡大され、デッドロックが解消する。あるいは、前述のような制約条件の変更、入力指令値urefの予備補正、及び、待機等の対策を行ってもデッドロックが解消しない場合には、経路生成器２１は、経路を生成し直してもよい。
　続いて、制御装置１００の動作について図１２を参照しながら説明する。図１２は、指令値の制限処理のフローチャートである。図１２は、指令速度が自動的に設定される自動運転の例である。
　まず、制御装置１００は、ステップＳ１０１において、経路を生成する。具体的には、経路生成器２１は、地図情報、移動体に関する情報等に基づいて、移動体１の現在位置から経由地を介して目的地に至る経路を生成する。さらに、経路生成器２１は、経路上のウェイポイント、即ち、現在位置から経路上の直近の目標位置を決定する。
　次に、制御装置１００は、ステップＳ１０２において、入力指令速度urefを算出する。具体的には、指令演算器２３は、移動体１が目標位置に移動するための入力指令速度urefを演算する。
　そして、制御装置１００は、ステップＳ１０３において、指令速度uの制約条件を設定する。具体的には、制限器２５は、移動体に関する情報及び障害に関する情報に基づいて制約条件を演算する。このとき、制限器２５は、出力特性上の制約条件も設定する。
　続いて、制御装置１００は、ステップＳ１０４において、入力指令速度urefが制限範囲に入っているか否かを判定する。具体的には、補正器２６は、評価関数J及び制約条件を用いて、入力指令速度urefに対して評価関数Jが０になるか否かを判定する。評価関数Jが０になる場合には、補正器２６は、入力指令速度urefが制限範囲に入っていると判定する。
　入力指令速度urefが制限範囲に含まれる場合には、制御装置１００は、ステップＳ１０５において、入力指令速度urefを最終指令速度uとして決定する。入力指令速度urefが制限範囲外の場合は、制御装置１００は、ステップＳ１０７において、入力指令速度urefを制限範囲内へ補正する。具体的には、補正器２６は、前述のように評価関数J及び制約条件を用いて、入力指令速度urefを制限範囲内へ補正する。
　続いて、制御装置１００は、ステップＳ１０８において、入力指令速度urefの補正によってデッドロックが発生するか否かを判定する。具体的には、補正器２６は、補正後の指令速度uが所定の閾値未満となるか否かを判定する。閾値は、０に近い微小な値である。補正後の指令速度uが閾値未満とならない場合には、制御装置１００は、ステップＳ１０５において、補正後の指令速度uを最終指令速度uとして決定する。
　補正後の指令速度uが閾値未満となる場合には、制限器２５は、ステップＳ１０９において、制約条件の緩和が可能か否かを判定する。例えば、制限器２５は、保護部位又は障害の種別に応じて制約条件が緩和可能か否かを判定する。
　制約条件の緩和が可能な場合には、補正器２６は、ステップＳ１１０において、制約条件を緩和する。その後、補正器２６は、ステップＳ１０７において、入力指令速度urefを制限範囲内へ補正し、ステップＳ１０８においてデッドロックを判定する。制約条件の変更によって制限範囲が変更されているので、入力指令速度urefは、先の補正とは異なる最終指令速度uに補正され得る。その結果、デッドロックが発生しない状態となっていることが多い。
　一方、制約条件の緩和が不能な場合には、補正器２６は、ステップＳ１１１において、入力指令速度urefを予備補正する。その後、補正器２６は、ステップＳ１０７において、予備補正後の入力指令速度uref’を制限範囲内へ補正し、ステップＳ１０８においてデッドロックを判定する。入力指令速度urefは、予備補正されているので、先の補正とは異なる最終指令速度uに補正され得る。その結果、デッドロックが発生しない状態となっていることが多い。
　このように、デッドロックが解消されるまでステップＳ１１０又はＳ１１１が実行される。これにより、最終指令速度uは、いずれは閾値より大きな値に補正される。
　最終指令速度uが決定されると、制御装置１００は、ステップＳ１０６において、最終指令速度uに応じてアクチュエータ１２を動作させ、移動体１を移動させる。具体的には、サーボ制御器２７は、最終指令速度uに応じて指令力を演算し、操作量演算器２８は、指令力に基づいて複数のアクチュエータ１２のそれぞれの指令操作量を演算する。その後、複数のアクチュエータ１２は、それぞれの指令操作量に基づいて動作する。これにより、移動体１が目標位置へ向かって移動する。
　このとき、制御装置１００、具体的には、報知制御器２９は、生成された経路を表示装置１６に表示させる。制御装置１００は、最終指令速度uをベクトル等の形態で表示装置１６に表示させる。さらに、制御装置１００は、入力指令速度urefが補正された場合、即ち、障害の回避を実行している場合には、表示装置１６及び警報器１７の少なくとも一方を介して、入力指令速度urefの補正をユーザに報知する。このとき、制御装置１００は、デッドロックの有無、制限条件の緩和又は、入力指令速度urefの予備補正等の具体的な補正内容を表示装置１６及び警報器１７の少なくとも一方を介して報知してもよい。
　このような処理が繰り返されることによって、移動体１が経路に沿って移動する。その際、移動体１と障害との干渉の可能性がある場合には、指令速度uが瞬時に制限され、安全が確保される。
　尚、ユーザから入力指令速度urefが入力装置１５を介して入力される場合には、ステップＳ１０１及びＳ１０２に代えて、入力装置１５を介したマニュアル入力が行われる。そして、ステップＳ１０３以降の処理は、自動運転の場合と同様であり、制約条件が設定され（ステップＳ１０３）、マニュアル入力された入力指令速度urefが制限範囲内か否か判定され（ステップＳ１０４）、入力指令速度urefが制限範囲外であれば、入力指令速度urefが制限範囲内に入るように補正される（ステップＳ１０５）。その他の処理も同様である。つまり、マニュアル入力であっても、移動体１と障害との干渉の可能性がある場合には、指令速度uが瞬時に制限され、安全が確保される。
　次に、制御装置１００による自動運転の具体例について説明する。図１３は、船舶のモデルを示す説明図である。図１３の例では、移動体１は、船舶である。
　移動体１の運動学モデルを指令速度u＝（u,v,γ）Tに対してＡｆｆｉｎｅな形に整理すると、下記の式のように表される。kは、移動体１における部位を識別するための番号である。
Figure JPOXMLDOC01-appb-M000006
　図１４は、車両のモデルを示す説明図である。図１４の例では、移動体１は、牽引車１Ａと被牽引車１Ｂとを含む車両である。被牽引車１Ｂは、牽引車１Ａに揺動自在に連結されている。
　車両の運動学モデルを指令速度u＝(U, γ)Tに対してＡｆｆｉｎｅな形に整理すると、下記式のようになる。
Figure JPOXMLDOC01-appb-M000007
　ここで、Uは、牽引車１Ａに与えられる前進速度であり、γは、牽引車１Ａのヨーレートである。θは、地図座標系における被牽引車１Ｂの角度であり、βは、被牽引車１Ｂに対する牽引車１Ａの相対角度である。（xt, yt）は、地図座標系における被牽引車１Ｂの保護部位の位置座標である。L1は、牽引車１Ａにおける、被牽引車１Ｂの連結点から保護部位までの距離である。L2は、被牽引車１Ｂにおける、被牽引車１Ｂの保護部位から牽引車１Ａとの連結点までの距離である。Mは、牽引車１Ａの連結点と被牽引車１Ｂの連結点との距離である。
　制限器２５は、このような運動学モデル、即ち、キネマティクスモデルを用いて移動体１をモデル化して、制約条件を設定する。
　例えば、図１３の例において、移動体１が自動運転によって岸壁に接岸する際に、制限器２５は、移動体１の保護部位と障害の距離が安全距離（即ち、必要最小距離に余裕分を加えた距離）よりも大きくなることを制約条件として設定する。例えば、図１３の船舶の例において保護部位が船首で、障害が岸壁である場合に、船首と岸壁が安全距離を保つための制約条件は、下記式のように設定される。
Figure JPOXMLDOC01-appb-M000008
　式（１１）を時間微分すると、下記式（１２）のようになる。
Figure JPOXMLDOC01-appb-M000009
　ここで、式（１２）の右辺は、２×（岸壁方向単位ベクトル）×（自船運動学のヤコビアン）×（自船と岸壁の相対速度）である。
　式（１１）及び（１２）に基づいて、指令速度uの制約条件が以下のように設定される。
Figure JPOXMLDOC01-appb-M000010
　この制約条件のもと、補正器２６は、船首と岸壁とが安全距離を保ちつつ、移動体１を岸壁に接近させるための指令速度uを算出できる。最終的に、移動体１は、目的地、即ち、接岸する位置に到達する。このときも、船首と岸壁との間には、必要最小距離に余裕分を加えた距離が確保されている。尚、制限器２５が船尾についても同様の制約条件を設定することによって、船尾と岸壁との間にも安全距離を確保することができる。さらに、移動体１の周辺に他の船舶が出現した場合には、制限器２５が他の船舶に対する制約条件、例えば、船首と他の船舶との制約条件及び船尾と他の船舶との制約条件を設定することによって、移動体１と他の船舶との間にも安全距離を確保することができる。
　さらに、制限器２５は、移動体１の移動状況に応じて制約条件を変更してもよい。例えば、前述の例では、船首と岸壁との距離が安全距離に略等しくなると、移動体１は、それ以上岸壁に接近しない。そこで、制限器２５は、制約条件を緩和する。例えば、制限器２５は、制約条件において、保護領域半径rk及び岸壁マージンΔsの少なくとも一方を減少させる。つまり、必要最小距離及び余裕分の少なくとも一方が減少する。その結果、移動体１はより岸壁に接近することができる。制限器２５は、移動体１が目的地に到達するまで制約条件を緩和することによって、移動体１を目的地に到達させることができる。尚、制約条件の緩和は、制約条件を直接変更することに限定されず、補正器２６が、前述の可変の制約条件を含む評価関数Jにおいて対応するスラック変数σiの重みづけを変更する、例えば、減少させることによっても実現できる。
　このような制御装置１００によれば、障害を回避するための指令値の制約条件が設定され、制約条件で定められる制限範囲から指令値が外れている場合には、指令値が制限範囲内に補正される。つまり、指令値が制約条件を満たすように制限される。これにより、移動体１は障害を回避しつつ、目標位置へ移動する。このように、指令値の制限によって障害の回避が実現されるため、目標位置又は経路を再生成する必要がない。指令値の制限に必要な演算量は、目標位置又は経路の再計算に比べて少ない。つまり、指令値の制限を迅速に実行することができ、障害に対して瞬時に対応することができる。
　さらに、指令値は制限範囲内に補正されるので、補正される指令値の選択肢は拡がりを有する。つまり、移動体１を単に停止させるとか、移動体１を決まった向きに（例えば、左向きに）一律に方向転換させるといった対応と異なり、移動体１は柔軟な対応を取ることができる。つまり、移動体１は、指令値が制限されながらも、目標位置へ向けた移動を継続することができる。
　また、制約条件は逐次更新されるので、時々刻々と変化する移動体１及びその周辺環境に対応して制約条件が設定される。つまり、指令値は常に一律に制限されるのではなく、移動体１及びその周辺環境の変化に応じた制約条件によって制限される。
　さらに、制御装置１００は、指令値として指令速度を制限する。そのため、制御装置１００は、制約条件の演算、及び、指令速度を制限範囲内へ補正するための演算を移動体１の運動学モデル、即ち、キネマティクスモデルを用いて実現することができる。これにより、移動体１のダイナミクスモデルを用いて演算する場合と比較して、計算式が単純化され、ユーザによる設計が容易になる。
　また、制御装置１００は、サーボ制御器２７よりも前段において指令値を制限しているので、外乱の影響及びモデル誤差をサーボ制御によって低減することができる。例えば、サーボ制御器２７の後段において指令値を制限すると、外乱の影響等を低減するために調節された指令値が変更されることになる。その結果、サーボ制御による外乱の影響及びモデル誤差の低減効果が小さくなってしまう。サーボ制御器２７よりも前段において指令値が制限されることによって、安全を確保しつつ、外乱の影響及びモデル誤差を適切に低減することができる。
　また、制御装置１００は、可変の制約条件を設定することによって、移動体１の適切な移動を実現するように制約条件を調節できる。例えば、移動体１の円滑な移動が実現できない場合には、制約条件を緩和することによって、移動体１の移動を円滑にすることができ得る。
　さらに、制御装置１００は、デッドロックが生じる場合に、制約条件の変更、又は、入力指令値の予備補正によってデッドロックを解消することができる。
　また、制御装置１００は、指令値が制限範囲外である場合には、その旨を表示装置１６及び警報器１７の少なくとも一方を介してユーザに報知する。これにより、ユーザは、障害の回避が実行されていることを知ることができる。さらに、制御装置１００は、経路及び指令値の少なくとも一方を、指令値が制限範囲外か否かにかかわらず、表示装置１６に表示させる。これにより、ユーザは、障害の回避を行っているか否かにかかわらず、移動体１の経路及び指令値の少なくとも一方を知ることができる。
　《その他の実施形態》
　以上のように、本出願において開示する技術の例示として、前記実施形態を説明した。しかしながら、本開示における技術は、これに限定されず、適宜、変更、置き換え、付加、省略などを行った実施の形態にも適用可能である。また、前記実施形態で説明した各構成要素を組み合わせて、新たな実施の形態とすることも可能である。また、添付図面および詳細な説明に記載された構成要素の中には、課題解決のために必須な構成要素だけでなく、前記技術を例示するために、課題解決のためには必須でない構成要素も含まれ得る。そのため、それらの必須ではない構成要素が添付図面や詳細な説明に記載されていることをもって、直ちに、それらの必須ではない構成要素が必須であるとの認定をするべきではない。
　例えば、運転支援システム１０００における移動体１は、前述の例に限定されない。例えば、移動体１は、車両又は船舶に限定されず、自動運転を実行可能であれば、任意の移動体が採用され得る。移動体１は、自律ロボット（例えば、キャリアパレット）、又は飛行装置（例えば、ドローン）等であってもよい。
　運転支援システム１０００に含まれる移動体１の個数は、図１の例に限定されず、任意である。
　アクチュエータ１２は、移動体１の移動に関するアクチュエータである。アクチュエータ１２は、電動モータ、エンジン及びステアリング装置に限定されない。例えば、移動体１が船舶の場合は、プロペラ及びスラスタ等がアクチュエータである。移動体１がドローンの場合は、プロペラがアクチュエータである。
　制約条件によって制限される対象は、指令速度に限定されない。アクチュエータ１２よりも前段の指令値であれば、任意の指令値が制限の対象となる。つまり、前述の説明において、「指令速度」は「指令値」と読み替えることができる。例えば、指令力が制限されてもよい。その場合、制御演算器２１２の詳細な機能ブロックは、図１５に示すようになる。図１５の制御演算器２１２では、設定器２２からの指令速度がサーボ制御器２７へ直接入力される。サーボ制御器２７から出力される指令力が補正器２２６へ入力される。補正器２２６は、制限器２２５からの制約条件によって定められる制限範囲内へ指令力を補正する。この例では、補正器２２６は、操作量演算器としても機能し、アクチュエータ１２の指令操作量も演算する。制限器２２５は、移動体１の動力学モデル、即ち、ダイナミクスモデルを用いて、力指令を用いて、移動体１と障害との間に所定の距離が確保されるように指令力の制約条件を設定する。例えば、外乱の影響及びモデル誤差が小さい場合には、このようにサーボ制御の後段において指令値を制限してもよい。
　制限器２５は、制御特性上に制約条件及びアクチュエータ特性上の制約条件を設定しているが、これに限定されない。安全上の制約条件以外の制約条件は、必須ではない。
　フローチャートは、一例に過ぎない。フローチャートにおけるステップを適宜、変更、置き換え、付加、省略等を行ってもよい。また、フローチャートにおけるステップの順番を変更したり、直列的な処理を並列的に処理したりしてもよい。例えば、デッドロックの発生が判定された後に、制約条件の緩和（ステップＳ１０９及びＳ１１０）を行わずに、指令値の予備補正（ステップＳ１１１）がまず行われてもよい。指令値の予備補正を行ってもデッドロックが解消しない場合に、制約条件の緩和が行われてもよい。
　あるいは、デッドロックの判定以降のステップＳ１０８，Ｓ１０９，Ｓ１１０及びＳ１１１を省略してもよい。すなわち、ステップＳ１０７において指令値が補正された後は、補正後の指令値に指令値が決定され（ステップＳ１０５）、移動体１の駆動が行われてもよい（ステップＳ１０６）。このときデッドロックが生じた場合には、移動体１は停止し得る。しかし、時間の経過とともに、障害を含む周辺環境が変化して、デッドロックが自然と解消し得る。
　本明細書中に記載されている構成要素により実現される機能は、当該記載された機能を実現するようにプログラムされた、汎用プロセッサ、特定用途プロセッサ、集積回路、ＡＳＩＣｓ（Application Specific Integrated Circuits）、ＣＰＵ（a Central Processing Unit）、従来型の回路、及び／又はそれらの組合せを含む、回路（circuitry）又は演算回路（processing circuitry）において実装されてもよい。プロセッサは、トランジスタ及びその他の回路を含み、回路又は演算回路とみなされる。プロセッサは、メモリに格納されたプログラムを実行する、プログラマブルプロセッサ（programmed processor）であってもよい。
　本明細書において、回路（circuitry）、ユニット、手段は、記載された機能を実現するようにプログラムされたハードウェア、又は実行するハードウェアである。当該ハードウェアは、本明細書に開示されているあらゆるハードウェア、又は、当該記載された機能を実現するようにプログラムされた、又は、実行するものとして知られているあらゆるハードウェアであってもよい。
　当該ハードウェアが回路（circuitry）のタイプであるとみなされるプロセッサである場合、当該回路、手段、又はユニットは、ハードウェアと、当該ハードウェア及び又はプロセッサを構成する為に用いられるソフトウェアの組合せである。
　本開示の技術をまとめると、以下のようになる。
［１］移動体１を移動させるためのアクチュエータ１２の操作量を演算する制御装置１００は、前記移動体１を目標位置へ移動させるための指令値を設定する設定器２２と、障害を回避するための指令値の制約条件を設定する制限器２５と、前記指令値が前記制約条件によって定められる制限範囲外である場合に、前記指令値を前記制限範囲内へ補正する補正器２６と、前記指令値に基づいて前記操作量を演算する操作量演算器２８とを備える。
　この構成によれば、移動体１を目標位置に移動させるための指令値が暫定的に設定される。このとき、障害を回避するように指令値を制限する制約条件が設定される。制約条件は、指令値の制限範囲を定める。暫定的な指令値が制限範囲外の場合には、指令値は制限範囲内へ補正される。指令値に基づいて求められた操作量によれば、移動体は、障害を回避しつつ、目標位置へ向かって移動する。このとき、制御装置１００は、障害を回避するように目標位置を変更したり、経路を変更したりする必要がない。つまり、制御装置１００は、指令値を制限するだけで、目標位置又は経路を変更するための演算を行う必要がない。その結果、制御装置１００の演算負荷を低減されるので、指令値を迅速に制限でき、障害に対して瞬時に対応することができる。
　［２］　［１］に記載の制御装置１００において、前記制限器２５は、前記制約条件を逐次演算して、前記制約条件を更新する。
　この構成によれば、制約条件は、常に一定ではなく、刻々と変化する移動体１及びその周辺環境に対応して更新される。その結果、状況に応じた制限範囲が設定され、移動体１の安全が適切に確保される。
　［３］　［１］又は［２］に記載の制御装置１００において、前記指令値としての前記移動体１の指令速度に基づいてサーボ制御を実行するサーボ制御器２７をさらに備え、前記補正器２６は、前記指令速度を前記制限範囲内へ補正して最終的な指令速度を前記サーボ制御器２７へ出力し、前記サーボ制御器２７は、前記最終的な指令速度に基づいて指令力を出力し、前記操作量演算器２８は、前記指令力に基づいて前記操作量を演算する。
　この構成によれば、サーボ制御器２７よりも前段で指令値が制限されるため、サーボ制御器２７によるサーボ制御によって調節された指令力が制限されずに出力される。つまり、外乱の影響又はモデル誤差を低減させる指令力が出力される。この指令力に基づいて操作量が演算されるので、移動体１を安定的且つ精度よく移動させることができる。
　［４］　［１］乃至［３］の何れか１つに記載の制御装置１００において、前記制限器２５は、障害の回避に加え、前記サーボ制御器２７の制御特性に関する制約条件を設定する。
　この構成によれば、サーボ制御器２７の制御特性上の制約も考慮して、適切な操作量を演算することができる。詳しくは、サーボ制御器２７は、固有の制御特性を有し、全ての指令速度に対して期待通りの指令力を出力するわけではない。サーボ制御器２７の制御特性上の制約により、指令速度通りの指令力が出力されない場合があり得る。制御特性上の制約条件を予め考慮して制限範囲が設定されることによって、安全を確保しつつ且つ制御特性の影響を受け難い範囲に指令速度を補正することができる。これにより、指令速度から期待される通りの指令力が出力され、結果として、指令速度に適切に対応する操作量が演算される。
　［５］　［１］乃至［４］の何れか１つに記載の制御装置１００において、前記制限器２５は、障害の回避に加え、前記アクチュエータ１２に関する制約条件を設定する。
　この構成によれば、アクチュエータ１２の特性上の制約を考慮して、適切な推力を発揮させることができる。詳しくは、アクチュエータ１２は、固有のアクチュエータ特性を有し、全ての指令力に対して期待通りの推力を発揮するわけではない。つまり、アクチュエータ特性上の制約により、指令速度通りの推力が出力されない場合があり得る。アクチュエータ特性上の制約条件を予め考慮して制限範囲が設定されることによって、安全を確保しつつ且つアクチュエータ特性の影響を受け難い範囲に指令速度を補正することができる。これにより、指令速度に適切に対応する推力が発揮される。
　［６］　［１］乃至［５］の何れか１つに記載の制御装置１００において、前記制限器２５は、前記移動体１と障害との間に所定の距離が確保されるように前記制約条件を設定する。
　この構成によれば、移動体１は、移動体１と障害との間に所定の距離が確保しつつ、目標位置に向かって移動することができる。
　［７］　［１］乃至［６］に記載の制御装置１００において、前記制限器２５は、前記移動体１のうち複数の保護部位が障害を回避するように、前記複数の保護部位に対応する複数の前記制約条件を設定する。
　この構成によれば、移動体１が目標位置へ向かって移動する際に、複数の保護部位の安全を確保することができる。
　［８］　［１］乃至［７］の何れか１つに記載の制御装置１００において、前記制限器２５は、前記制約条件を調節可能に設定する。
　この構成によれば、制限器２５は、制約条件を柔軟に変更することができる。例えば、指令値を制限範囲内に補正した結果、移動体１の適切な移動を実現できない場合もあり得る。そのような場合に、制約条件を調節して制限範囲を変更することによって、移動体１を適切に移動させ得る。
　［９］　［１］乃至［８］の何れか１つに記載の制御装置１００において、補正後の前記指令値が所定の閾値未満となる場合に、前記制限器２５は、前記制約条件を変更して、前記補正器２６は、変更された前記制限範囲内へ前記指令値を補正する。
　この構成によれば、補正後の指令値が微小になって移動体１の適切な移動を実現できない場合に、制約条件が変更される。制約条件が変更されると、制限範囲が変更され、補正後の指令値も変化して閾値以上となる可能性がある。つまり、制約条件を変更することによって、補正後の指令値が閾値未満となる状況が解消され得る。
　［１０］　［１］乃至［９］の何れか１つに記載の制御装置１００において、補正後の前記指令値が所定の閾値未満となる場合に、前記補正器２６は、前記指令値を予備補正して、予備補正された前記指令値を前記制限範囲内へ補正する。
　この構成によれば、補正後の指令値が微小になって移動体１の適切な移動を実現できない場合に、指令値が予備補正される。指令値が予備補正されると、指令値を制限範囲内へ補正するための補正量も変化するため、補正後の指令値が閾値以上となる可能性がある。つまり、指令値を予備補正することによって、補正後の指令値が閾値未満となる状況が解消され得る。
　［１１］　［１］乃至［１０］の何れか１つに記載の制御装置１００において、前記設定器２２は、前記指令値として指令速度を設定し、前記制限器２５は、前記移動体１の運動学モデルに基づいて前記制限範囲を演算する。
　この構成によれば、いわゆるキネマティクスによって移動体１がモデル化される。それにより、計算式が単純化され、ユーザによる設計が容易になる。
　［１２］　［１］乃至［１１］の何れか１つに記載の制御装置１００において、前記指令値の補正を前記移動体１が備える表示装置１６又は警報器１７を介してユーザに報知する報知制御器２９をさらに備える。
　この構成によれば、指令値の補正が行われていることをユーザが知ることができるこれにより、ユーザは、移動体１の運転状況を的確に知ることができる。例えば、移動体１の自動運転中にユーザがオーバーライドを実行する際に、指令値の補正の有無を参考にすることができる。

 

Claims (12)

  1. 　移動体を移動させるためのアクチュエータの操作量を演算する制御装置であって、
    　前記移動体を目標位置へ移動させるための指令値を設定する設定器と、
    　障害を回避するための前記指令値の制約条件を設定する制限器と、
    　前記指令値が前記制約条件によって定められる制限範囲外である場合に、前記指令値を前記制限範囲内へ補正する補正器と、
    　前記指令値に基づいて前記操作量を演算する操作量演算器とを備える制御装置。
  2. 　請求項１に記載の制御装置において、
    　前記制限器は、前記制約条件を逐次演算して、前記制約条件を更新する制御装置。
  3. 　請求項１に記載の制御装置において、
    　前記指令値としての前記移動体の指令速度に基づいてサーボ制御を実行するサーボ制御器をさらに備え、
    　前記補正器は、前記指令速度を前記制限範囲内へ補正して最終的な指令速度を前記サーボ制御器へ出力し、
    　前記サーボ制御器は、前記最終的な指令速度に基づいて指令力を出力し、
    　前記操作量演算器は、前記指令力に基づいて前記操作量を演算する制御装置。
  4. 　請求項３に記載の制御装置において、
    　前記制限器は、障害の回避に加え、前記サーボ制御器の制御特性に関する制約条件を設定する制御装置。
  5. 　請求項３に記載の制御装置において、
    　前記制限器は、障害の回避に加え、前記アクチュエータに関する制約条件を設定する制御装置。
  6. 　請求項１に記載の制御装置において、
    　前記制限器は、前記移動体と障害との間に所定の距離が確保されるように前記制約条件を設定する制御装置。
  7. 　請求項１に記載の制御装置において、
    　前記制限器は、前記移動体のうち複数の保護部位が障害を回避するように、前記複数の保護部位に対応する複数の前記制約条件を設定する制御装置。
  8. 　請求項１に記載の制御装置において、
    　前記制限器は、前記制約条件を調節可能に設定する制御装置。
  9. 　請求項１に記載の制御装置において、
    　補正後の前記指令値が所定の閾値未満となる場合に、
    　　前記制限器は、前記制約条件を変更して、
    　　前記補正器は、変更された前記制限範囲内へ前記指令値を補正する制御装置。
  10. 　請求項１に記載の制御装置において、
    　補正後の前記指令値が所定の閾値未満となる場合に、前記補正器は、前記指令値を予備補正して、予備補正された前記指令値を前記制限範囲内へ補正する制御装置。
  11. 　請求項１に記載の制御装置において、
    　前記設定器は、前記指令値として指令速度を設定し、
    　前記制限器は、前記移動体の運動学モデルに基づいて前記制約条件を設定する制御装置。
  12. 　請求項１に記載の制御装置において、
    　前記指令値の補正を前記移動体が備える表示装置又は警報器を介してユーザに報知する報知制御器をさらに備える制御装置。

     
PCT/JP2023/028799 2022-08-08 2023-08-07 制御装置 WO2024034579A1 (ja)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2022126401A JP2024022942A (ja) 2022-08-08 2022-08-08 制御装置
JP2022-126401 2022-08-08

Publications (1)

Publication Number Publication Date
WO2024034579A1 true WO2024034579A1 (ja) 2024-02-15

Family

ID=89851790

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2023/028799 WO2024034579A1 (ja) 2022-08-08 2023-08-07 制御装置

Country Status (2)

Country Link
JP (1) JP2024022942A (ja)
WO (1) WO2024034579A1 (ja)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2010173616A (ja) * 2009-02-02 2010-08-12 Nissan Motor Co Ltd 車両走行支援装置及び車両走行支援方法
WO2014141351A1 (ja) * 2013-03-11 2014-09-18 株式会社 日立製作所 自律制御装置
WO2016080555A1 (ja) * 2015-11-27 2016-05-26 株式会社小松製作所 鉱山機械の制御システム、鉱山機械、鉱山機械の管理システム、及び鉱山機械の管理方法
JP2020035182A (ja) * 2018-08-30 2020-03-05 トヨタ自動車株式会社 制御装置及び制御方法
CN112506194A (zh) * 2020-12-03 2021-03-16 中山大学 一种移动机器人集群分布式安全学习控制方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2010173616A (ja) * 2009-02-02 2010-08-12 Nissan Motor Co Ltd 車両走行支援装置及び車両走行支援方法
WO2014141351A1 (ja) * 2013-03-11 2014-09-18 株式会社 日立製作所 自律制御装置
WO2016080555A1 (ja) * 2015-11-27 2016-05-26 株式会社小松製作所 鉱山機械の制御システム、鉱山機械、鉱山機械の管理システム、及び鉱山機械の管理方法
JP2020035182A (ja) * 2018-08-30 2020-03-05 トヨタ自動車株式会社 制御装置及び制御方法
CN112506194A (zh) * 2020-12-03 2021-03-16 中山大学 一种移动机器人集群分布式安全学习控制方法

Also Published As

Publication number Publication date
JP2024022942A (ja) 2024-02-21

Similar Documents

Publication Publication Date Title
US10401856B2 (en) Vehicle travel control apparatus
Moreira et al. Path following control system for a tanker ship model
EP3022618B1 (en) Route planning
WO2021230356A1 (ja) 船舶の自動誘導方法、船舶の自動誘導プログラム、船舶の自動誘導システム、及び船舶
KR101764600B1 (ko) 무인항공기 기반의 선박 항해 방법 및 시스템
US20210206460A1 (en) Navigation scene analysis systems and methods
EP3022619B1 (en) Route planning
US11862026B2 (en) Marine propulsion control system and method with proximity-based velocity limiting
JP7170019B2 (ja) 追尾制御装置
EP2827210A1 (en) Route planning
WO2024034579A1 (ja) 制御装置
Paez et al. Implementation of an unmanned surface vehicle for environmental monitoring applications
Lee et al. Design and experiment of a small boat track-keeping autopilot
Tomera Path controller for ships with switching approach
US11631337B2 (en) Optimal-route generating system
Miotto et al. UUV on-board path planning in a dynamic environment for the Manta test vehicle
CN113093739A (zh) 一种多无人艇编队避碰的优化控制器及其结构、设计方法
Mehdi et al. Autonomous navigation of low cost underwater vehicle for shallow freshwater applications
WO2024101344A1 (ja) 制御システム、制御方法及び制御プログラム
CN115268459B (zh) 一种基于双喷水推进器无人艇自主靠泊控制方法
Reis et al. A ROS Implementation of the Situational Awareness and Maneuvering Systems for an Autonomous Marine Vessel
WO2023089843A1 (ja) 船舶用自動操舵装置
JP2023149910A (ja) 移動体制御装置及び移動体制御システム
WO2024018638A1 (ja) 地図生成装置及び運転支援システム
US20230205204A1 (en) Method for controlling a robot-aircraft and corresponding control system

Legal Events

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

Ref document number: 23852541

Country of ref document: EP

Kind code of ref document: A1