WO2020157862A1 - 障害物回避制御装置、障害物回避制御システム、障害物回避制御方法および記録媒体 - Google Patents
障害物回避制御装置、障害物回避制御システム、障害物回避制御方法および記録媒体 Download PDFInfo
- Publication number
- WO2020157862A1 WO2020157862A1 PCT/JP2019/003177 JP2019003177W WO2020157862A1 WO 2020157862 A1 WO2020157862 A1 WO 2020157862A1 JP 2019003177 W JP2019003177 W JP 2019003177W WO 2020157862 A1 WO2020157862 A1 WO 2020157862A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- obstacle
- control
- command value
- avoidance
- target device
- Prior art date
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
- B25J9/1666—Avoiding collision or forbidden zones
-
- 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
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/39—Robotics, robotics to robotics hand
- G05B2219/39091—Avoid collision with moving obstacles
-
- 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
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/40—Robotics, robotics mapping to robotics vision
- G05B2219/40476—Collision, planning for collision free path
Definitions
- the present invention relates to an obstacle avoidance control device, an obstacle avoidance control system, an obstacle avoidance control method, and a recording medium.
- a technology has been proposed for controlling a controlled device such as a robot to avoid obstacles.
- a controlled device such as a robot to avoid obstacles.
- the robot control device described in Patent Document 1 when an obstacle is present between the target position of the tip of the robot arm and the first position before the tip reaches the target position. , Set a convex trajectory to avoid obstacles. The tip of the arm moves according to the locus.
- the location where the robot may come into contact with an obstacle is not limited to the tip of the arm. It is preferable that obstacles can be avoided not only in the tip portion of the robot arm but also in various portions of the device to be controlled.
- One example of an object of the present invention is to provide an obstacle avoidance control device, an obstacle avoidance control system, an obstacle avoidance control method, and a recording medium that can solve the above problems.
- the obstacle avoidance control device is a control command value for a control target device, and satisfies a constraint condition including a sufficient condition for the control target device not to contact an obstacle, And, based on the result of processing by the avoidance command value calculation unit for obtaining an avoidance command value which is a control command value in which an evaluation value obtained by applying the control command value to an evaluation function is a control command value satisfying a predetermined end condition. And a device control unit that controls the device to be controlled.
- the obstacle avoidance control method is a control command value for a control target device, and satisfies a constraint condition including a sufficient condition for the control target device not to contact an obstacle, And, based on the result of the process in the step of obtaining the avoidance command value is an evaluation value that is a control command value that the evaluation value applying the control command value to the evaluation function, a predetermined end condition, Controlling the device to be controlled.
- the recording medium is a control command value for the control target device to the computer, satisfying a constraint condition including a sufficient condition for the control target device not to contact an obstacle, And, based on the result of the process in the step of obtaining the avoidance command value is an evaluation value that is a control command value that the evaluation value applying the control command value to the evaluation function, a predetermined end condition, It is a recording medium in which a program for executing the step of controlling the controlled device is recorded.
- the obstacle avoidance control device According to the obstacle avoidance control device, the obstacle avoidance control system, the obstacle avoidance control method, and the recording medium described above, various parts of the control target device can be controlled to avoid the obstacle.
- 6 is a flowchart showing an example of a processing procedure in which the obstacle avoidance control device according to the first embodiment calculates a change amount ⁇ B(x, u) of an interference function value.
- It is a schematic block diagram which shows the example of an apparatus structure of the obstacle avoidance control system which concerns on 2nd Embodiment.
- It is a figure which shows the example of the flow of the data in the obstacle avoidance control system which concerns on 2nd Embodiment.
- FIG. 1 is a schematic configuration diagram showing an example of a device configuration of an obstacle avoidance control system according to the first embodiment.
- the obstacle avoidance control system 1 includes an imaging device 100, an information acquisition device 200, a nominal (Nominal) control device 300, and an obstacle avoidance control device 400.
- the obstacle avoidance control system 1 controls the control target device 900.
- the obstacle avoidance control system 1 controls the control target device 900 such that the control target device 900 performs a desired operation and the control target device 900 does not come into contact with the obstacle.
- the desired operation here is an operation to achieve the target set in the controlled device 900.
- the contacting here is not limited to merely touching, but includes hitting.
- the contact of the controlled device 900 with the obstacle means that at least a part of the controlled device 900 contacts with at least a part of the obstacle.
- control target device 900 is a vertical articulated robot
- the control target of the obstacle avoidance control system 1 operates according to a control command value and may contact an obstacle. It can be any device.
- the control target device 900 may be an industrial robot other than the vertical articulated robot.
- control target device 900 may be a robot other than an industrial robot, such as a construction robot or a housework robot.
- the robot to be controlled is not limited to a specific application, and various robots accompanied by a change in shape can be an example of the control target device 900.
- the control target device 900 may be a moving body such as an automated guided vehicle or a drone. Further, the control target device 900 may be a device that operates autonomously as long as it can be controlled by the control command value.
- the obstacle here is an object with which the control target device 900 may come into contact. Obstacles are not limited to a particular type. For example, the obstacle may be a human, another robot, a surrounding wall or machine, a temporarily placed luggage, or a combination thereof. Further, the control target device 900 itself may be treated as an obstacle. For example, when the control target device 900 is a vertical articulated robot, and the robot arm and the pedestal part contact each other depending on its posture, the obstacle avoidance control system 1 treats the control target device 900 as an obstacle, so that the robot arm It is possible to avoid contact between the base and the pedestal part.
- the imaging device 100 images an obstacle.
- the image capturing apparatus 100 captures an image of the entire space in which the control target device 900 is operable so as to be included in the image capturing range, thereby capturing an image of the obstacle when it exists.
- the information acquisition device 200 identifies the position of the obstacle based on the captured image of the imaging device 100.
- a camera that can obtain three-dimensional information such as a depth camera (Depth Camera) or a stereo camera (Stereo Camera) may be used.
- the information acquisition device 200 may use a technique for obtaining three-dimensional information from a two-dimensional image, such as three-dimensional extension of OpenPose.
- the obstacle avoidance control system 1 may include a device for obtaining three-dimensional information such as 3D-LiDAR (3-Dimension Light Detection and Ranging) in addition to or instead of the imaging device 100. .. Further, the image pickup apparatus 100 may be composed of a plurality of cameras. The imaging device 100 may be movable. The imaging device 100 may indicate the situation of the space to be observed by voxel data.
- 3D-LiDAR 3-Dimension Light Detection and Ranging
- the information acquisition device 200 acquires sensing data from a sensor that observes the controlled device 900, such as a sensor included in the controlled device 900, and detects the position and operation of the controlled device 900.
- the sensor from which the information acquisition device 200 acquires sensing data is not limited to a specific type of sensor.
- the information acquisition device 200 acquires information on any one of a joint angle, a joint angular velocity, a joint velocity, and a joint acceleration of each joint of the control target device 900, or a combination thereof, from the sensing data. May be.
- the information acquisition device 200 generates and transmits position information of the control target device 900 and information indicating the movement of the control target device 900 based on the obtained information.
- the information acquisition device 200 may transmit the position information of the control target device 900 as voxel data.
- the obstacle avoidance control device 400 causes the surface of the control target device 900 and the position of the obstacle instead of one point. The relationship can be grasped, and the distance between the controlled device 900 and the obstacle can be grasped more accurately.
- the obstacle avoidance control device 400 can perform control for causing the control target device 900 to avoid the obstacle with higher accuracy.
- the information acquisition device 200 may transmit the coordinates of the representative point set in the control target device 900 as the position information of the control target device 900.
- the information acquisition device 200 transmits, for example, the velocity, acceleration, angular velocity, angular acceleration of the control target device 900, or a combination thereof as the information indicating the movement of the control target device 900.
- the information acquisition device 200 may transmit information indicating the movement of the surface of the control target device 900 as voxel data.
- the information acquisition device 200 may transmit data indicating the movement of the representative point of the control target device 900.
- the information acquisition device 200 may transmit a vector in which the generalized coordinates q of the control target device 900 and the generalized speed q′ are arranged.
- the information acquisition device 200 may transmit information indicating the movement of the actuator of the control target device, such as the angular velocity of the joint of the control target device.
- the position information of the controlled device 900 and the information indicating the movement of the controlled device 900 are collectively referred to as the state information of the controlled device 900.
- the information acquisition device 200 transmits the state information of the control target device to the nominal control device 300 and the obstacle avoidance control device 400.
- the information acquisition device 200 transmits the position information of the obstacle.
- the information acquisition device 200 may transmit the position information of the obstacle in the voxel data format.
- the obstacle avoidance control device 400 determines the positional relationship between the surface of the obstacle instead of one point and the control target device 900.
- the distance between the control target device 900 and the obstacle can be grasped more accurately.
- the obstacle avoidance control device 400 can perform control for causing the control target device 900 to avoid the obstacle with higher accuracy.
- the information acquisition device 200 may transmit the coordinates of the representative point set in the control target device 900 as the position information of the control target device 900.
- the information acquisition device 200 may transmit information indicating the movement of the obstacle in addition to the position information of the obstacle.
- the information acquisition device 200 may transmit, as the information indicating the movement of the obstacle, for example, the speed, acceleration, angular velocity, angular acceleration of the obstacle, or a combination thereof.
- the information acquisition device 200 may transmit the information indicating the movement of the surface of the obstacle as voxel data.
- the information acquisition device 200 may transmit data indicating the movement of the representative point of the obstacle.
- the information acquisition device 200 may transmit a vector in which the generalized coordinates q of the obstacle and the generalized speed q′ are arranged.
- the position information of the obstacle or the combination of the position information of the obstacle and the information indicating the movement of the obstacle when the obstacle moves is referred to as obstacle state information.
- the information acquisition device 200 transmits the obstacle state information to the obstacle avoidance control device 400.
- the nominal control device 300 calculates the nominal command value.
- the nominal command value is a control command value for the control target device 900 when the obstacle avoidance by the control target device 900 is not considered. That is, the nominal command value is a control command value for the control target device 900 for achieving the target set in the control target device 900 under the assumption that there is no obstacle.
- the control method for the nominal control device 300 to calculate the nominal command value is not limited to a specific one, and various known control methods can be used.
- the nominal command value calculated by the nominal control device 300 serves as a reference for the obstacle avoidance control device 400 to acquire the control command value (that is, the control command value actually used) instructed to the control target device 900. Is used as the control command value.
- the obstacle avoidance control device 400 executes control of the control target device 900 by the obstacle avoidance control system 1. Therefore, as described above regarding the obstacle avoidance control system 1, the obstacle avoidance control device 400 causes the control target device 900 to perform a desired operation, and the control target device 900 does not contact the obstacle. The device 900 is controlled. The obstacle avoidance control device 400 acquires a control command value for controlling the control target device 900. The obstacle avoidance control device 400 controls the control target device 900 by transmitting the acquired control command value to the control target device 900.
- the obstacle avoidance control device 400 uses the control command value calculated by the nominal control device 300 as the control command value for the control target device 900. To use. This is because it is expected that the control target device 900 will not come into contact with the obstacle even if the control command value calculated by the nominal control device 300 and that does not consider obstacle avoidance is used.
- the obstacle avoidance control device 400 does not need to newly calculate the control threshold value in consideration of obstacle avoidance, and in this respect, the load of the obstacle avoidance control device 400 is increased. Is light.
- the obstacle avoidance control device 400 calculates a control command value for the control target device 900 to avoid the obstacle and achieve the target, and transmits the calculated control command value to the control target device 900.
- FIG. 2 is a schematic block diagram showing an example of the functional configuration of the information acquisition device 200.
- the information acquisition device 200 includes a first communication unit 210, a first storage unit 280, and a first control unit 290.
- the first control unit 290 includes a state observing unit 291 and a three-dimensional measuring unit 292.
- the first communication unit 210 communicates with other devices.
- the first communication unit 210 receives sensing data from a sensor included in the control target device 900.
- the first communication unit 210 also receives data from the imaging device 100.
- the first communication unit 210 receives the image data of the captured image of the imaging device 100, and further, if there is additional information such as distance information in the depth direction, receives the additional information in addition to the image data.
- the first communication unit 210 also transmits the position information and operation information of the control target device 900 generated by the information acquisition device 200 to the nominal control device 300 and the obstacle avoidance control device 400.
- the first communication unit 210 also transmits the position information of the obstacle generated by the information acquisition device 200 to the obstacle avoidance control device 400.
- the first communication unit 210 transmits the motion information of the obstacle (for example, information on speed or acceleration) to the obstacle avoidance control device 400.
- the first storage unit 280 stores various data.
- the function of the first storage unit 280 is executed using a storage device included in the information acquisition device 200.
- the first control unit 290 controls each unit of the information acquisition device 200 and executes various processes.
- the function of the first control unit 290 is executed by a CPU (Central Processing Unit) included in the information acquisition device 200 by reading a program from the first storage unit 280 and executing the program.
- CPU Central Processing Unit
- the state observing unit 291 identifies the position and operation of the controlled device 900 based on the sensing data from the sensor of the controlled device 900.
- the state observing unit 291 calculates a coordinate value indicating the position of the specified control target device 900.
- the state observing unit 291 transmits data indicating the position and operation of the control target device 900 to the nominal control device 300 and the obstacle avoidance control device 400 via the first communication unit 210. That is, the state observing unit 291 transmits the state information of the control target device 900 to the nominal control device 300 and the obstacle avoidance control device 400 via the first communication unit 210.
- the three-dimensional measurement unit 292 identifies the position of the obstacle based on the data from the image pickup apparatus 100.
- the three-dimensional measurement unit 292 calculates a coordinate value indicating the specified position, and transmits the calculated data to the obstacle avoidance control device 400 via the first communication unit 210. Further, when the obstacle moves, the three-dimensional measurement unit 292 calculates the motion information (for example, the speed or acceleration information) of the obstacle, and the calculated data is subjected to the obstacle avoidance control via the first communication unit 210. It may be transmitted to the device 400. In this way, the three-dimensional measurement unit 292 transmits the obstacle state information to the obstacle avoidance control device 400 via the first communication unit 210.
- FIG. 3 is a schematic block diagram showing an example of the functional configuration of the obstacle avoidance control device 400.
- the obstacle avoidance control device 400 includes a second communication unit 410, a second storage unit 480, and a second control unit 490.
- the second storage unit 480 includes a structural information storage unit 481 and a dynamic model storage unit 482.
- the second controller 490 includes an interference function calculator 491 and a device controller 492.
- the device control unit 492 includes a nominal command value acquisition unit 493, an avoidance necessity determination unit 494, an avoidance command value calculation unit 495, and a virtual force field reflection unit 496.
- the second communication unit 410 communicates with other devices.
- the second communication unit 410 receives the status information of the control target device 900 and the status information of the obstacle transmitted by the information acquisition device 200.
- the second communication unit 410 receives the control command value calculated and transmitted by the nominal control device 300.
- the second communication unit 410 also transmits a control command value to the control target device 900.
- the obstacle avoidance control device 400 controls the control target device 900 by transmitting a control command value to the control target device 900.
- the second storage unit 480 stores various data.
- the function of the second storage unit 480 is executed using a storage device included in the obstacle avoidance control device 400.
- the structure information storage unit 481 stores the structure information of the control target device 900.
- the structural information of the controlled device 900 is information on the shape and operation of the controlled device 900, such as the length and thickness of the arm of the controlled device 900, the direction in which the joint moves, and the movable range.
- CAD Computer Aided Design
- the dynamic model storage unit 482 stores the dynamic model of the control target device 900.
- the dynamic model of the controlled device 900 simulates an operation when the controlled device 900 receives the state information of the controlled device 900 and the control command value and the controlled device 900 follows the control command value.
- the dynamic model may output position information of the predicted position of the controlled device 900 at a future time.
- the dynamic model may output the operation amount of the control target device 900. That is, the dynamic model may output the difference obtained by subtracting the current position from the future predicted position of the control target device 900.
- the dynamic model is a model in which the differential value or the difference of the state indicated by the state information x of the controlled device 900 is obtained with respect to the input of the control command value u, and may be, for example, a state space model.
- the space occupied by the controlled device 900 can be reduced. Can be specified.
- the structure information of the control target device 900 includes information such as the thickness of the arm
- the obstacle avoidance control device 400 allows the obstacle avoidance control device 400 not only to position the skeleton of the control target device 900 but also to measure the thickness of the arm. It is possible to acquire the position information in consideration of. That is, the obstacle avoidance control device 400 can grasp the position information of the control target device 900 as a position having a spatial spread.
- the obstacle avoidance control device 400 can grasp the operation of the control target device 900 more accurately by using the dynamic model of the control target device 900. This makes it possible to determine whether or not the control target device 900 contacts an obstacle, and how the control target device 900 moves and contacts when contacting, and the control target device 900 can avoid obstacles with higher accuracy. Can be done.
- the second control unit 490 controls each unit of the obstacle avoidance control device 400 and executes various processes.
- the function of the second control unit 490 is executed by the CPU included in the obstacle avoidance control device 400 by reading the program from the second storage unit 480 and executing the program.
- the interference function calculation unit 491 calculates an interference function value and a value indicating a temporal change of the interference function value.
- the interference function indicates a value according to the positional relationship between the control target device 900 and the obstacle.
- the interference function B takes a value as in Expression (1).
- x indicates the status information of the control target device 900.
- the information acquisition device 200 transmits the position information of the surface of the control target device 900 as voxel data, and the interference function calculation unit 491 applies the state information of the control target device 900 to the interference function B to control the control target device 900.
- the distance between the device 900 and the obstacle at the closest position may be calculated.
- the interference function value B(x) indicates the distance between the position of the control target device 900 indicated by the state information x of the control target device 900 and the obstacle. When there are a plurality of obstacles, it indicates the distance to the obstacle closest to the position of the control target device 900. Normally, the control target device 900 does not enter the obstacle, so the interference function value B(x) when the control target device 900 is located inside the obstacle does not have to be defined.
- the interference function value B(x) indicates whether the control target device 900 is in contact with an obstacle and the distance between the control target device 900 and the obstacle.
- the interference function calculation unit 491 generates an interference function based on the obstacle state information, and stores it in the second storage unit 480.
- the interference function calculation unit 491 calculates the amount of change in the interference function value B(x) between the control steps as a value indicating the change over time in the interference function value B(x).
- the control step here is a series of processing steps for the obstacle avoidance control device 400 to transmit the control command value once to the control target device 900. That is, the obstacle avoidance control device 400 transmits the control command value to the control target device 900 at each periodic control step.
- the interference function calculation unit 491 predicts the amount of change in the interference function value B(x) from the current control step to the next control step.
- the change amount of the interference function value between the control steps is expressed as ⁇ B(x,u).
- the change amount of the interference function value B(x) depends on the change of the position of the control target device 900, and the change of the position of the control target device 900 depends on the control command value u. Therefore, the control command value u is specified. ..
- the interference function calculation unit 491 calculates the potential function value P(x) with respect to the position of the control target device 900.
- the potential function value P(x) is a virtual force field value based on the distance between the control target device 900 and the obstacle.
- the interference function calculation unit 491 determines that the potential function value P(x) is the closer the position of the controlled device 900 indicated by the state information of the controlled device 900 is to the obstacle (therefore, the smaller the interference function value B(x) is). Is calculated to be large.
- the device control unit 492 controls the control target device 900 based on one of the nominal command value from the nominal control device 300 and the result of the processing by the avoidance command value calculation unit 495.
- the nominal command value is a control command value that does not consider obstacle avoidance.
- the avoidance command value calculation unit 495 obtains a control command value such that the control target device 900 can avoid an obstacle.
- the device control unit 492 When it is determined that the distance between the control target device 900 and the obstacle is larger than the predetermined threshold, the device control unit 492 reflects the virtual force field on the nominal command value from the nominal control device 300 or on the nominal command value.
- the control command value corrected by the unit 496 is transmitted to the control target device 900 via the second communication unit 410.
- the avoidance command value calculation unit 495 attempts to calculate the control command value that can avoid the obstacle.
- the device control unit 492 causes the device control unit 492 to calculate the control command value or the virtual force field reflection unit for the control command value.
- the control command value corrected by 496 is transmitted to the control target device 900 via the second communication unit 410.
- the avoidance command value calculation unit 495 fails to calculate the control command value, that is, when the control command value that can avoid the obstacle cannot be obtained, the device control unit 492 controls the device 900 to be decelerated.
- the command value is transmitted to the control target device 900 via the second communication unit 410.
- the nominal command value acquisition unit 493 acquires the nominal command value. Specifically, the nominal command value acquisition unit 493 extracts the nominal command value from the nominal control device 300 from the reception signal of the second communication unit 410.
- the avoidance necessity determination unit 494 determines whether or not the avoidance command value calculation unit 495 needs to perform the process for acquiring the control command value based on the positional relationship between the control target device 900 and the obstacle. As described above, the avoidance necessity determination unit 494 compares the distance between the control target device 900 and the obstacle with the threshold, and determines whether the distance is larger than the threshold. Specifically, the avoidance necessity determination unit 494 compares the interference function value B(x) with a threshold value ⁇ of a predetermined constant, and determines whether B(x)> ⁇ .
- the avoidance necessity determination unit 494 determines that the avoidance command value calculation unit 495 does not need to perform the process for acquiring the control command value.
- the avoidance necessity determination unit 494 determines that the avoidance command value calculation unit 495 needs to perform the process for acquiring the control command value.
- the avoidance command value calculation unit 495 obtains the avoidance command value.
- the avoidance command value is a control command value for the control target device 900, satisfies constraint conditions including a sufficient condition for the control target device 900 not to contact an obstacle, and applies the control command value to the evaluation function.
- the evaluation value is a control command value that satisfies a predetermined end condition.
- the obstacle avoidance control device 400 controls the control target device 900 using the avoidance command value, so that the control target device 900 can be controlled so as not to contact the obstacle.
- the avoidance command value calculation unit 495 obtains the avoidance command value by using an ending condition that the degree of difference between the control command value as the solution to be obtained and the nominal command value satisfies a predetermined condition. For example, the avoidance command value calculation unit 495 solves the avoidance command value by solving an optimization problem that minimizes the degree of difference between the control command value and the nominal command value as the solution to be obtained under the above constraint conditions. Ask.
- the evaluation function (also referred to as the objective function) in the optimization problem solved by the avoidance command value calculation unit 495 is represented by, for example, Expression (2).
- “U * ” indicates a control command value that is a solution to this optimization problem.
- "Argmin” is a function that minimizes the value of the argument.
- “argmin” has a function value that is a value of u that minimizes the argument “(u ⁇ u r ) T (u ⁇ u r )”.
- the superscript "T” on a vector or matrix indicates the transpose of the vector or matrix.
- “U r ” represents the nominal command value from the nominal control device 300.
- Expression (2) indicates that a control command value that is as close as possible to the nominal command value u r is obtained under the constraint condition. Since the nominal command value u r is a command value calculated to cause the controlled device 900 to execute the purpose set in the controlled device 900, by obtaining a command value close to the nominal command value u r , the controlled object It is expected that the control target device 900 can execute the purpose set in the device 900.
- U indicating the control command value *, u, data format u r is assumed to be a vector of the same dimension. The number of dimensions of these vectors is the same as the number of dimensions of the control command value transmitted by the obstacle avoidance control device 400 to the control target device 900.
- Expression (2) corresponds to an example of the evaluation function.
- the control command value u that is the minimum solution in Expression (2) corresponds to an example of the control command value in which the evaluation value obtained by applying the control command value to the evaluation function satisfies the predetermined end condition.
- Constraint conditions in the minimization problem solved by the avoidance command value calculation unit 495 are represented by two types of expressions.
- the first of the two types of equations is expressed as in equation (3).
- ⁇ is a constant satisfying 0 ⁇ 1.
- ⁇ it is possible to adjust how much the margin of the interval between the control target device 900 and the obstacle is expected so that the control target device 900 and the obstacle do not come into contact with each other.
- B(x) indicates the distance between the control target device 900 and the obstacle.
- (1- ⁇ )B(x) is because the control target device 900 and the obstacle do not come into contact with each other.
- the margin is excluded from the operable range of the control target device 900.
- the larger the value of ⁇ is set the wider the operable range of the controlled device 900 becomes.
- the smaller the value of ⁇ is set the larger the margin for preventing the control target device 900 from coming into contact with the obstacle, and the control target device 900 is pushed toward the obstacle by an unexpected external force, for example. Even if the control target device 900 is hit, it is difficult for the control target device 900 to hit an obstacle.
- the avoidance command value calculation unit 495 obtains the avoidance command value using the interference function value and the value indicating the change over time of the interference function value.
- the constraint condition of Expression (3) can be set for each obstacle.
- the obstacle avoidance control device 400 can perform control so that the control target device 900 does not contact the entire obstacle.
- the interference function may be designed for a group of obstacles.
- Expression (3) shows a sufficient condition that the controlled device 900 does not contact the obstacle in the next control step as long as the controlled device 900 does not contact the obstacle in the current control step.
- the present control step is indicated by t
- the control step next to the control step t is indicated by t+1.
- the interference function value at the control step t is expressed as B(x t ).
- the interference function value at the control step t+1 is expressed as B(x t+1 ).
- the difference obtained by subtracting B(x t ) from B(x t+1 ) is expressed as ⁇ B(x t , u t ).
- ⁇ B(x t , u t ) is expressed as in Expression (4).
- Equation (5) can be obtained from Equation (3).
- Expression (6) can be obtained from Expression (4) and Expression (4).
- the avoidance command value calculation unit 495 can more accurately determine whether the condition of Expression (3) is satisfied.
- the avoidance command value calculation unit 495 uses the interference function B to determine that the control target device 900 and the obstacle are the best. You can get the distance between them in close proximity.
- the avoidance command value calculation unit 495 can determine the condition more accurately by determining the condition of Expression (3) using this distance. In this respect, it is expected that the avoidance command value calculation unit 495 can obtain a more accurate control command value, and the device control unit 492 can control the control target device 900 with higher accuracy.
- the information acquisition device 200 uses the voxel data as the position information of the surface of the control target device 900 to avoid an obstacle.
- An example is a case of transmitting to the control device 400.
- the information acquisition device 200 sends the position information of the surface of the obstacle, for example, voxel data to the obstacle avoidance control device 400.
- the interference function calculation unit 491 transmits the information and then uses the position information to generate an interference function.
- the conditional expression of the expression (3) can be provided for each reference point.
- the obstacle avoidance control device 400 can relatively easily cope with the case where there are a plurality of reference points. Even when there are a plurality of obstacles, the conditional expression (3) can be provided for each obstacle. In this respect, the obstacle avoidance control device 400 can relatively easily cope with a plurality of obstacles.
- u i (i is an integer of 1 ⁇ i ⁇ N) is a scalar value indicating a control command value for each movable part of the control target device 900, such as each joint of the control target device 900.
- N indicates the number of movable parts of the control target device 900.
- i is an identification number for identifying a movable part. The movable part identified by the identification number i is referred to as the i-th movable part. Therefore, u i is a control command value for the i-th movable part.
- u i — min and u i — max are a lower limit value and an upper limit value of u i that are predetermined by the specifications of the control target device 900, respectively.
- Expression (7) shows the constraint condition that any control command value is set within the range of the upper and lower limit values defined by the specifications of the movable part.
- the specifications of the movable part are defined by, for example, the specifications of the actuator used for the movable part.
- the avoidance command value calculation unit 495 obtains the avoidance command value when the avoidance necessity determination unit 494 determines that the process for acquiring the avoidance command value is necessary.
- the virtual force field reflection unit 496 reflects the operation of the controlled device 900 moving away from the obstacle as the device 900 is closer to the obstacle, in at least one of the nominal command value and the avoidance command value.
- the closer the control target device 900 is to the obstacle, the farther the control target device 900 is from the obstacle may be that the control target device 900 is farther from the obstacle with a larger acceleration.
- the closer the control target device 900 is to the obstacle, the further away from the obstacle may be that the closer the control target device 900 is to the obstacle, the longer the distance from the obstacle is.
- the closer the control-target device 900 is to the obstacle the more the control-target device 900 moves away from the obstacle only when the distance between the control-target device 900 and the obstacle is equal to or less than a predetermined threshold value. It may be.
- the virtual force field reflection unit 496 may simulate a virtual force field based on the positional relationship between the control target device 900 and the obstacle. In this case, the virtual force field reflection unit 496 may calculate a larger repulsive force as the distance between the control target device 900 and the obstacle is shorter. The greater the repulsive force generated in the controlled device 900, the greater the acceleration of the controlled device 900, and the greater the amount of movement of the controlled device 900 away from the obstacle.
- the obstacle may be represented by a mass point (Point Mass), and the coordinates of the obstacle may be (x 0 , y 0 , z 0 ). Then, based on the virtual force field reflecting unit 496, the coordinates (x, y, z) with respect to the control target device 900 located in the control command value u p for go away from the obstacle, the equation (8) You may make it calculate by.
- P(x, y, z) indicates a potential function. ⁇ indicates Nabla. P 0 (x, y, z) is expressed as in Expression (9).
- the virtual force field reflection unit 496 may calculate the movement amount of the control target device 900 as a command value for one representative point such as the tip of the robot arm, and distribute the calculated movement amount to each joint. Good.
- the virtual force field reflection unit 496 may calculate the instructed movement amount for each of the plurality of parts of the control target device 900 such as each joint of the robot.
- the virtual force field reflection unit 496 By causing the virtual force field reflection unit 496 to reflect the calculated command value on the control command value, it is possible to more reliably prevent contact between the control target device 900 and the obstacle.
- the virtual force field reflection unit 496 reflects the calculated command value on both the nominal command value and the avoidance command value will be described as an example.
- the processing of the virtual force field reflection unit 496 is not essential for the obstacle avoidance control device 400. Therefore, the obstacle avoidance control device 400 may not have the virtual force field reflection unit 496.
- FIG. 4 is a diagram showing an example of a data flow in the obstacle avoidance control system 1.
- the obstacle is denoted by reference numeral 950.
- the obstacle 950 is similar to the obstacle described above.
- the sensing data of the sensor of the control target device 900 is input to the state observation unit 291 of the information acquisition device 200.
- the image data of the imaging device 100 is input to the three-dimensional measuring unit 292 of the information acquisition device 200.
- the additional information of the imaging device 100 is present, the additional information is also input to the three-dimensional measuring unit 292 in addition to the image data.
- the information acquisition device 200 transmits the state information of the control target device 900 calculated by the state observation unit 291 to the nominal control device 300 and the obstacle avoidance control device 400. For example, the information acquisition device 200 transmits the position information of the surface of the control target device 900 as voxel data. In addition, the information acquisition device 200 transmits information indicating the operation of the control target device 900, such as the velocity, acceleration, angular velocity or angular acceleration of the surface or reference point of the control target device 900. For example, the information acquisition device 200 may transmit a vector in which the generalized coordinates q and the generalized speed q′ are arranged.
- the information acquisition device 200 also transmits the state information of the obstacle 950 to the obstacle avoidance control device 400.
- the information acquisition device 200 transmits the three-dimensional position information of the obstacle 950 to the obstacle avoidance control device 400.
- the data format in which the information acquisition device 200 transmits the position information of the obstacle 950 is not limited to a particular one.
- the information acquisition device 200 may transmit the position information of the obstacle 950 as Voxel data.
- the information acquisition device 200 may transmit the position information of the obstacle 950 as surface data.
- the information acquisition device 200 may transmit the position information of the obstacle 950 as point cloud data.
- the nominal control device 300 calculates the nominal command value u r using the state information of the control target device 900 received from the information acquisition device 200.
- the nominal control device 300 transmits the calculated nominal command value u r to the obstacle avoidance control device 400.
- the interference function calculation unit 491 of the obstacle avoidance control device 400 is based on the state information of the control target device 900 and the obstacle state information obtained from the information acquisition device 200, and the control command value set by the device control unit 492. Then, the interference function value B(x), the change amount ⁇ B(x, u) of B(x) between the control steps, and the potential function value P(x) are calculated. As described above, the processing of the virtual force field reflection unit 496 is not essential. Therefore, the calculation of the potential function value by the interference function calculation unit 491 is not essential.
- the device control unit 492 of the obstacle avoidance control device 400 receives the nominal command value u r obtained from the nominal control device 300, the interference function value B(x) obtained from the interference function calculation unit 491, and the change amount ⁇ B of the interference function value.
- the control command value u for the control target device 900 is acquired using (x, u) and the potential function value P(x).
- the device control unit 492 controls the control target device 900 by transmitting the acquired control command value u to the control target device 900 via the second communication unit 410.
- FIG. 5 is a flowchart showing an example of a processing procedure in which the obstacle avoidance control device 400 acquires a control command value for the control target device 900.
- the obstacle avoidance control device 400 performs the process of FIG. 5 for each control step of control of the control target device 900.
- virtual force field reflecting unit 496 the interference function calculator 491 is based on the potential function value P (x) of calculating calculates a control command value u p to simulate the force field (step S111).
- Control command value u p is used as a correction term for adding a value to the nominal command or avoidance command value.
- the avoidance necessity determination unit 494 determines whether or not the interference function value B(x) calculated by the interference function calculation unit 491 is larger than the threshold value ⁇ (step S112).
- the device control unit 492 sets the control command value u for the control target device 900 to the nominal command value u r.
- the device control unit 492 controls the control target device 900 by transmitting the control command value u to the control target device 900 via the second communication unit 410 (step S161).
- the obstacle avoidance control device 400 ends the process of FIG.
- step S112 determines that the interference function value B(x) is equal to or less than the threshold value ⁇ in step S112 (step S112: NO)
- the avoidance command value calculation unit 495 causes the optimization problem described above. An attempt is made to obtain a control command value by calculation (step S131).
- the device controller 492 determines whether the avoidance command value calculator 495 has obtained a solution to the optimization problem in step S131 (step S132).
- step S132 when it is determined in step S132 that the solution to the optimization problem has not been obtained (step S132: NO), the device control unit 492 generates the control command value u so as to decelerate the control target device 900. Yes (step S151). After step S151, the process proceeds to step S161.
- FIG. 6 is a flowchart showing an example of a processing procedure in which the obstacle avoidance control device 400 calculates a value related to the interference function.
- the obstacle avoidance control device 400 performs the process of FIG. 6 as a pre-process of the process of FIG. 5 for each control step of control of the control target device 900.
- the interference function calculation unit 491 uses the information of the control target device 900 transmitted by the information acquisition device 200, the structure information of the control target device 900 stored in the structure information storage unit 481, and the like.
- the position information of the occupied space of the controlled device 900 is calculated (step S211).
- the interference function calculation unit 491 calculates the position information of the occupied space of the control target device 900 by the coordinate value of the control target device 900 in the coordinate system.
- the coordinate system of the control target device 900 here is, for example, a coordinate value used for a control command value transmitted by the obstacle avoidance control device 400 to the control target device 900.
- the interference function calculation unit 491 acquires the position information of the obstacle (step S212).
- the three-dimensional measurement unit 292 may detect the position of the obstacle and generate position information in the coordinate system of the control target device 900, and the interference function calculation unit 491 may use the position information.
- the three-dimensional measuring unit 292 generates the position information of the obstacle with the coordinate value of the coordinate system different from the coordinate system of the control target device 900, and the interference function calculating unit 491 determines the coordinate system of the control target device 900. You may make it coordinate-convert into a coordinate value.
- the interference function calculation unit 491 calculates the interference function value B(x) based on the position information of the control target device 900 and the position information of the obstacle (step S213). Further, the interference function calculation unit 491 calculates the change amount ⁇ B(x, u) of the interference function value based on the position information of the control target device 900, the operation information of the control target device 900, and the position information of the obstacle. Yes (step S214).
- the interference function calculation unit 491 may use the operation information of the control target device 900 calculated from the sensing data of the sensor of the control target device 900 by the state observation unit 291 as the operation information of the control target device 900. Alternatively, the interference function calculation unit 491 may use the control command value u to the control target device 900 in the previous control step as the operation information of the control target device 900. Alternatively, the interference function calculation unit 491 may use both of them as the operation information of the control target device 900. After step S214, the obstacle avoidance control device 400 ends the process of FIG.
- FIG. 7 is a flowchart showing an example of a processing procedure in which the obstacle avoidance control device 400 calculates the change amount ⁇ B(x, u) of the interference function value.
- the obstacle avoidance control device 400 performs the process of FIG. 7 in step S214 of FIG.
- the interference function calculation unit 491 determines whether ⁇ B(x,u) can be calculated by the calculation of the dynamic model (step S221). When it is determined that the calculation is possible (step S221; YES), the interference function calculation unit 491 calculates ⁇ B(x, u) by the calculation of the dynamic model (step S231). After step S231, the obstacle avoidance control device 400 ends the process of FIG. 7.
- step S221 when it is determined in step S221 that ⁇ B(x,u) cannot be calculated by the calculation of the dynamic model (step S221: NO), the interference function calculation unit 491 uses ⁇ B using a minute change in the control command value u. Calculate (x, u). For example, the interference function calculation unit 491 approximately calculates ⁇ B(x, u) using the equation (10).
- ⁇ t indicates a time interval between control steps.
- B(x,u) indicates an interference function value.
- the process of step S231 does not need to use a trial using a minute change of u and the approximation as in Expression (10).
- the processing of step S231 is expected to have a higher accuracy of the obtained ⁇ B(x, u) than the processing of step S241.
- the interference function calculation unit 491 determines whether or not ⁇ B(x,u) can be calculated in the process of step S231, and if it can be calculated, performs the process of step S231.
- the interference function calculation unit 491 performs the process of step S241.
- the nominal command value acquisition unit 493 acquires the nominal command value.
- the nominal command value is a so-called reference control command value that is used to obtain a command value for the control target device 900.
- the avoidance command value calculation unit 495 obtains the avoidance command value.
- the avoidance command value satisfies a constraint condition including a sufficient condition for the controlled device 900 not to come into contact with an obstacle, as in Expression (3), and is different from the nominal command value as in Expression (2).
- the device control unit 492 controls the control target device 900 based on either the nominal command value or the result of the processing by the avoidance command value calculation unit 495.
- a constraint condition for avoiding contact between the control target device 900 and an obstacle can be provided.
- a constraint condition can be set for each reference point.
- the reference point for controlling the control target device 900 so as to avoid the obstacle is not limited to one place, and the obstacle is avoided for various parts of the control target device 900.
- a constraint condition can be set for each obstacle. In this respect, according to the obstacle avoidance control device 400, it is possible to cope with a plurality of obstacles relatively easily.
- the nominal command value acquisition unit 493 acquires the nominal command value as the nominal command value.
- the nominal command value is a control command value when the obstacle avoidance by the controlled device 900 is not considered.
- the nominal command value is a command value set to achieve the target set in the control target device 900, and the obstacle avoidance control device 400 sets the avoidance command value considering obstacle avoidance as the nominal command value. It is expected that the goal can be achieved by generating them close to each other.
- the nominal command value can be used as a reference when generating the avoidance command value, and in this respect, the avoidance command value can be generated relatively easily.
- the interference function calculation unit 491 calculates an interference function value (B(x)) and a value ( ⁇ B(x, u)) indicating a change with time of the interference function value.
- the interference function value indicates a value according to the positional relationship between the control target device 900 and the obstacle.
- the avoidance command value calculation unit 495 obtains the avoidance command value using the interference function value and a value indicating a change over time in the interference function value.
- the obstacle avoidance control device 400 by using these values, it is possible to estimate the degree of possibility that the control target device 900 will contact an obstacle, and take measures such as obstacle avoidance as necessary. Can be taken.
- the distance between the control target device 900 and the obstacle can be used as an interference function, and the function value can be easily calculated from the position of the control target device 900 and the position of the obstacle.
- the load on the obstacle avoidance control device 400 can be light.
- the avoidance necessity determination unit 494 determines whether or not the process for acquiring the avoidance command value is necessary based on the positional relationship between the control target device 900 and the obstacle.
- the avoidance command value calculation unit 495 obtains the avoidance command value when the avoidance necessity determination unit 494 determines that the process for acquiring the avoidance command value is necessary.
- the avoidance necessity determination unit 494 determines that the process for acquiring the avoidance command value is unnecessary, the nominal command value generated by the nominal control device 300 can be used as the command value to the control target device 900, The obstacle avoidance control device 400 does not need to generate a new command value. In this respect, the load on the obstacle avoidance control device 400 is relatively light.
- the device control unit 492 decelerates the control target device 900.
- the avoidance command value calculation unit 495 fails to acquire the avoidance command value, there is a possibility that the contact between the control target device 900 and the obstacle cannot be avoided. Therefore, the device control unit 492 decelerates the control target device 900. As a result, it is possible to secure a time until the control target device 900 comes into contact with the obstacle, and it is possible to handle the obstacle manually, for example. Further, even when the controlled device 900 contacts an obstacle, it is expected that the controlled device 900 is decelerating, so that no trouble occurs or the degree of the trouble is light.
- the virtual force field reflection unit 496 reflects the operation of the controlled device 900 moving away from the obstacle with a greater acceleration as the device 900 is closer to the obstacle, in at least one of the nominal command value and the avoidance command value.
- the device control unit 492 controls the control target device 900 based on one of the nominal command value in which this operation is reflected and the avoidance command value in which this operation is reflected.
- the controlled device 900 operates such that the closer it is to the obstacle, the farther it is from the obstacle. According to the obstacle avoidance control device 400, it is possible to make the control target device 900 avoid the obstacle more reliably in this respect.
- the avoidance command value calculation unit 495 obtains, for each reference point set in the control target device 900, an avoidance command value that satisfies a sufficient condition that the reference point does not contact an obstacle.
- the equation of the constraint condition can be provided for each reference point. According to the obstacle avoidance control device 400, in this respect, when a plurality of reference points are set in the control target device, it can be relatively easily handled.
- the avoidance command value calculation unit 495 obtains, for each obstacle, an avoidance command value that satisfies a sufficient condition for the controlled device 900 not to contact the obstacle.
- the equation of the constraint condition can be provided for each obstacle. According to the obstacle avoidance control device 400, in this respect, when there are a plurality of obstacles, it is possible to cope with them relatively easily.
- the avoidance command value calculation unit 495 is determined based on the distance between the control target device 900 and the obstacle at the position where the control target device 900 and the obstacle are closest to each other, and the control target device 900 does not contact the obstacle.
- the avoidance command value that more accurately reflects the possibility of contact between the obstacle and the obstacle can be obtained.
- the obstacle avoidance control device 400 can control the control target device 900 with relatively high accuracy.
- FIG. 8 is a schematic configuration diagram showing an example of the apparatus configuration of the obstacle avoidance control system according to the second embodiment.
- the obstacle avoidance control system 2 includes an imaging device 100, an information acquisition device 200, a nominal control device 300, an obstacle avoidance control device 400, and an obstacle position identifying device 500. Further, the obstacle avoidance control system 2 controls the control target device 900.
- the obstacle avoidance control system 2 is different from the obstacle avoidance control system 1 in that it includes an obstacle position identifying device 500. In other respects, the obstacle avoidance control system 2 is similar to the obstacle avoidance control system 1. Regarding the obstacle avoidance control system 2, the description of the same points as the obstacle avoidance control system 1 is omitted, and the reference numerals shown in FIG. 2 and the reference numerals shown in FIG. 3 are cited as necessary.
- FIG. 9 is a schematic block diagram showing an example of the functional configuration of the obstacle position identifying device 500.
- the obstacle position identifying device 500 includes a third communication unit 510, a third storage unit 580, and a third control unit 590.
- the third storage unit 580 includes an object information storage unit 581.
- the third control unit 590 includes a type identifying unit 591 and a position specifying unit 592.
- the third communication unit 510 communicates with other devices.
- the third communication unit 510 receives the position information of the obstacle from the information acquisition device 200.
- the third communication unit 510 transmits the position information of the obstacle, which reflects the processing of the position specifying unit 592, to the obstacle avoidance control device 400.
- the third communication unit 510 may transmit the position information of the obstacle calculated by the position specifying unit 592 to the obstacle avoidance control device 400 in addition to the information received from the information acquisition device 200.
- the third communication unit 510 transmits, to the obstacle avoidance control device 400, information obtained by replacing the position information calculated by the position specifying unit 592 with the position information of the obstacle with respect to the information received from the information acquisition device 200. You may do so.
- the third storage unit 580 stores various data.
- the function of the third storage unit 580 is executed using a storage device included in the obstacle position identifying device 500.
- the object information storage unit 581 stores the object information.
- the object information storage unit 581 stores, as the object information, information in which the type identification information for identifying the type of the object and the information for specifying the position of the object such as the shape and the size of the object are associated with each other.
- the object information storage unit 581 may store object information for each name of an object such as a plastic bottle and a cup.
- the object information storage unit 581 may store object information for each shape of the object such as a cylindrical object and a circular object.
- the object identification information may include the parameter, such as the capacity of the plastic bottle being a parameter when the object identification information includes the information of the plastic bottle.
- the third control unit 590 controls each unit of the obstacle position identifying device 500 to execute various processes.
- the function of the third control unit 590 is executed by the CPU included in the obstacle locating device 500 by reading the program from the third storage unit 580 and executing the program.
- the type identifying unit 591 identifies the type of the obstacle object based on the information on the obstacle calculated by the three-dimensional measuring unit 292.
- the type identifying unit 591 extracts the object in the obstacle by dividing the obstacle indicated by the information calculated by the three-dimensional measuring unit 292 into physically integrated portions. May be. Then, the type identifying unit 591 may specify the type of the object by performing pattern matching for each of the extracted objects.
- the type identifying unit 591 may be configured as an object detector using a neural network, such as YOLO (You Only Look Once) or SSD (Single Shot Multibox Detector).
- the position specifying unit 592 specifies the position of the obstacle object based on the information about the obstacle calculated by the three-dimensional measuring unit 292 and the object information stored in the object information storage unit 581. For example, the position identifying unit 592 reads information on the position and size of the object from the object information stored in the object information storage unit 581 based on the type of the object identified by the type identifying unit 591. Then, the position identifying unit 592 calculates the position information of the object, assuming that the position of the object indicated by the information about the obstacle calculated by the three-dimensional measuring unit 292 is the position and the size read from the object information. To do. The position specifying unit 592 may calculate the position of the object in the form of coordinate values.
- the position specifying unit 592 recognizes the obstacle as an object having a three-dimensional spread and calculates the coordinates thereof. For example, when the imaging device 100 images an obstacle from one side and an image of the opposite side is not obtained, the position identifying unit 592 calculates the position information of the obstacle that is not included in the image. You can
- the position specifying unit 592 transmits the calculated position information for each object to the obstacle avoidance control device 400 via the third communication unit 510.
- the third communication unit 510 may transmit the position information for each object calculated by the position specifying unit 592 to the obstacle avoidance control device 400 in addition to the information received from the information acquisition device 200. Good.
- the third communication unit 510 transmits, to the obstacle avoidance control device 400, information obtained by replacing the position information calculated by the position specifying unit 592 with the position information of the obstacle with respect to the information received from the information acquisition device 200. You may do so.
- FIG. 10 is a diagram showing an example of a data flow in the obstacle avoidance control system 2. Comparing FIG. 10 with FIG. 4, the obstacle avoidance control system 2 of FIG. 10 is different from the obstacle avoidance control system 1 of FIG. 4 in that an obstacle position specifying device 500 is provided. Due to this difference, in FIG. 4, the information acquisition device 200 transmits the information on the obstacle calculated by the three-dimensional measurement unit 292 to the obstacle avoidance control device 400, whereas in FIG. The information about the obstacle calculated by the three-dimensional measuring unit 292 is transmitted to the obstacle position identifying device 500.
- the obstacle position identifying device 500 replaces the information received from the information acquisition device 200 with the information obtained by replacing the position information of the obstacle with the position information calculated by the position identifying unit 592.
- the interference function calculation unit 491 uses the information calculated by the state observation unit 291 of the information acquisition device 200 and the information from the obstacle position specifying device 500 to determine the interference function value and the interference function value. The difference and the potential function value are calculated.
- FIG. 10 is the same as the case of FIG.
- the type identifying unit 591 identifies the type of the obstacle object.
- the position identifying unit 592 reads information according to the type identified by the type identifying unit 591 from the object information indicating the size and shape of the object for each type of object, and uses the read information as an obstacle. Identify the position of the object.
- the obstacle avoidance control system 2 can obtain the position information of the obstacle with higher accuracy.
- the obstacle position identifying device 500 can supplement the position information of the obstacle with respect to a portion that is not shown in the image of the imaging device 100.
- the obstacle avoidance control device 400 can calculate the command value for causing the control target device 900 to avoid the obstacle with higher accuracy.
- FIG. 11 is a schematic configuration diagram showing an example of the apparatus configuration of the obstacle avoidance control system according to the third embodiment.
- the obstacle avoidance control system 3 includes an imaging device 100, an information acquisition device 200, a nominal control device 300, an obstacle avoidance control device 400, an obstacle position identifying device 500, and an obstacle. And a motion prediction device 600. Further, the obstacle avoidance control system 3 controls the control target device 900.
- the obstacle avoidance control system 3 is different from the obstacle avoidance control system 2 in that it includes an obstacle motion prediction device 600. Otherwise, the obstacle avoidance control system 3 is similar to the obstacle avoidance control system 2. Regarding the obstacle avoidance control system 3, the description of the same points as the obstacle avoidance control system 2 is omitted, and the reference numerals shown in FIG. 2, the reference numerals shown in FIG. 3, and the reference numerals shown in FIG. ..
- FIG. 12 is a schematic block diagram showing an example of the functional configuration of the obstacle motion prediction device 600.
- the obstacle motion prediction device 600 includes a fourth communication unit 610, a fourth storage unit 680, and a fourth control unit 690.
- the fourth storage unit 680 includes an object movement information storage unit 681.
- the fourth control unit 690 includes a position prediction unit 691.
- the fourth communication unit 610 communicates with other devices.
- the obstacle position identifying device 500 transmits the information indicating the type of obstacle identified by the type identifying unit 591 and the position information of the obstacle calculated by the position identifying unit 592 in association with each other. To do.
- the fourth communication unit 610 receives the information transmitted by the obstacle location identifying device 500. Further, the fourth communication unit 610 transmits, to the obstacle avoidance control device 400, information obtained by adding the future position information of the obstacle predicted by the position prediction unit 691 to the information received from the obstacle position specifying device 500. ..
- the fourth storage unit 680 stores various data.
- the function of the fourth storage unit 680 is executed using a storage device included in the obstacle motion prediction device 600.
- the object motion information storage unit 681 stores object motion information.
- the object motion information storage unit 681 stores, as the object motion information, information in which the type identification information for identifying the type of the object and the information for predicting the motion of the object are associated with each other.
- the object motion information storage unit 681 may store the motion model of the object as the object motion information. Alternatively, the object motion information storage unit 681 may store history information of the position of the object detected as an obstacle as the object motion information.
- the fourth control unit 690 controls each unit of the obstacle motion prediction device 600 to execute various processes.
- the function of the fourth control unit 690 is executed by the CPU included in the obstacle motion prediction device 600 by reading the program from the fourth storage unit 680 and executing the program.
- the position prediction unit 691 identifies the future position of the obstacle object. For example, the position prediction unit 691 uses the identification information of the type of the object identified by the type identification unit 591 of the obstacle location identification device 500 to obtain information for predicting the action of the object from the object action information storage unit 681. read out. Then, the position prediction unit 691 predicts the amount of movement of the object between the control steps by using the read information. The position predicting unit 691 predicts the position of the object in the next control step by adding the predicted movement amount to the position of the object calculated by the position specifying unit 592 of the obstacle position specifying device 500.
- FIG. 13 is a diagram showing an example of a data flow in the obstacle avoidance control system 3. Comparing FIG. 13 with FIG. 10, the obstacle avoidance control system 3 of FIG. 13 differs from the obstacle avoidance control system 2 of FIG. 10 in that an obstacle motion prediction device 600 is provided. Due to this difference, in FIG. 10, the obstacle position specifying device 500 transmits information to the obstacle avoidance control device 400, whereas in FIG. 13, the obstacle position specifying device 500 causes the obstacle movement prediction device 600. Information is transmitted to the obstacle avoidance control device 400, and the obstacle motion prediction device 600 transmits information to the obstacle avoidance control device 400.
- the obstacle position identifying device 500 transmits, for each object, information indicating the type of the object and the position information of the object to the obstacle motion prediction device 600.
- the obstacle motion prediction device 600 transmits to the obstacle avoidance control device 400, for each object, information indicating the type of the object, the position information of the object, and the prediction information of the position of the object in association with each other. ..
- the interference function calculation unit 491 uses the information calculated by the state observation unit 291 of the information acquisition device 200 and the information from the obstacle motion prediction device 600 to determine the interference function value and the interference function value. The difference and the potential function value are calculated.
- FIG. 13 is similar to the case of FIG.
- the obstacle motion prediction device 600 reads the information corresponding to the identified type from the object motion information indicating the information regarding the movement of the object for each type of the object, and uses the read information to read the obstacle. Predict the future position of.
- the interference function calculation unit 491 of the obstacle avoidance control device 400 calculates a value indicating a change over time of the interference function value based on the predicted future position of the obstacle.
- the obstacle avoidance control system 3 can use the information about the motion of the obstacle, which is shown as a characteristic of the obstacle, to predict the future position of the obstacle.
- the position of can be predicted with relatively high accuracy.
- the obstacle avoidance control system 3 can avoid the obstacle of the control target device 900 with relatively high accuracy in that the prediction result of the future position of the obstacle is used for the obstacle avoidance of the control target device 900. ..
- FIG. 14 is a schematic configuration diagram showing an example of the apparatus configuration of the obstacle avoidance control system according to the fourth embodiment.
- the obstacle avoidance control system 4 includes an imaging device 100, an information acquisition device 200, and an obstacle avoidance control device 401. Further, the obstacle avoidance control system 2 controls the control target device 900.
- the obstacle avoidance control system 4 differs from the obstacle avoidance control device 400 in that it does not include the nominal control device 300 and includes an obstacle avoidance control device 401 instead of the obstacle avoidance control device 400.
- one of the constraint conditions used in the process of obtaining the avoidance command value includes a constraint condition for reflecting the target set in the control target device 900 in the control command value.
- the obstacle avoidance control system 4 is similar to the obstacle avoidance control system 1.
- description of the same points as the obstacle avoidance control system 1 will be omitted, and the reference numerals shown in FIG.
- the obstacle avoidance control system 4 may include the obstacle position specifying device 500 according to the second embodiment.
- the obstacle avoidance control system 4 may include an obstacle position specifying device 500 and an obstacle motion prediction device 600 according to the third embodiment.
- FIG. 15 is a schematic block diagram showing an example of the functional configuration of the obstacle avoidance control device 401.
- the obstacle avoidance control device 401 includes a second communication unit 410, a second storage unit 480, and a second control unit 490.
- the second storage unit 480 includes a structural information storage unit 481, a dynamic model storage unit 482, and a control function storage unit 483.
- the second controller 490 includes an interference function calculator 491 and a device controller 492.
- the device control unit 492 includes an avoidance command value calculation unit 495 and a virtual force field reflection unit 496.
- the second storage unit 480 includes the control function storage unit 483, and the device control unit 492 does not include the nominal command value acquisition unit 493 and the avoidance necessity determination unit 494. It is different from the obstacle avoidance control device 400. This difference is due to the difference in the method of reflecting the target set in the control target device 900 in the control command value.
- the avoidance command value calculation unit 495 causes the avoidance command value to approach the nominal command value by using the evaluation function, thereby reflecting the target set in the control target device 900 in the avoidance command value.
- one of the constraint conditions used by the avoidance command value calculation unit 495 in the process of obtaining the avoidance command value is to reflect the target set in the control target device 900 in the control command value. Includes constraints.
- the obstacle avoidance control device 401 is similar to the obstacle avoidance control device 400.
- the constraint conditions in the optimization problem solved by the avoidance command value calculation unit 495 in the fourth embodiment are represented by three types of expressions.
- the first type among the three types of formulas is a formula showing a sufficient condition for the controlled device 900 not to contact an obstacle, as in the case of the first embodiment.
- the first type among the three types of formulas is expressed as, for example, the above-mentioned formula (3).
- the second type among the three types of expressions is a constraint that the control command value is set within the range of the upper and lower limit values defined by the specifications of the movable part of the controlled device 900, as in the case of the first embodiment. It is an expression showing a condition.
- the second type among the three types of formulas is expressed as, for example, the above-mentioned formula (7).
- the third type of the three types of equations indicates a condition regarding the degree to which the target set in the control target device 900 is stably achieved by executing the control command.
- the Lyapunov function may be represented by V
- the amount of change in the Lyapunov function value between control steps may be represented by ⁇ V
- the third type of equation indicating the constraint condition may be expressed by equation (11).
- “D” is provided in order to ease the constraint condition and facilitate the solution.
- the solution is a control command value for strictly achieving the target set in the controlled device 900.
- the solution is searched pinpoint, and there is a concern that the solution cannot be obtained. Therefore, by setting d ⁇ 0, it is possible to allow the deviation between the operation result of the control target device 900 based on the control command value and the target and widen the search range of the solution.
- the deviation between the operation result of the controlled device 900 and the target is referred to as an error.
- the third type of equation indicating the constraint condition in the optimization problem solved by the avoidance command value calculation unit 495 in the fourth embodiment is not limited to the equation (11), and the degree of achievement of the target set in the control target device 900 , It is possible to use various expressions that can adjust the trade-off with the ease of obtaining the solution.
- the third type of expression indicating the constraint condition in the optimization problem solved by the avoidance command value calculation unit 495 in the fourth embodiment may be expressed by Expression (12).
- the function D is a function for calculating the Euclidean distance between the target value and the actual value set in the control target device 900. Assuming that the target value set in the control target device 900 is indicated by a vector, the avoidance command value calculation unit 495 calculates the function D and calculates the difference of 2 between the target value and the actual value for all the elements of the vector. The squares are added together and the square root is calculated.
- the function ⁇ D is a function for calculating the amount of change between the control steps of the Euclidean distance between the target value and the actual value set in the controlled device 900.
- the function value D(x) at the control step t is represented as D(x t )
- the function value D(x) at the control step t+1 is represented as D(x t+1 ).
- the formula (12) is a constraint formula in which the condition for guaranteeing the stability of the controlled device 900 is relaxed. For example, if the avoidance command value calculation unit 495 always obtains the control command u that satisfies ⁇ D(x, u) ⁇ 0, the controlled device 900 can stably achieve the target value.
- the objective function in the optimization problem solved by the avoidance command value calculation unit 495 in the fourth embodiment is represented by, for example, Expression (13).
- u T u indicates the square of the norm as a vector of the control command value u.
- the norm here is also referred to as Euclidean norm or Euclidean distance.
- u T u in Expression 13 is provided to reduce the magnitude of the control command value as much as possible so that the controlled device 900 does not perform unnecessary operation.
- Expression 13 is a term for evaluating the magnitude of “d” in Expression (11) or Expression (12).
- P of “p ⁇ d 2 ”denotes a weight for adjusting the weighting of “u T u” and “d 2 ”.
- P is set as a constant of p>0, for example.
- Expression (13) corresponds to an example of the evaluation function.
- the control command value u that is the minimum solution in Expression (13) corresponds to an example of the control command value in which the evaluation value obtained by applying the control command value to the evaluation function satisfies the predetermined end condition.
- FIG. 16 is a diagram showing an example of a data flow in the obstacle avoidance control system 4.
- the obstacle avoidance control system 4 does not include the nominal control device 300 in FIG. 16. Accordingly, in FIG. 16, the value regarding the control target from the information acquisition device 200 is input to the device control unit 492. In other respects, FIG. 16 is similar to the case of FIG.
- FIG. 17 is a flowchart showing an example of a processing procedure in which the obstacle avoidance control device 401 acquires a control command value for the control target device 900.
- the obstacle avoidance control device 401 performs the process of FIG. 17 for each control step of control of the control target device 900.
- the process in step S311 in FIG. 17 is the same as the process in step S111 in FIG.
- the avoidance command value calculation unit 495 attempts to acquire the control command value by calculating the optimization problem described above (step S312).
- the device control unit 492 determines whether the avoidance command value calculation unit 495 has obtained a solution to the optimization problem in step S312 (step S313).
- step S313 If the device control unit 492 determines that a solution has been obtained (step S313: YES), the process proceeds to step S321.
- the process in step S321 is similar to the process in step S141 of FIG. After step S321, the process proceeds to step S341.
- the process in step S341 is similar to the process in step S161 of FIG. After step S341, the obstacle avoidance control device 401 ends the process of FIG.
- step S313 determines that the solution cannot be obtained in step S313 (step S313: NO)
- step S3331 determines that the solution cannot be obtained in step S313 (step S313: NO)
- step S3331 the process proceeds to step S3331.
- the obstacle avoidance control device 401 can also provide a constraint condition for avoiding contact between the control target device 900 and the obstacle.
- a constraint condition can be set for each reference point.
- a constraint condition can be set for each obstacle. In this respect, the obstacle avoidance control device 401 can relatively easily cope with the case where there are a plurality of obstacles.
- FIG. 18 is a diagram showing an example of the configuration of the obstacle avoidance control device according to the fifth embodiment.
- the obstacle avoidance control device 10 shown in FIG. 18 includes an avoidance command value calculation unit 11 and a device control unit 12.
- the avoidance command value calculation unit 11 satisfies the constraint condition including the sufficient condition for the control target device not to contact the obstacle, which is the control command value for the control target device, and
- An avoidance command value which is a control command value whose evaluation value applied to the evaluation function satisfies a predetermined end condition, is obtained.
- the device control unit 12 controls the control target device based on the result of the processing by the avoidance command value calculation unit 11.
- a constraint condition for avoiding contact between the control target device and the obstacle can be provided.
- a constraint condition can be set for each reference point.
- the obstacle avoidance control device 10 is not limited to one reference point for avoiding an obstacle in the control target device, and controls various parts of the control target device so as to avoid the obstacle. can do.
- a constraint condition can be set for each obstacle. In this respect, the obstacle avoidance control device 10 can relatively easily cope with the case where there are a plurality of obstacles.
- FIG. 19 is a figure which shows the example of the procedure of the process in the obstacle avoidance control method which concerns on 6th Embodiment.
- the obstacle avoidance control method shown in FIG. 19 is a control command value for a control target device, satisfies constraint conditions including a sufficient condition for the control target device not to contact an obstacle, and evaluates the control command value.
- a step of obtaining an avoidance command value that is a control command value in which the evaluation value applied to the function satisfies a predetermined end condition (step S11), and a step of controlling the control-target equipment based on the result of the processing in step S11 (step S12) is included.
- the obstacle avoidance control device 10 is not limited to one reference point for avoiding an obstacle in the control target device, and controls various parts of the control target device so as to avoid the obstacle. can do.
- FIG. 20 is a schematic block diagram showing the configuration of a computer according to at least one embodiment.
- the computer 700 includes a CPU 710, a main storage device 720, an auxiliary storage device 730, and an interface 740. Any one or more of the information acquisition device 200, the nominal control device 300, the obstacle avoidance control device 400, the obstacle avoidance control device 401, the obstacle position specifying device 500, and the obstacle motion prediction device 600, It may be implemented in the computer 700. In that case, the operation of each processing unit described above is stored in the auxiliary storage device 730 in the form of a program.
- the CPU 710 reads the program from the auxiliary storage device 730, expands it in the main storage device 720, and executes the above processing according to the program. Further, the CPU 710 secures a storage area corresponding to each storage unit described above in the main storage device 720 according to the program. Communication between each device and other devices is performed by the interface 740 having a communication function and performing communication under the control of the CPU 710.
- the operations of the first control unit 290 and each unit thereof are stored in the auxiliary storage device 730 in the form of a program.
- the CPU 710 reads the program from the auxiliary storage device 730, expands it in the main storage device 720, and executes the above processing according to the program. Further, the CPU 710 secures a storage area corresponding to the first storage unit 280 in the main storage device 720 according to the program.
- the communication performed by the first communication unit 210 is executed by the interface 740 having a communication function and performing the communication under the control of the CPU 710.
- the operations of the second control unit 490 and each of its units are stored in the auxiliary storage device 730 in the form of a program.
- the CPU 710 reads the program from the auxiliary storage device 730, expands it in the main storage device 720, and executes the above processing according to the program. Further, CPU 710 secures a second storage unit 480 and a storage area corresponding to each unit in main storage device 720 according to the program.
- the communication performed by the second communication unit 410 is executed by the interface 740 having a communication function and performing the communication under the control of the CPU 710.
- the operations of the second control unit 490 and each of its units are stored in the auxiliary storage device 730 in the form of a program.
- the CPU 710 reads the program from the auxiliary storage device 730, expands it in the main storage device 720, and executes the above processing according to the program. Further, CPU 710 secures a second storage unit 480 and a storage area corresponding to each unit in main storage device 720 according to the program.
- the communication performed by the second communication unit 410 is executed by the interface 740 having a communication function and performing the communication under the control of the CPU 710.
- the operations of the third control unit 590 and each of its units are stored in the auxiliary storage device 730 in the form of a program.
- the CPU 710 reads the program from the auxiliary storage device 730, expands it in the main storage device 720, and executes the above processing according to the program. Further, CPU 710 secures a third storage unit 580 and a storage area corresponding to each unit in main storage device 720 according to the program.
- the communication performed by the third communication unit 510 is executed by the interface 740 having a communication function and performing the communication under the control of the CPU 710.
- the motions of the fourth control unit 690 and each of its units are stored in the auxiliary storage device 730 in the form of a program.
- the CPU 710 reads the program from the auxiliary storage device 730, expands it in the main storage device 720, and executes the above processing according to the program. Further, the CPU 710 secures a storage area corresponding to the fourth storage unit 680 and each unit in the main storage device 720 according to the program.
- the communication performed by the fourth communication unit 610 is executed by the interface 740 having a communication function and performing the communication under the control of the CPU 710.
- the information acquisition device 200, the nominal control device 300, the obstacle avoidance control device 400, the obstacle avoidance control device 401, the obstacle position specifying device 500, and the obstacle motion prediction device 600 realize all or part of the functions. It is also possible to record the program for doing so in a computer-readable recording medium, read the program recorded in this recording medium into a computer system, and execute it to perform the processing of each unit.
- the “computer system” mentioned here includes an OS (operating system) and hardware such as peripheral devices.
- the "computer-readable recording medium” is a portable medium such as a flexible disk, a magneto-optical disk, a ROM (Read Only Memory), a CD-ROM (Compact Disc Read Only Memory), a hard disk built in a computer system, or the like. A memory device.
- the program may be one for realizing some of the functions described above, or may be one that can realize the functions described above in combination with a program already recorded in the computer system.
- the embodiment of the present invention may be applied to an obstacle avoidance control device, an obstacle avoidance control system, an obstacle avoidance control method, and a recording medium.
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
障害物回避制御装置が、制御対象機器に対する制御指令値であって、前記制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める回避指令値演算部と、前記回避指令値演算部による処理の結果に基づいて、前記制御対象機器を制御する機器制御部と、を備える。
Description
本発明は、障害物回避制御装置、障害物回避制御システム、障害物回避制御方法および記録媒体に関する。
ロボット等の制御対象機器に障害物を回避させるように制御する技術が提案されている。例えば、特許文献1に記載のロボット制御装置は、ロボットのアームの先端部の目標位置と、先端部が目標位置に到達する前の位置である第1位置との間に障害物が存在する場合、障害物を避ける凸状の軌跡を設定する。アームの先端部は、その軌跡に従って移動する。
ロボットが障害物に接触する可能性がある箇所は、アームの先端部に限らない。ロボットアームの先端部に限らず、制御対象機器のいろいろな部分について、障害物を回避できることが好ましい。
本発明の目的の一例は、上記の問題を解決することができる障害物回避制御装置、障害物回避制御システム、障害物回避制御方法および記録媒体を提供することである。
本発明の第1の態様によれば、障害物回避制御装置は、制御対象機器に対する制御指令値であって、前記制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める回避指令値演算部と、前記回避指令値演算部による処理の結果に基づいて、前記制御対象機器を制御する機器制御部と、を備える。
本発明の第2の態様によれば、障害物回避制御方法は、制御対象機器に対する制御指令値であって、前記制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める工程と、前記回避指令値を求める工程での処理の結果に基づいて、前記制御対象機器を制御する工程と、を含む。
本発明の第3の態様によれば、記録媒体は、コンピュータに、制御対象機器に対する制御指令値であって、前記制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める工程と、前記回避指令値を求める工程での処理の結果に基づいて、前記制御対象機器を制御する工程と、を実行させるためのプログラムを記録した記録媒体である。
上記した障害物回避制御装置、障害物回避制御システム、障害物回避制御方法および記録媒体によれば、制御対象機器のいろいろな部分について、障害物を回避するように制御することができる。
以下、本発明の実施形態を説明するが、以下の実施形態は請求の範囲にかかる発明を限定するものではない。また、実施形態の中で説明されている特徴の組み合わせの全てが発明の解決手段に必須であるとは限らない。
<第1実施形態>
図1は、第1実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。図1に示す構成で、障害物回避制御システム1は、撮像装置100と、情報取得装置200と、ノミナル(Nominal)制御装置300と、障害物回避制御装置400とを備える。
図1は、第1実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。図1に示す構成で、障害物回避制御システム1は、撮像装置100と、情報取得装置200と、ノミナル(Nominal)制御装置300と、障害物回避制御装置400とを備える。
障害物回避制御システム1は、制御対象機器900を制御する。障害物回避制御システム1は、制御対象機器900に所望の動作をさせ、かつ、制御対象機器900が障害物に接触しないように、制御対象機器900を制御する。
ここでいう所望の動作は、制御対象機器900に設定されている目標を達成する動作である。ここでいう接触することは、単に触れることに限定されず、ぶつかることを含む。制御対象機器900が障害物に接触することは、制御対象機器900の少なくとも一部分が、障害物の少なくとも一部分に接触することである。
ここでいう所望の動作は、制御対象機器900に設定されている目標を達成する動作である。ここでいう接触することは、単に触れることに限定されず、ぶつかることを含む。制御対象機器900が障害物に接触することは、制御対象機器900の少なくとも一部分が、障害物の少なくとも一部分に接触することである。
以下では、制御対象機器900が垂直多関節ロボットである場合を例に説明するが、障害物回避制御システム1の制御対象は、制御指令値に従って動作し、障害物に接触する可能性があるいろいろな機器とすることができる。例えば、制御対象機器900が、垂直多関節ロボット以外の産業用ロボットであってもよい。
あるいは、制御対象機器900が、建築用ロボットまたは家事ロボットなど、産業用ロボット以外のロボットであってもよい。特定の用途に限定されず、形状変化を伴ういろいろなロボットを、制御対象機器900の例とすることができる。
あるいは、制御対象機器900が、無人搬送車またはドローンなどの移動体であってもよい。また、制御対象機器900が、制御指令値にて制御可能であれば、自律的に動作する装置であってもよい。
あるいは、制御対象機器900が、無人搬送車またはドローンなどの移動体であってもよい。また、制御対象機器900が、制御指令値にて制御可能であれば、自律的に動作する装置であってもよい。
ここでいう障害物は、制御対象機器900が接触する可能性がある物である。障害物は、特定の種類のものに限定されない。例えば、障害物が、人間、他のロボット、周囲の壁または機械、一時的に置かれた荷物の何れか、あるいはこれらの組み合わせであってもよい。
また、制御対象機器900自らも障害物として扱われていてもよい。例えば、制御対象機器900が垂直多関節ロボットであり、その姿勢によってはロボットアームと台座部分とが接触する場合、障害物回避制御システム1が制御対象機器900を障害物として扱うことで、ロボットアームと台座部分との接触を回避し得る。
また、制御対象機器900自らも障害物として扱われていてもよい。例えば、制御対象機器900が垂直多関節ロボットであり、その姿勢によってはロボットアームと台座部分とが接触する場合、障害物回避制御システム1が制御対象機器900を障害物として扱うことで、ロボットアームと台座部分との接触を回避し得る。
撮像装置100は、障害物を撮像する。例えば、撮像装置100は、制御対象機器900が動作可能な空間全体を撮像範囲内に含むように撮像を行うことで、障害物がある場合にその障害物を撮像する。
撮像装置100の撮像画像に基づいて、情報取得装置200が障害物の位置を特定する。撮像装置100として、例えばデプスカメラ(Depth Camera)、ステレオカメラ(Stereo Camera)など、3次元の情報を得られるカメラを用いるようにしてもよい。あるいは、情報取得装置200が、OpenPoseの3次元拡張など、2次元の画像から3次元の情報を得るための技術を用いるようにしてもよい。
撮像装置100の撮像画像に基づいて、情報取得装置200が障害物の位置を特定する。撮像装置100として、例えばデプスカメラ(Depth Camera)、ステレオカメラ(Stereo Camera)など、3次元の情報を得られるカメラを用いるようにしてもよい。あるいは、情報取得装置200が、OpenPoseの3次元拡張など、2次元の画像から3次元の情報を得るための技術を用いるようにしてもよい。
あるいは、障害物回避制御システム1が、撮像装置100に加えて、あるいは代えて、3D-LiDAR(3-Dimension Light Detection and Ranging)など、3次元情報を得るための装置を備えるようにしてもよい。
また、撮像装置100が複数台のカメラで構成されていてもよい。撮像装置100が移動可能になっていてもよい。
撮像装置100が、観測対象の空間の状況をボクセルデータで示すようにしてもよい。
また、撮像装置100が複数台のカメラで構成されていてもよい。撮像装置100が移動可能になっていてもよい。
撮像装置100が、観測対象の空間の状況をボクセルデータで示すようにしてもよい。
情報取得装置200は、制御対象機器900が備えるセンサなど制御対象機器900を観測するセンサからセンシングデータを取得して、制御対象機器900の位置および動作を検出する。情報取得装置200がセンシングデータを取得するセンサは、特定の種類のセンサに限定されない。例えば、情報取得装置200が、センシングデータから、制御対象機器900の各関節の、関節角度、関節角速度、関節速度、および、関節加速度のうち何れか、あるいはこれらの組み合わせの情報を取得するようにしてもよい。
情報取得装置200は、得られた情報に基づいて、制御対象機器900の位置情報、および、制御対象機器900の動きを示す情報を生成し送信する。
情報取得装置200が、制御対象機器900の位置情報をボクセルデータで送信するようにしてもよい。例えば、情報取得装置200が制御対象機器900の表面の位置情報をボクセルデータで送信することで、障害物回避制御装置400は、1点ではなく制御対象機器900の表面と、障害物との位置関係を把握でき、制御対象機器900と障害物との距離をより正確に把握できる。制御対象機器900と障害物との距離をより正確に把握できることで、障害物回避制御装置400は、制御対象機器900に障害物を回避させる制御をより高精度に行うことができる。あるいは、情報取得装置200が、制御対象機器900の位置情報として、制御対象機器900に設定されている代表点の座標を送信するようにしてもよい。
情報取得装置200が、制御対象機器900の位置情報をボクセルデータで送信するようにしてもよい。例えば、情報取得装置200が制御対象機器900の表面の位置情報をボクセルデータで送信することで、障害物回避制御装置400は、1点ではなく制御対象機器900の表面と、障害物との位置関係を把握でき、制御対象機器900と障害物との距離をより正確に把握できる。制御対象機器900と障害物との距離をより正確に把握できることで、障害物回避制御装置400は、制御対象機器900に障害物を回避させる制御をより高精度に行うことができる。あるいは、情報取得装置200が、制御対象機器900の位置情報として、制御対象機器900に設定されている代表点の座標を送信するようにしてもよい。
情報取得装置200は、制御対象機器900の動きを示す情報として、例えば、制御対象機器900の速度、加速度、角速度、角加速度、またはこれらの組み合わせを送信する。情報取得装置200が、制御対象機器900の表面の動きを示す情報をボクセルデータで送信するようにしてもよい。あるいは、情報取得装置200が、制御対象機器900の代表点の動きを示すデータを送信するようにしてもよい。例えば、情報取得装置200が、制御対象機器900の一般化座標qと、一般化速度q’とを並べたベクトルを送信するようにしてもよい。
あるいは、情報取得装置200が、制御対象機器の関節の角速度など、制御対象機器のアクチュエータの動きを示す情報を送信するようにしてもよい。
制御対象機器900の位置情報と制御対象機器900の動きを示す情報とを総称して制御対象機器900の状態情報と称する。
情報取得装置200は、制御対象機器の状態情報を、ノミナル制御装置300および障害物回避制御装置400へ送信する。
あるいは、情報取得装置200が、制御対象機器の関節の角速度など、制御対象機器のアクチュエータの動きを示す情報を送信するようにしてもよい。
制御対象機器900の位置情報と制御対象機器900の動きを示す情報とを総称して制御対象機器900の状態情報と称する。
情報取得装置200は、制御対象機器の状態情報を、ノミナル制御装置300および障害物回避制御装置400へ送信する。
情報取得装置200は、障害物の位置情報を送信する。情報取得装置200が、障害物の位置情報をボクセルデータのデータ形式で送信するようにしてもよい。例えば、情報取得装置200が障害物の表面の位置情報をボクセルデータで送信することで、障害物回避制御装置400は、1点ではなく障害物の表面と、制御対象機器900との位置関係を把握でき、制御対象機器900と障害物との距離をより正確に把握できる。制御対象機器900と障害物との距離をより正確に把握できることで、障害物回避制御装置400は、制御対象機器900に障害物を回避させる制御をより高精度に行うことができる。あるいは、情報取得装置200が、制御対象機器900の位置情報として、制御対象機器900に設定されている代表点の座標を送信するようにしてもよい。
障害物が動く場合、情報取得装置200が、障害物の位置情報に加えて、障害物の動きを示す情報を送信するようにしてもよい。情報取得装置200が、障害物の動きを示す情報として、例えば、障害物の速度、加速度、角速度、角加速度、またはこれらの組み合わせを送信するようにしてもよい。情報取得装置200が、障害物の表面の動きを示す情報をボクセルデータで送信するようにしてもよい。あるいは、情報取得装置200が、障害物の代表点の動きを示すデータを送信するようにしてもよい。例えば、情報取得装置200が、障害物の一般化座標qと、一般化速度q’とを並べたベクトルを送信するようにしてもよい。
障害物の位置情報、あるいは、障害物が動く場合の、障害物の位置情報と障害物の動きを示す情報との組み合わせを、障害物の状態情報と称する。
情報取得装置200は、障害物の状態情報を障害物回避制御装置400へ送信する。
障害物の位置情報、あるいは、障害物が動く場合の、障害物の位置情報と障害物の動きを示す情報との組み合わせを、障害物の状態情報と称する。
情報取得装置200は、障害物の状態情報を障害物回避制御装置400へ送信する。
ノミナル制御装置300は、ノミナル指令値を算出する。ノミナル指令値は、制御対象機器900による障害物回避を考慮しない場合の、制御対象機器900に対する制御指令値である。すなわち、ノミナル指令値は、障害物が無いとの仮定の下で、制御対象機器900に設定された目標を達成するための、制御対象機器900に対する制御指令値である。
ノミナル制御装置300がノミナル指令値を算出するための制御方法は特定のものに限定されず、公知のいろいろな制御方法を用いることができる。
ノミナル制御装置300が算出するノミナル指令値は、障害物回避制御装置400が、制御対象機器900に対して指示する制御指令値(すなわち、実際に用いられる制御指令値)を取得するための基準となる制御指令値として用いられる。
ノミナル制御装置300が算出するノミナル指令値は、障害物回避制御装置400が、制御対象機器900に対して指示する制御指令値(すなわち、実際に用いられる制御指令値)を取得するための基準となる制御指令値として用いられる。
障害物回避制御装置400は、障害物回避制御システム1による制御対象機器900の制御を実行する。したがって、障害物回避制御システム1について上述したように、障害物回避制御装置400は、制御対象機器900に所望の動作をさせ、かつ、制御対象機器900が障害物に接触しないように、制御対象機器900を制御する。
障害物回避制御装置400は、制御対象機器900を制御するための制御指令値を取得する。障害物回避制御装置400は、取得した制御指令値を制御対象機器900へ送信することで、制御対象機器900を制御する。
障害物回避制御装置400は、制御対象機器900を制御するための制御指令値を取得する。障害物回避制御装置400は、取得した制御指令値を制御対象機器900へ送信することで、制御対象機器900を制御する。
制御対象機器900と障害物との距離が所定の閾値よりも離れている場合、障害物回避制御装置400は、ノミナル制御装置300が算出した制御指令値を制御対象機器900への制御指令値として用いる。ノミナル制御装置300が算出した、障害物回避を考慮していない制御指令値を用いても、制御対象機器900が障害物に接触しないと期待されるからである。ノミナル制御装置300が算出した制御閾値を用いることで、障害物回避制御装置400は、障害物回避を考慮した制御閾値を新たに算出する必要が無く、この点で障害物回避制御装置400の負荷が軽くて済む。
一方、制御対象機器900と障害物との距離が所定の閾値未満である場合。ノミナル制御装置300が算出した制御指令値では、制御対象機器900が障害物に接触する可能性が比較的高い。そこで、障害物回避制御装置400は、制御対象機器900が障害物を回避し、かつ、目標を達成するための制御指令値を算出し、算出した制御指令値を制御対象機器900へ送信する。
図2は、情報取得装置200の機能構成の例を示す概略ブロック図である。図2に示す構成で、情報取得装置200は、第1通信部210と、第1記憶部280と、第1制御部290とを備える。第1制御部290は、状態観測部291と、三次元計測部292とを備える。
第1通信部210は、他の装置と通信を行う。特に、第1通信部210は、制御対象機器900が備えるセンサからのセンシングデータを受信する。また、第1通信部210は、撮像装置100からのデータを受信する。第1通信部210は、撮像装置100の撮像画像の画像データを受信し、さらに、奥行き方向の距離情報など付加情報がある場合は、画像データに加えて付加情報を受信する。
第1通信部210は、他の装置と通信を行う。特に、第1通信部210は、制御対象機器900が備えるセンサからのセンシングデータを受信する。また、第1通信部210は、撮像装置100からのデータを受信する。第1通信部210は、撮像装置100の撮像画像の画像データを受信し、さらに、奥行き方向の距離情報など付加情報がある場合は、画像データに加えて付加情報を受信する。
また、第1通信部210は、情報取得装置200が生成する制御対象機器900の位置情報および動作情報をノミナル制御装置300および障害物回避制御装置400へ送信する。また、第1通信部210は、情報取得装置200が生成する障害物の位置情報を障害物回避制御装置400へ送信する。情報取得装置200が障害物の動作も特定する場合、第1通信部210は、障害物の動作情報(例えば、速度または加速度の情報)を障害物回避制御装置400へ送信する。
第1記憶部280は、各種データを記憶する。第1記憶部280の機能は、情報取得装置200が備える記憶デバイスを用いて実行される。
第1制御部290は、情報取得装置200の各部を制御して各種処理を実行する。第1制御部290の機能は、情報取得装置200が備えるCPU(Central Processing Unit、中央処理装置)が、第1記憶部280からプログラムを読み出して実行することで実行される。
第1制御部290は、情報取得装置200の各部を制御して各種処理を実行する。第1制御部290の機能は、情報取得装置200が備えるCPU(Central Processing Unit、中央処理装置)が、第1記憶部280からプログラムを読み出して実行することで実行される。
状態観測部291は、制御対象機器900が備えるセンサからのセンシングデータに基づいて、制御対象機器900の位置および動作を特定する。状態観測部291は、特定した制御対象機器900の位置を示す座標値を算出する。状態観測部291は、制御対象機器900の位置および動作を示すデータを、第1通信部210を介してノミナル制御装置300および障害物回避制御装置400へ送信する。すなわち、状態観測部291は、制御対象機器900の状態情報を、第1通信部210を介してノミナル制御装置300および障害物回避制御装置400へ送信する。
三次元計測部292は、撮像装置100からのデータに基づいて、障害物の位置を特定する。三次元計測部292は、特定した位置を示す座標値を算出し、算出したデータを、第1通信部210を介して障害物回避制御装置400へ送信する。また、障害物が動く場合、三次元計測部292が、障害物の動作情報(例えば、速度または加速度の情報)を算出し、算出したデータを、第1通信部210を介して障害物回避制御装置400へ送信するようにしてもよい。このように、三次元計測部292は、障害物の状態情報を、第1通信部210を介して障害物回避制御装置400へ送信する
図3は、障害物回避制御装置400の機能構成の例を示す概略ブロック図である。図3に示す構成で、障害物回避制御装置400は、第2通信部410と、第2記憶部480と、第2制御部490とを備える。第2記憶部480は、構造情報記憶部481と、動的モデル記憶部482とを備える。第2制御部490は、干渉関数演算部491と、機器制御部492とを備える。機器制御部492は、ノミナル指令値取得部493と、回避要否判定部494と、回避指令値演算部495と、仮想力場反映部496とを備える。
第2通信部410は、他の装置と通信を行う。特に、第2通信部410は、情報取得装置200が送信する、制御対象機器900の状態情報および障害物の状態情報を受信する。また、第2通信部410は、ノミナル制御装置300が算出して送信する制御指令値を受信する。また、第2通信部410は、制御対象機器900へ制御指令値を送信する。障害物回避制御装置400は、制御対象機器900へ制御指令値を送信することで制御対象機器900を制御する。
第2記憶部480は、各種データを記憶する。第2記憶部480の機能は、障害物回避制御装置400が備える記憶デバイスを用いて実行される。
構造情報記憶部481は、制御対象機器900の構造情報を記憶する。制御対象機器900の構造情報は、制御対象機器900のアームの長さおよび太さ、関節が動く方向および可動範囲など、制御対象機器900の形状および動作に関する情報である。制御対象機器900のCAD(Computer Aided Design)データがある場合、そのCADデータを構造情報として用いるようにしてもよい。
構造情報記憶部481は、制御対象機器900の構造情報を記憶する。制御対象機器900の構造情報は、制御対象機器900のアームの長さおよび太さ、関節が動く方向および可動範囲など、制御対象機器900の形状および動作に関する情報である。制御対象機器900のCAD(Computer Aided Design)データがある場合、そのCADデータを構造情報として用いるようにしてもよい。
動的モデル記憶部482は、制御対象機器900の動的モデルを記憶する。制御対象機器900の動的モデルは、制御対象機器900の状態情報、および、制御指令値の入力を受けて制御対象機器900がその制御指令値に従う場合の動作を模擬する。
動的モデルが、将来の時点における制御対象機器900の予測位置の位置情報を出力するようにしてもよい。あるいは、動的モデルが、制御対象機器900の動作量を出力するようにしてもよい。すなわち、動的モデルが、制御対象機器900の将来の予測位置から現在位置を減算した差分を出力するようにしてもよい。
動的モデルは、制御指令値uの入力に対して、制御対象機器900の状態情報xで示される状態の微分値または差分が求まるモデルであり、例えば状態空間モデルであってもよい。
動的モデルが、将来の時点における制御対象機器900の予測位置の位置情報を出力するようにしてもよい。あるいは、動的モデルが、制御対象機器900の動作量を出力するようにしてもよい。すなわち、動的モデルが、制御対象機器900の将来の予測位置から現在位置を減算した差分を出力するようにしてもよい。
動的モデルは、制御指令値uの入力に対して、制御対象機器900の状態情報xで示される状態の微分値または差分が求まるモデルであり、例えば状態空間モデルであってもよい。
制御対象機器900のセンサからの情報と、構造情報記憶部481が記憶する構造情報と、動的モデル記憶部482が記憶する動的モデルとを用いることで、制御対象機器900が占有する空間を特定することができる。
特に、制御対象機器900の構造情報にアームの太さ等の情報が含まれていることで、障害物回避制御装置400は、制御対象機器900の骨格の位置のみでなく、アームの太さ等を考慮した位置情報を取得できる。すなわち、障害物回避制御装置400は、制御対象機器900の位置情報を、空間的な広がりを有する位置として把握できる。
特に、制御対象機器900の構造情報にアームの太さ等の情報が含まれていることで、障害物回避制御装置400は、制御対象機器900の骨格の位置のみでなく、アームの太さ等を考慮した位置情報を取得できる。すなわち、障害物回避制御装置400は、制御対象機器900の位置情報を、空間的な広がりを有する位置として把握できる。
また、障害物回避制御装置400は、制御対象機器900の動的モデルを用いることで、制御対象機器900の動作をより正確に把握できる。これにより、制御対象機器900が障害物に接触するか否か、接触する場合は制御対象機器900がどのように動いて接触するかを判定でき、制御対象機器900による障害物回避をより高精度に行わせることができる。
第2制御部490は、障害物回避制御装置400の各部を制御して各種処理を実行する。第2制御部490の機能は、障害物回避制御装置400が備えるCPUが第2記憶部480からプログラムを読み出して実行することで実行される。
干渉関数演算部491は、干渉関数値、および、干渉関数値の経時変化を示す値を算出する。干渉関数は、制御対象機器900と障害物との位置関係に応じた値を示す。干渉関数Bは、式(1)のような値をとる。
干渉関数演算部491は、干渉関数値、および、干渉関数値の経時変化を示す値を算出する。干渉関数は、制御対象機器900と障害物との位置関係に応じた値を示す。干渉関数Bは、式(1)のような値をとる。
式(1)では、xは、制御対象機器900の状態情報を示す。例えば、情報取得装置200が、制御対象機器900の表面の位置情報をボクセルデータで送信し、干渉関数演算部491は、制御対象機器900の状態情報を干渉関数Bに適用することで、制御対象機器900と障害物とが最も近い位置における両者の距離を算出するようにしてもよい。
以下では、干渉関数値B(x)が、制御対象機器900の状態情報xが示す制御対象機器900の位置と障害物との距離を示すものとする。障害物が複数ある場合は、制御対象機器900の位置に最も近い障害物との距離を示すものとする。通常、制御対象機器900は障害物の中には入らないので、制御対象機器900が障害物の内部に位置する場合の干渉関数値B(x)は定義されていなくてもよい。
干渉関数値B(x)により、制御対象機器900が障害物に接触しているか否か、および、制御対象機器900と障害物との距離が示される。
干渉関数演算部491は、障害物の状態情報に基づいて干渉関数を生成し、第2記憶部480に記憶させておく。
干渉関数値B(x)により、制御対象機器900が障害物に接触しているか否か、および、制御対象機器900と障害物との距離が示される。
干渉関数演算部491は、障害物の状態情報に基づいて干渉関数を生成し、第2記憶部480に記憶させておく。
制御対象機器900が動作して制御対象機器900の位置が経時変化することで、干渉関数値B(x)も経時変化する。この場合、干渉関数演算部491は、干渉関数値B(x)の経時変化を示す値として、制御ステップ間における干渉関数値B(x)の変化量を算出する。
ここでの制御ステップは、障害物回避制御装置400が制御対象機器900に対して制御指令値を1回送信するための一連の処理のステップである。すなわち、障害物回避制御装置400は、周期的な制御ステップ毎に制御対象機器900に対して制御指令値を送信する。
干渉関数演算部491は、今回の制御ステップから次回の制御ステップまでの間の、干渉関数値B(x)の変化量を予測する。制御ステップ間における干渉関数値の変化量をΔB(x,u)と表記する。干渉関数値B(x)の変化量は制御対象機器900の位置の変化に依存し、制御対象機器900の位置の変化は制御指令値uに依存するため、制御指令値uを明示している。
干渉関数演算部491は、今回の制御ステップから次回の制御ステップまでの間の、干渉関数値B(x)の変化量を予測する。制御ステップ間における干渉関数値の変化量をΔB(x,u)と表記する。干渉関数値B(x)の変化量は制御対象機器900の位置の変化に依存し、制御対象機器900の位置の変化は制御指令値uに依存するため、制御指令値uを明示している。
また、干渉関数演算部491は、制御対象機器900の位置に対するポテンシャル関数値P(x)を算出する。ポテンシャル関数値P(x)は、制御対象機器900と障害物との距離に基づく仮想的な力場の値である。干渉関数演算部491は、制御対象機器900の状態情報に示される制御対象機器900の位置が障害物に近いほど(従って、干渉関数値B(x)が小さいほど)ポテンシャル関数値P(x)を大きく算出する。
機器制御部492は、ノミナル制御装置300からのノミナル指令値、および、回避指令値演算部495による処理の結果のうち何れか一方に基づいて、制御対象機器900を制御する。上述したように、ノミナル指令値は、障害物回避を考慮していない制御指令値である。一方、回避指令値演算部495は、制御対象機器900が障害物を回避できるような制御指令値を求める。
制御対象機器900と障害物との距離が所定の閾値より大きいと判定された場合、機器制御部492は、ノミナル制御装置300からのノミナル指令値、または、ノミナル指令値に対して仮想力場反映部496による補正を加えた制御指令値を、第2通信部410を介して制御対象機器900へ送信する。
一方、制御対象機器900と障害物との距離が所定の閾値以下であると判定された場合、回避指令値演算部495が、障害物を回避できる制御指令値の算出を試みる。回避指令値演算部495が制御指令値の算出に成功した場合、機器制御部492は、回避指令値演算部495が算出した制御指令値、または、その制御指令値に対して仮想力場反映部496による補正を加えた制御指令値を、第2通信部410を介して制御対象機器900へ送信する。
一方、回避指令値演算部495が制御指令値の算出に失敗した場合、すなわち、障害物を回避できる制御指令値を得られなかった場合、機器制御部492は、制御対象機器900を減速させる制御指令値を、第2通信部410を介して制御対象機器900へ送信する。
一方、回避指令値演算部495が制御指令値の算出に失敗した場合、すなわち、障害物を回避できる制御指令値を得られなかった場合、機器制御部492は、制御対象機器900を減速させる制御指令値を、第2通信部410を介して制御対象機器900へ送信する。
ノミナル指令値取得部493は、ノミナル指令値を取得する。具体的には、ノミナル指令値取得部493は、第2通信部410の受信信号の中から、ノミナル制御装置300からのノミナル指令値を抽出する。
回避要否判定部494は、制御対象機器900と障害物との位置関係に基づいて、回避指令値演算部495が制御指令値を取得するための処理を行う必要の有無を判定する。上記のように、回避要否判定部494は、制御対象機器900と障害物との距離と閾値とを比較して、距離が閾値より大きいか否かを判定する。具体的には、回避要否判定部494は、干渉関数値B(x)と所定の定数の閾値αとを比較し、B(x)>αか否かを判定する。
回避要否判定部494は、制御対象機器900と障害物との位置関係に基づいて、回避指令値演算部495が制御指令値を取得するための処理を行う必要の有無を判定する。上記のように、回避要否判定部494は、制御対象機器900と障害物との距離と閾値とを比較して、距離が閾値より大きいか否かを判定する。具体的には、回避要否判定部494は、干渉関数値B(x)と所定の定数の閾値αとを比較し、B(x)>αか否かを判定する。
B(x)>αである場合、制御対象機器900の位置が障害物から比較的離れている。この場合、回避要否判定部494は、回避指令値演算部495が制御指令値を取得するための処理を行う必要は無いと判定する。一方、B(x)≦αである場合、制御対象機器900の位置が障害物に比較的近い。この場合、回避要否判定部494は、回避指令値演算部495が制御指令値を取得するための処理を行う必要があると判定する。
回避指令値演算部495は、回避指令値を求める。回避指令値は、制御対象機器900に対する制御指令値であって、制御対象機器900が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である。特に、障害物回避制御装置400が回避指令値を用いて制御対象機器900を制御することで、制御対象機器900が障害物に接触しないように制御できる。
回避指令値演算部495は、求める解としての制御指令値とノミナル指令値との相違の度合いが所定の条件を満たすという終了条件を用いて回避指令値を求める。例えば、回避指令値演算部495は、上記の制約条件の下で、求める解としての制御指令値とノミナル指令値との相違の度合いを最小化する最適化問題を解くことで、回避指令値を求める。
この場合、回避指令値演算部495が解く最適化問題における評価関数(目的関数とも称される)は、例えば、式(2)のように示される。
「u*」は、この最適化問題の解となる制御指令値を示す。「argmin」は、引数の値を最小化する関数である。式(2)の場合、「argmin」は、引数「(u-ur)T(u-ur)」を最小化するような、uの値を関数値とする。
ベクトルまたは行列に上付の「T」は、ベクトルまたは行列の転置を示す。
「ur」は、ノミナル制御装置300からのノミナル指令値を示す。
ベクトルまたは行列に上付の「T」は、ベクトルまたは行列の転置を示す。
「ur」は、ノミナル制御装置300からのノミナル指令値を示す。
式(2)は、制約条件の下で、ノミナル指令値urになるべく近い制御指令値を求めることを示している。ノミナル指令値urは、制御対象機器900に設定されている目的を制御対象機器900に実行させるように算出された指令値なので、ノミナル指令値urに近い指令値を求めることで、制御対象機器900に設定されている目的を制御対象機器900に実行させることができると期待される。
制御指令値を示すu*、u、urのデータ形式は、同じ次元のベクトルであるとする。これらのベクトルの次元数は、障害物回避制御装置400が制御対象機器900へ送信する制御指令値の次元数と同じとする。
式(2)は、評価関数の例に該当する。式(2)における最小解となる制御指令値uは、制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値の例に該当する。
制御指令値を示すu*、u、urのデータ形式は、同じ次元のベクトルであるとする。これらのベクトルの次元数は、障害物回避制御装置400が制御対象機器900へ送信する制御指令値の次元数と同じとする。
式(2)は、評価関数の例に該当する。式(2)における最小解となる制御指令値uは、制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値の例に該当する。
回避指令値演算部495が解く最小化問題における制約条件は、2種類の式で示される。2種類の式のうち1種類目は、式(3)のように示される。
γは、0≦γ<1の定数である。
γの値によって、制御対象機器900と障害物とが接触しないための、制御対象機器900と障害物との間隔の余裕分をどれだけ見込むか調整できる。
通常、制御対象機器900と障害物とは接触しておらず、B(x)が、制御対象機器900と障害物との距離を示す。制御対象機器900が障害物に接近してΔB(x,u)が負の値をとるとき、ΔB(x,u)の大きさがγB(x)以下である場合に式(3)が成り立つ。
γの値によって、制御対象機器900と障害物とが接触しないための、制御対象機器900と障害物との間隔の余裕分をどれだけ見込むか調整できる。
通常、制御対象機器900と障害物とは接触しておらず、B(x)が、制御対象機器900と障害物との距離を示す。制御対象機器900が障害物に接近してΔB(x,u)が負の値をとるとき、ΔB(x,u)の大きさがγB(x)以下である場合に式(3)が成り立つ。
このことから、B(x)で示される制御対象機器900と障害物との距離のうち、(1-γ)B(x)の分は、制御対象機器900と障害物とが接触しないための余裕分として、制御対象機器900の動作可能範囲から除外されているといえる。γの値を大きく設定するほど、制御対象機器900の動作可能範囲が広がる。一方、γの値を小さく設定するほど、制御対象機器900と障害物とが接触しないための余裕分を大きくとることになり、例えば予期しない外力によって制御対象機器900が障害物のほうへ押された場合でも、制御対象機器900が障害物にぶつかりにくい。
式(3)に示されるように、回避指令値演算部495は、干渉関数値、および、干渉関数値の経時変化を示す値を用いて回避指令値を求める。
複数の障害物が存在する場合、障害物毎に式(3)の制約条件を設けることができる。これにより、障害物回避制御装置400は、制御対象機器900が障害物全体に接触しないよう制御することができる。あるいは、複数の障害物をまとめたものに対して干渉関数を設計してもよい。
複数の障害物が存在する場合、障害物毎に式(3)の制約条件を設けることができる。これにより、障害物回避制御装置400は、制御対象機器900が障害物全体に接触しないよう制御することができる。あるいは、複数の障害物をまとめたものに対して干渉関数を設計してもよい。
式(3)は、制御対象機器900が今回の制御ステップで障害物に接触していなければ、次の制御ステップでも障害物に接触しないことの十分条件を示す。この点について説明する。
今回の制御ステップをtで示し、制御ステップtの次の制御ステップをt+1で示す。制御ステップtにおける干渉関数値をB(xt)と表記する。
制御ステップt+1における干渉関数値をB(xt+1)と表記する。B(xt+1)からB(xt)を減算した差分を、ΔB(xt,ut)と表記する。ΔB(xt,ut)は、式(4)のように示される。
今回の制御ステップをtで示し、制御ステップtの次の制御ステップをt+1で示す。制御ステップtにおける干渉関数値をB(xt)と表記する。
制御ステップt+1における干渉関数値をB(xt+1)と表記する。B(xt+1)からB(xt)を減算した差分を、ΔB(xt,ut)と表記する。ΔB(xt,ut)は、式(4)のように示される。
また、式(3)より式(5)を得られる。
式(4)および式(4)より式(6)を得られる。
0≦γ<1なので、B(xt)>0の場合、B(xt)-γB(xt)≧0であり、B(xt+1)>0である。従って、制御ステップtで制御対象機器900の位置が障害物の外部にあれば、制御ステップt+1でも制御対象機器900の位置は障害物の外部にある。
式(3)の制約条件の判定に、制御対象機器900の表面の位置情報が用いられる場合、回避指令値演算部495は、式(3)の条件の成否をより正確に判定できる。特に、制御対象機器900の表面の位置情報と、障害物の表面の位置情報とが用いられる場合、回避指令値演算部495は、干渉関数Bを用いて制御対象機器900と障害物とが最も近い位置での両者の距離を得られる。
回避指令値演算部495は、この距離を用いて式(3)の条件を判定することで、より正確に条件判定を行うことができる。この点で、回避指令値演算部495が、より高精度な制御指令値を得られ、機器制御部492が、より高精度に制御対象機器900を制御できると期待される。
回避指令値演算部495は、この距離を用いて式(3)の条件を判定することで、より正確に条件判定を行うことができる。この点で、回避指令値演算部495が、より高精度な制御指令値を得られ、機器制御部492が、より高精度に制御対象機器900を制御できると期待される。
式(3)の制約条件の判定に制御対象機器900の表面の位置情報が用いられる場合の例として、情報取得装置200が、制御対象機器900の表面の位置情報を例えばボクセルデータで障害物回避制御装置400へ送信する場合が挙げられる。
式(3)の制約条件の判定に障害物の表面の位置情報が用いられる場合の例として、情報取得装置200が、障害物の表面の位置情報を例えばボクセルデータを障害物回避制御装置400へ送信し、干渉関数演算部491が、その位置情報を用いて干渉関数を生成する場合が挙げられる。
式(3)の制約条件の判定に障害物の表面の位置情報が用いられる場合の例として、情報取得装置200が、障害物の表面の位置情報を例えばボクセルデータを障害物回避制御装置400へ送信し、干渉関数演算部491が、その位置情報を用いて干渉関数を生成する場合が挙げられる。
一方、式(3)の判定に制御対象機器900に設定された基準点の位置情報が用いられる場合、基準点毎に式(3)の条件式を設けることができる。障害物回避制御装置400では、この点で、基準点が複数の場合に比較的容易に対応できる。
障害物が複数ある場合も、障害物毎に式(3)の条件式を設けることができる。障害物回避制御装置400では、この点で、障害物が複数の場合に比較的容易に対応できる。
障害物が複数ある場合も、障害物毎に式(3)の条件式を設けることができる。障害物回避制御装置400では、この点で、障害物が複数の場合に比較的容易に対応できる。
回避指令値演算部495が解く最小化問題における制約条件を示す2種類の式のうち2種類目は、式(7)のように示される。
ui(iは、1≦i≦Nの整数)は、例えば、制御対象機器900の関節毎など、制御対象機器900の可動部分毎の制御指令値を示すスカラ値である。Nは、制御対象機器900の可動部分の個数を示す。iは、可動部分を識別する識別番号である。
識別番号iで識別される可動部分をi番目の可動部分と表記する。従って、uiは、i番目の可動部分に対する制御指令値である。
識別番号iで識別される可動部分をi番目の可動部分と表記する。従って、uiは、i番目の可動部分に対する制御指令値である。
ui_min、ui_maxは、それぞれ、制御対象機器900の仕様により予め定められている、uiの下限値、上限値である。
式(7)は、いずれの制御指令値も、可動部分の仕様により規定される上下限値の範囲内で設定されるという制約条件を示している。可動部分の仕様は、例えば、その可動部分に用いられているアクチュエータの仕様により規定される。
制御指令値uは、ui(i=1,2,・・・,N)を纏めてベクトルで表したものである。
上述したように、回避指令値演算部495は、回避指令値を取得するための処理が必要であると回避要否判定部494が判定した場合に、回避指令値を求める。
制御指令値uは、ui(i=1,2,・・・,N)を纏めてベクトルで表したものである。
上述したように、回避指令値演算部495は、回避指令値を取得するための処理が必要であると回避要否判定部494が判定した場合に、回避指令値を求める。
仮想力場反映部496は、制御対象機器900が障害物に近いほど障害物から離れる動作を、ノミナル指令値および回避指令値のうち少なくとも何れか一方に反映させる。
制御対象機器900が障害物に近いほど障害物から離れることは、制御対象機器900が障害物に近いほど大きい加速度で障害物から離れることであってもよい。あるいは、制御対象機器900が障害物に近いほど障害物から離れることは、制御対象機器900が障害物に近いほど、障害物から離れる距離が長いことであってもよい。あるいは、制御対象機器900が障害物に近いほど障害物から離れることは、制御対象機器900と障害物との距離が所定の閾値以下である場合のみ、制御対象機器900が障害物から離れることであってもよい。
制御対象機器900が障害物に近いほど障害物から離れることは、制御対象機器900が障害物に近いほど大きい加速度で障害物から離れることであってもよい。あるいは、制御対象機器900が障害物に近いほど障害物から離れることは、制御対象機器900が障害物に近いほど、障害物から離れる距離が長いことであってもよい。あるいは、制御対象機器900が障害物に近いほど障害物から離れることは、制御対象機器900と障害物との距離が所定の閾値以下である場合のみ、制御対象機器900が障害物から離れることであってもよい。
例えば、仮想力場反映部496が、制御対象機器900と障害物との位置関係に基づく仮想的な力場を模擬するようにしてもよい。この場合、仮想力場反映部496が、制御対象機器900と障害物との距離が近いほど反発力を大きく算出してもよい。制御対象機器900に生じさせる反発力が大きいほど、制御対象機器900の加速度が大きくなり、制御対象機器900が障害物から離れる移動量の大きさが大きくなる。
例えば、仮想力場反映部496が算出する制御指令値をupと表記する。制御指令値uPが制御対象機器900に設定された基準点の移動量を示すとすると、up=(Δx,Δy,Δz)と示される。
障害物を質点(Point Mass)で代表し、障害物の座標を(x0,y0,z0)としてもよい。そして、仮想力場反映部496が、座標(x,y,z)に位置する制御対象機器900に対して、障害物から遠ざからせるための制御指令値upを、式(8)に基づいて算出するようにしてもよい。
障害物を質点(Point Mass)で代表し、障害物の座標を(x0,y0,z0)としてもよい。そして、仮想力場反映部496が、座標(x,y,z)に位置する制御対象機器900に対して、障害物から遠ざからせるための制御指令値upを、式(8)に基づいて算出するようにしてもよい。
P(x,y,z)はポテンシャル関数を示す。∇はナブラを示す。P0(x,y,z)は、式(9)のように示される。
なお、式(1)、式(3)から式(6)、および、式(10)から式(12)でxが制御対象機器900の3次元の位置を示すのと異なり、式(8)および式(9)では、(x,y,z)が座標を示す。したがって、式(1)、式(3)から式(6)、および、式(10)から式(12)でxがベクトル量または行列を示すのと異なり、式(8)および式(9)ではxはスカラ量を示す。
仮想力場反映部496が、例えばロボットアームの先端など代表的な1点について、指令値としての制御対象機器900の移動量を算出し、算出した移動量を各関節に分散させるようにしてもよい。あるいは、仮想力場反映部496が、ロボットの関節毎など制御対象機器900の複数部分の各々について、指示する移動量を算出するようにしてもよい。
仮想力場反映部496が、算出した指令値を制御指令値に反映させることで、制御対象機器900と障害物との接触を、より確実に防止できる。
以下では、仮想力場反映部496が、ノミナル指令値および回避指令値の両方に、算出した指令値を反映させる場合を例に説明する。
但し、仮想力場反映部496の処理は障害物回避制御装置400に必須ではない。従って、障害物回避制御装置400が仮想力場反映部496を備えていない構成となっていてもよい。
以下では、仮想力場反映部496が、ノミナル指令値および回避指令値の両方に、算出した指令値を反映させる場合を例に説明する。
但し、仮想力場反映部496の処理は障害物回避制御装置400に必須ではない。従って、障害物回避制御装置400が仮想力場反映部496を備えていない構成となっていてもよい。
図4は、障害物回避制御システム1におけるデータの流れの例を示す図である。図4では、障害物に符号950を付している。障害物950は、上記で説明した障害物と同様である。
上述したように、制御対象機器900のセンサのセンシングデータは、情報取得装置200の状態観測部291へ入力される。
撮像装置100の画像データは、情報取得装置200の三次元計測部292へ入力される。撮像装置100の付加情報がある場合、画像データに加えて付加情報も三次元計測部292へ入力される。
上述したように、制御対象機器900のセンサのセンシングデータは、情報取得装置200の状態観測部291へ入力される。
撮像装置100の画像データは、情報取得装置200の三次元計測部292へ入力される。撮像装置100の付加情報がある場合、画像データに加えて付加情報も三次元計測部292へ入力される。
情報取得装置200は、状態観測部291が算出する制御対象機器900の状態情報を、ノミナル制御装置300および障害物回避制御装置400へ送信する。例えば、情報取得装置200は、制御対象機器900の表面の位置情報をボクセルデータで送信する。加えて、情報取得装置200は、制御対象機器900の表面または基準点の速度、加速度、角速度または角加速度など、制御対象機器900の動作を示す情報を送信する。例えば、情報取得装置200が、一般化座標qと、一般化速度q’とを並べたベクトルを送信するようにしてもよい。
また、情報取得装置200は、障害物950の状態情報を障害物回避制御装置400へ送信する。特に情報取得装置200は、障害物950の3次元の位置情報を障害物回避制御装置400へ送信する。情報取得装置200が障害物950の位置情報を送信するデータ形式は、特定のものに限定されない。例えば、情報取得装置200が、障害物950の位置情報を、ボクセル(Voxel)データにて送信するようにしてもよい。あるいは、情報取得装置200が、障害物950の位置情報を、サーフェイス(Surface)データにて送信するようにしてもよい。あるいは、情報取得装置200が、障害物950の位置情報を、ポイントクラウド(Point Cloud)データにて送信するようにしてもよい。
ノミナル制御装置300は、情報取得装置200から受信する、制御対象機器900の状態情報を用いて、ノミナル指令値urを算出する。ノミナル制御装置300は、算出したノミナル指令値urを障害物回避制御装置400へ送信する。
障害物回避制御装置400の干渉関数演算部491は、情報取得装置200から得られた制御対象機器900の状態情報および障害物の状態情報と、機器制御部492が設定する制御指令値とに基づいて、干渉関数値B(x)、制御ステップ間におけるB(x)の変化量ΔB(x,u)、および、ポテンシャル関数値P(x)を算出する。上記のように、仮想力場反映部496の処理は必須ではない。従って、干渉関数演算部491によるポテンシャル関数値の算出は必須ではない。
障害物回避制御装置400の機器制御部492は、ノミナル制御装置300から得られるノミナル指令値urと、干渉関数演算部491から得られる干渉関数値B(x)、干渉関数値の変化量ΔB(x,u)、および、ポテンシャル関数値P(x)とを用いて、制御対象機器900に対する制御指令値uを取得する。機器制御部492は、取得した制御指令値uを、第2通信部410を介して制御対象機器900へ送信することで、制御対象機器900を制御する。
図5から図7を参照して、障害物回避制御装置400の動作について説明する。
図5は、障害物回避制御装置400が制御対象機器900に対する制御指令値を取得する処理手順の例を示すフローチャートである。障害物回避制御装置400は、制御対象機器900に対する制御の制御ステップ毎に図5の処理を行う。
図5の処理で、仮想力場反映部496は、干渉関数演算部491が算出するポテンシャル関数値P(x)に基づいて、力場を模擬するための制御指令値upを算出する(ステップS111)。制御指令値upは、ノミナル指令値または回避指令値に値を付加する補正項として用いられる。
図5は、障害物回避制御装置400が制御対象機器900に対する制御指令値を取得する処理手順の例を示すフローチャートである。障害物回避制御装置400は、制御対象機器900に対する制御の制御ステップ毎に図5の処理を行う。
図5の処理で、仮想力場反映部496は、干渉関数演算部491が算出するポテンシャル関数値P(x)に基づいて、力場を模擬するための制御指令値upを算出する(ステップS111)。制御指令値upは、ノミナル指令値または回避指令値に値を付加する補正項として用いられる。
また、回避要否判定部494は、干渉関数演算部491が算出する干渉関数値B(x)が、閾値αより大きいか否かを判定する(ステップS112)。
干渉関数値B(x)が閾値αより大きいと判定した場合(ステップS112:YES)、機器制御部492は、制御対象機器900に対する制御指令値uを、ノミナル指令値urに制御指令値upを加えた値と算出する(ステップS121)。すなわち、機器制御部492は、u=ur+upと算出する。
そして、機器制御部492は、制御指令値uを、第2通信部410を介して制御対象機器900へ送信することで、制御対象機器900を制御する(ステップS161)。
ステップS161の後、障害物回避制御装置400は、図5の処理を終了する。
干渉関数値B(x)が閾値αより大きいと判定した場合(ステップS112:YES)、機器制御部492は、制御対象機器900に対する制御指令値uを、ノミナル指令値urに制御指令値upを加えた値と算出する(ステップS121)。すなわち、機器制御部492は、u=ur+upと算出する。
そして、機器制御部492は、制御指令値uを、第2通信部410を介して制御対象機器900へ送信することで、制御対象機器900を制御する(ステップS161)。
ステップS161の後、障害物回避制御装置400は、図5の処理を終了する。
一方、ステップS112で干渉関数値B(x)が閾値α以下であると回避要否判定部494が判定した場合(ステップS112:NO)、回避指令値演算部495は、上述した最適化問題の演算による制御指令値の取得を試みる(ステップS131)。
機器制御部492は、回避指令値演算部495がステップS131で最適化問題の解を得られたか否かを判定する(ステップS132)。
機器制御部492は、回避指令値演算部495がステップS131で最適化問題の解を得られたか否かを判定する(ステップS132)。
解を得られたと判定した場合(ステップS132:YES)、機器制御部492は、制御対象機器900に対する制御指令値uを、最適化問題の解による制御指令値u*に制御指令値upを加えた値と算出する(ステップS141)。すなわち、機器制御部492は、u=u*+upと算出する。
ステップS141の後、処理がステップS161へ進む。
ステップS141の後、処理がステップS161へ進む。
一方、ステップS132の判定で、最適化問題の解を得られていないと判定した場合(ステップS132:NO)、機器制御部492は、制御対象機器900を減速させるように制御指令値uを生成する(ステップS151)。
ステップS151の後、処理がステップS161へ進む。
ステップS151の後、処理がステップS161へ進む。
図6は、障害物回避制御装置400が干渉関数に関する値を算出する処理手順の例を示すフローチャートである。障害物回避制御装置400は、制御対象機器900に対する制御の制御ステップ毎に、図5の処理の前処理として図6の処理を行う。
図6の処理で、干渉関数演算部491は、情報取得装置200が送信する制御対象機器900の情報、および、構造情報記憶部481が記憶している制御対象機器900の構造情報等を用いて、制御対象機器900の占有空間の位置情報を算出する(ステップS211)。干渉関数演算部491は、制御対象機器900の占有空間の位置情報を、制御対象機器900の座標系による座標値で算出する。ここでいう制御対象機器900の座標系は、例えば、障害物回避制御装置400が制御対象機器900に送信する制御指令値に用いられる座標値である。
図6の処理で、干渉関数演算部491は、情報取得装置200が送信する制御対象機器900の情報、および、構造情報記憶部481が記憶している制御対象機器900の構造情報等を用いて、制御対象機器900の占有空間の位置情報を算出する(ステップS211)。干渉関数演算部491は、制御対象機器900の占有空間の位置情報を、制御対象機器900の座標系による座標値で算出する。ここでいう制御対象機器900の座標系は、例えば、障害物回避制御装置400が制御対象機器900に送信する制御指令値に用いられる座標値である。
次に干渉関数演算部491は、障害物の位置情報を取得する(ステップS212)。三次元計測部292が、障害物の位置を検出して制御対象機器900の座標系による位置情報を生成し、干渉関数演算部491が、その位置情報を用いるようにしてもよい。あるいは、三次元計測部292が、障害物の位置情報を、制御対象機器900の座標系とは異なる座標系の座標値で生成し、干渉関数演算部491が、制御対象機器900の座標系の座標値に座標変換するようにしてもよい。
次に、干渉関数演算部491は、制御対象機器900の位置情報および障害物の位置情報に基づいて、干渉関数値B(x)を算出する(ステップS213)。
また、干渉関数演算部491は、制御対象機器900の位置情報、制御対象機器900の動作情報、および、障害物の位置情報に基づいて、干渉関数値の変化量ΔB(x,u)を算出する(ステップS214)。
また、干渉関数演算部491は、制御対象機器900の位置情報、制御対象機器900の動作情報、および、障害物の位置情報に基づいて、干渉関数値の変化量ΔB(x,u)を算出する(ステップS214)。
干渉関数演算部491が、制御対象機器900の動作情報として、状態観測部291が制御対象機器900のセンサのセンシングデータから算出する、制御対象機器900の動作情報を用いるようにしてもよい。あるいは、干渉関数演算部491が、制御対象機器900の動作情報として、前回の制御ステップでの制御対象機器900への制御指令値uを用いるようにしてもよい。あるいは、干渉関数演算部491が、制御対象機器900の動作情報として、これらの両方を用いるようにしてもよい。
ステップS214の後、障害物回避制御装置400は、図6の処理を終了する。
ステップS214の後、障害物回避制御装置400は、図6の処理を終了する。
図7は、障害物回避制御装置400が干渉関数値の変化量ΔB(x,u)を算出する処理手順の例を示すフローチャートである。障害物回避制御装置400は、図6のステップS214で図7の処理を行う。
図7の処理で、干渉関数演算部491は、動的モデルの演算でΔB(x,u)を算出可能か判定する(ステップS221)。
算出可能と判定した場合(ステップS221;YES)、干渉関数演算部491は、動的モデルの演算でΔB(x,u)を算出する(ステップS231)。
ステップS231の後、障害物回避制御装置400は、図7の処理を終了する。
図7の処理で、干渉関数演算部491は、動的モデルの演算でΔB(x,u)を算出可能か判定する(ステップS221)。
算出可能と判定した場合(ステップS221;YES)、干渉関数演算部491は、動的モデルの演算でΔB(x,u)を算出する(ステップS231)。
ステップS231の後、障害物回避制御装置400は、図7の処理を終了する。
一方、ステップS221で、動的モデルの演算でΔB(x,u)を算出できないと判定した場合(ステップS221:NO)、干渉関数演算部491は、制御指令値uの微小変化を用いてΔB(x,u)を算出する。例えば、干渉関数演算部491は、式(10)を用いてΔB(x,u)を近似的に算出する。
Δtは、制御ステップ間の時間間隔を示す。B(x,u)は、干渉関数値を示す。制御指令値uを変更すると制御対象機器900の動作が変わり干渉関数値が変わるので、干渉関数Bを制御指令値uの関数として表記している。
ステップS241の後、障害物回避制御装置400は図7の処理を終了する。
ステップS241の後、障害物回避制御装置400は図7の処理を終了する。
ステップS231の処理とステップS241の処理とを比較すると、ステップS231の処理では、uの微小な変化を用いた試行、および、式(10)のような近似を用いる必要が無い。この点で、ステップS231の処理のほうがステップS241の処理よりも、得られるΔB(x,u)の精度が高いと期待される。一方、ステップS231でΔB(x,u)を算出できない場合でも、ステップS241の処理によれば算出できると期待される。
そこで、干渉関数演算部491は、ステップS231の処理でΔB(x,u)を算出可能か判定し、算出可能であればステップS231の処理を行う。一方、ステップS231の処理ではΔB(x,u)を算出できない場合、干渉関数演算部491は、ステップS241の処理を行う。
そこで、干渉関数演算部491は、ステップS231の処理でΔB(x,u)を算出可能か判定し、算出可能であればステップS231の処理を行う。一方、ステップS231の処理ではΔB(x,u)を算出できない場合、干渉関数演算部491は、ステップS241の処理を行う。
以上のように、ノミナル指令値取得部493は、ノミナル指令値を取得する。ノミナル指令値は、制御対象機器900に対する指令値を求めるために用いられる、いわば基準となる制御指令値である。回避指令値演算部495は、回避指令値を求める。回避指令値は、式(3)のように、制御対象機器900が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、式(2)のように、ノミナル指令値との相違の度合いの小ささに関する所定の条件を満たす制御指令値である。機器制御部492は、ノミナル指令値、および、回避指令値演算部495による処理の結果のうち何れか一方に基づいて、制御対象機器900を制御する。
障害物回避制御装置400では、制御対象機器900と障害物との接触を回避するための制約条件を設けることができる。制御対象機器900と障害物との接触を回避するための基準点が制御対象機器900に複数設定される場合、基準点毎に制約条件を設けることができる。このように障害物回避制御装置400では、制御対象機器900について障害物を回避するように制御する基準点は1か所に限定されず、制御対象機器900のいろいろな部分について、障害物を回避するように制御することができる。
また、障害物回避制御装置400では、障害物が複数ある場合、障害物毎に制約条件を設けることができる。この点で、障害物回避制御装置400によれば、障害物が複数ある場合に比較的容易に対応できる。
また、障害物回避制御装置400では、障害物が複数ある場合、障害物毎に制約条件を設けることができる。この点で、障害物回避制御装置400によれば、障害物が複数ある場合に比較的容易に対応できる。
また、ノミナル指令値取得部493は、ノミナル指令値としてノミナル指令値を取得する。ノミナル指令値は、制御対象機器900による障害物回避を考慮しない場合の制御指令値である。
ノミナル指令値は、制御対象機器900に設定されている目標を達成するように設定される指令値であり、障害物回避制御装置400が、障害物回避を考慮した回避指令値をノミナル指令値に近付けるように生成することで、目標を達成できると期待される。
障害物回避制御装置400によれば、このように、回避指令値を生成する際にノミナル指令値を基準として用いることができ、この点で、回避指令値を比較的容易に生成できる。
ノミナル指令値は、制御対象機器900に設定されている目標を達成するように設定される指令値であり、障害物回避制御装置400が、障害物回避を考慮した回避指令値をノミナル指令値に近付けるように生成することで、目標を達成できると期待される。
障害物回避制御装置400によれば、このように、回避指令値を生成する際にノミナル指令値を基準として用いることができ、この点で、回避指令値を比較的容易に生成できる。
また、干渉関数演算部491は、干渉関数値(B(x))、および、干渉関数値の経時変化を示す値(ΔB(x,u))を算出する。干渉関数値は、制御対象機器900と障害物との位置関係に応じた値を示す。回避指令値演算部495は、干渉関数値、および、干渉関数値の経時変化を示す値を用いて、回避指令値を求める。
障害物回避制御装置400によれば、これらの値を用いることで、制御対象機器900が障害物に接触する可能性の度合いを推定することができ、必要に応じて障害物回避等の措置をとることができる。
障害物回避制御装置400では、制御対象機器900と障害物との距離を干渉関数として用いることができ、制御対象機器900の位置と障害物の位置とから容易に関数値を算出できる。この点で、障害物回避制御装置400の負荷が軽くて済む。
障害物回避制御装置400では、制御対象機器900と障害物との距離を干渉関数として用いることができ、制御対象機器900の位置と障害物の位置とから容易に関数値を算出できる。この点で、障害物回避制御装置400の負荷が軽くて済む。
また、回避要否判定部494は、制御対象機器900と障害物との位置関係に基づいて、回避指令値を取得するための処理の要否を判定する。回避指令値演算部495は、回避指令値を取得するための処理が必要であると回避要否判定部494が判定した場合に、回避指令値を求める。
回避指令値を取得するための処理が不要であると回避要否判定部494が判定した場合、ノミナル制御装置300が生成したノミナル指令値を制御対象機器900への指令値として用いることができ、障害物回避制御装置400が新たに指令値を生成する必要が無い。この点で、障害物回避制御装置400の負荷が比較的軽くて済む。
回避指令値を取得するための処理が不要であると回避要否判定部494が判定した場合、ノミナル制御装置300が生成したノミナル指令値を制御対象機器900への指令値として用いることができ、障害物回避制御装置400が新たに指令値を生成する必要が無い。この点で、障害物回避制御装置400の負荷が比較的軽くて済む。
また、機器制御部492は、回避指令値演算部495が回避指令値の取得に失敗したと判定した場合、制御対象機器900を減速させる。
回避指令値演算部495が回避指令値の取得に失敗した場合、制御対象機器900と障害物との接触を回避できない可能性がある。そこで、機器制御部492が制御対象機器900を減速させる。これにより、制御対象機器900が障害物に接触するまでの時間を確保でき、例えば人手で障害物を動かすといった対応が可能である。また、制御対象機器900が障害物に接触した場合でも、制御対象機器900が減速していることで、不具合が生じないか、あるいは、不具合の程度が軽くて済むと期待される。
回避指令値演算部495が回避指令値の取得に失敗した場合、制御対象機器900と障害物との接触を回避できない可能性がある。そこで、機器制御部492が制御対象機器900を減速させる。これにより、制御対象機器900が障害物に接触するまでの時間を確保でき、例えば人手で障害物を動かすといった対応が可能である。また、制御対象機器900が障害物に接触した場合でも、制御対象機器900が減速していることで、不具合が生じないか、あるいは、不具合の程度が軽くて済むと期待される。
また、仮想力場反映部496は、制御対象機器900が障害物に近いほど大きい加速度で障害物から離れる動作を、ノミナル指令値および回避指令値のうち少なくとも何れか一方に反映させる。機器制御部492は、この動作が反映されたノミナル指令値、および、この動作が反映された回避指令値のうち何れか一方に基づいて、制御対象機器900を制御する。
これにより、制御対象機器900は、障害物に近いほど、障害物から離れるように動作する。障害物回避制御装置400によれば、この点で、より確実に、制御対象機器900に障害物を回避させることができる。
これにより、制御対象機器900は、障害物に近いほど、障害物から離れるように動作する。障害物回避制御装置400によれば、この点で、より確実に、制御対象機器900に障害物を回避させることができる。
また、回避指令値演算部495は、制御対象機器900に設定された基準点毎に、その基準点が障害物に接触しないための十分条件を満たす回避指令値を求める。
このように、障害物回避制御装置400では、基準点毎に制約条件の式を設けることができる。障害物回避制御装置400によればこの点で、制御対象機器に基準点が複数設定されている場合に、比較的容易に対応できる。
このように、障害物回避制御装置400では、基準点毎に制約条件の式を設けることができる。障害物回避制御装置400によればこの点で、制御対象機器に基準点が複数設定されている場合に、比較的容易に対応できる。
また、回避指令値演算部495は、障害物毎に、制御対象機器900がその障害物に接触しないための十分条件を満たす回避指令値を求める。
このように、障害物回避制御装置400では、障害物毎に制約条件の式を設けることができる。障害物回避制御装置400によればこの点で、障害物が複数ある場合に、比較的容易に対応できる。
このように、障害物回避制御装置400では、障害物毎に制約条件の式を設けることができる。障害物回避制御装置400によればこの点で、障害物が複数ある場合に、比較的容易に対応できる。
また、回避指令値演算部495は、制御対象機器900と障害物とが最も近い位置における制御対象機器900と障害物との距離に基づいて判定される、制御対象機器900が障害物に接触しないための十分条件を満たす回避指令値を求める。
このように、回避指令値演算部495が、制御対象機器900と障害物とが最も近い位置における制御対象機器900と障害物との距離を用いて回避指令値を求めることで、制御対象機器900と障害物との接触の可能性をより正確に反映した回避指令値を求めることができる。障害物回避制御装置400によればこの点で、制御対象機器900の制御を比較的高精度に行うことができる。
このように、回避指令値演算部495が、制御対象機器900と障害物とが最も近い位置における制御対象機器900と障害物との距離を用いて回避指令値を求めることで、制御対象機器900と障害物との接触の可能性をより正確に反映した回避指令値を求めることができる。障害物回避制御装置400によればこの点で、制御対象機器900の制御を比較的高精度に行うことができる。
<第2実施形態>
図8は、第2実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。図8に示す構成で、障害物回避制御システム2は、撮像装置100と、情報取得装置200と、ノミナル制御装置300と、障害物回避制御装置400と、障害物位置特定装置500とを備える。また、障害物回避制御システム2は制御対象機器900を制御する。
図8は、第2実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。図8に示す構成で、障害物回避制御システム2は、撮像装置100と、情報取得装置200と、ノミナル制御装置300と、障害物回避制御装置400と、障害物位置特定装置500とを備える。また、障害物回避制御システム2は制御対象機器900を制御する。
障害物回避制御システム2は、障害物位置特定装置500を備える点で障害物回避制御システム1の場合と異なる。それ以外の点では、障害物回避制御システム2は障害物回避制御システム1と同様である。障害物回避制御システム2について、障害物回避制御システム1と同様の点の説明を省略し、必要に応じて図2に示す符号、および、図3に示す符号を引用する。
図9は、障害物位置特定装置500の機能構成の例を示す概略ブロック図である。図9に示す構成で、障害物位置特定装置500は、第3通信部510と、第3記憶部580と、第3制御部590とを備える。第3記憶部580は、物体情報記憶部581を備える。第3制御部590は、種類識別部591と、位置特定部592とを備える。
第3通信部510は、他の装置と通信を行う。特に、第3通信部510は、情報取得装置200からの障害物の位置情報を受信する。また、第3通信部510は、位置特定部592の処理が反映された障害物の位置情報を障害物回避制御装置400へ送信する。第3通信部510が、情報取得装置200から受信した情報に加えて、位置特定部592が算出した障害物の位置情報を障害物回避制御装置400へ送信するようにしてもよい。あるいは、第3通信部510が、情報取得装置200から受信した情報に対して、障害物の位置情報を位置特定部592が算出した位置情報に置き換えた情報を、障害物回避制御装置400へ送信するようにしてもよい。
第3記憶部580は、各種データを記憶する。第3記憶部580の機能は、障害物位置特定装置500が備える記憶デバイスを用いて実行される。
物体情報記憶部581は、物体情報を記憶する。物体情報記憶部581は、物体の種類を識別する種類識別情報と、物体の形状および大きさなど物体の位置を特定するための情報とが紐付けられた情報を、物体情報として記憶する。
物体情報記憶部581は、物体情報を記憶する。物体情報記憶部581は、物体の種類を識別する種類識別情報と、物体の形状および大きさなど物体の位置を特定するための情報とが紐付けられた情報を、物体情報として記憶する。
ここでの物体の種類は識別可能なものであればよく、特定のものに限定されない。例えば、物体情報記憶部581が、ペットボトル、コップなど、物体の名称毎の物体情報を記憶していてもよい。あるいは、物体情報記憶部581が、円柱状の物、円形の物など、物体の形状毎の物体情報を記憶していてもよい。また、物体識別情報にペットボトルの情報が含まれる場合にペットボトルの容量がパラメータになっているなど、物体識別情報がパラメータを含んでいてもよい。
第3制御部590は、障害物位置特定装置500の各部を制御して各種処理を実行する。第3制御部590の機能は、障害物位置特定装置500が備えるCPUが、第3記憶部580からプログラムを読み出して実行することで実行される。
種類識別部591は、三次元計測部292が算出した障害物に関する情報に基づいて、障害物となっている物体の種類を特定する。
種類識別部591は、三次元計測部292が算出した障害物に関する情報に基づいて、障害物となっている物体の種類を特定する。
例えば、種類識別部591が、三次元計測部292が算出した情報で示される障害物を、物理的に一纏まりとなっている部分毎に分割するなど、障害物中の物体を抽出するようにしてもよい。そして、種類識別部591が、抽出した物体毎にパターンマッチングを行うことで、物体の種類を特定するようにしてもよい。
種類識別部591が、例えばYOLO(You Only Look Once)またはSSD(Single Shot Multibox Detector)などのように、ニューラルネットワークによる物体検出器として構成されていてもよい。
種類識別部591が、例えばYOLO(You Only Look Once)またはSSD(Single Shot Multibox Detector)などのように、ニューラルネットワークによる物体検出器として構成されていてもよい。
位置特定部592は、三次元計測部292が算出した障害物に関する情報、および、物体情報記憶部581が記憶している物体情報に基づいて、障害物となっている物体の位置を特定する。
例えば、位置特定部592は、種類識別部591が特定した物体の種類に基づいて、物体情報記憶部581が記憶している物体情報から、物体の位置および大きさの情報を読み出す。そして、位置特定部592は、三次元計測部292が算出した障害物に関する情報が示す物体の位置に、物体情報から読み出した位置および大きさの物体があるものとして、その物体の位置情報を算出する。位置特定部592が、物体の位置を座標値の形式で算出するようにしてもよい。
例えば、位置特定部592は、種類識別部591が特定した物体の種類に基づいて、物体情報記憶部581が記憶している物体情報から、物体の位置および大きさの情報を読み出す。そして、位置特定部592は、三次元計測部292が算出した障害物に関する情報が示す物体の位置に、物体情報から読み出した位置および大きさの物体があるものとして、その物体の位置情報を算出する。位置特定部592が、物体の位置を座標値の形式で算出するようにしてもよい。
これにより、位置特定部592は、障害物を三次元的な広がりをもつ物体として捉え、その座標を算出する。例えば、撮像装置100が障害物を一方の側から撮像し、反対側の画像を得られていない場合に、位置特定部592は、画像に写っていない部分の障害物の位置情報を算出することができる。
位置特定部592は、算出した物体毎の位置情報を、第3通信部510を介して障害物回避制御装置400へ送信する。上述したように、第3通信部510が、情報取得装置200から受信した情報に加えて、位置特定部592が算出した物体毎の位置情報を障害物回避制御装置400へ送信するようにしてもよい。あるいは、第3通信部510が、情報取得装置200から受信した情報に対して、障害物の位置情報を位置特定部592が算出した位置情報に置き換えた情報を、障害物回避制御装置400へ送信するようにしてもよい。
図10は、障害物回避制御システム2におけるデータの流れの例を示す図である。
図10を図4と比較すると、図10の障害物回避制御システム2は、障害物位置特定装置500を備える点で、図4の障害物回避制御システム1と異なる。この違いに伴い、図4では、情報取得装置200が、三次元計測部292が算出した障害物に関する情報を障害物回避制御装置400へ送信するのに対し、図10では、情報取得装置200は、三次元計測部292が算出した障害物に関する情報を障害物位置特定装置500へ送信する。
図10を図4と比較すると、図10の障害物回避制御システム2は、障害物位置特定装置500を備える点で、図4の障害物回避制御システム1と異なる。この違いに伴い、図4では、情報取得装置200が、三次元計測部292が算出した障害物に関する情報を障害物回避制御装置400へ送信するのに対し、図10では、情報取得装置200は、三次元計測部292が算出した障害物に関する情報を障害物位置特定装置500へ送信する。
図10では、障害物位置特定装置500は、情報取得装置200から受信した情報に対して、障害物の位置情報を位置特定部592が算出した位置情報に置き換えた情報を、障害物回避制御装置400へ送信する。障害物回避制御装置400では、干渉関数演算部491が、情報取得装置200の状態観測部291が算出した情報および障害物位置特定装置500からの情報を用いて、干渉関数値、干渉関数値の差分、および、ポテンシャル関数値を算出する。
それ以外の点では、図10は図4の場合と同様である。
それ以外の点では、図10は図4の場合と同様である。
以上のように、種類識別部591は、障害物となっている物体の種類を識別する。位置特定部592は、物体の種類毎に物体の大きさおよび形状を示す物体情報から、種類識別部591が識別した種類に応じた情報を読み出し、読み出した情報を用いて障害物となっている物体の位置を特定する。
これにより、障害物回避制御システム2では、障害物の位置情報をより高精度に得られる。例えば、障害物位置特定装置500は、撮像装置100の画像に写っていない部分について、障害物の位置情報を補完することができる。
障害物の位置情報をより高精度に得られることで、障害物回避制御装置400は、制御対象機器900に障害物を回避させるための指令値を、より高精度に算出することができる。
障害物の位置情報をより高精度に得られることで、障害物回避制御装置400は、制御対象機器900に障害物を回避させるための指令値を、より高精度に算出することができる。
<第3実施形態>
図11は、第3実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。図11に示す構成で、障害物回避制御システム3は、撮像装置100と、情報取得装置200と、ノミナル制御装置300と、障害物回避制御装置400と、障害物位置特定装置500と、障害物動作予測装置600とを備える。また、障害物回避制御システム3は制御対象機器900を制御する。
図11は、第3実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。図11に示す構成で、障害物回避制御システム3は、撮像装置100と、情報取得装置200と、ノミナル制御装置300と、障害物回避制御装置400と、障害物位置特定装置500と、障害物動作予測装置600とを備える。また、障害物回避制御システム3は制御対象機器900を制御する。
障害物回避制御システム3は、障害物動作予測装置600を備える点で、障害物回避制御システム2と異なる。
それ以外の点では、障害物回避制御システム3は障害物回避制御システム2と同様である。障害物回避制御システム3について、障害物回避制御システム2と同様の点の説明を省略し、必要に応じて図2に示す符号、図3に示す符号、および、図9に示す符号を引用する。
それ以外の点では、障害物回避制御システム3は障害物回避制御システム2と同様である。障害物回避制御システム3について、障害物回避制御システム2と同様の点の説明を省略し、必要に応じて図2に示す符号、図3に示す符号、および、図9に示す符号を引用する。
図12は、障害物動作予測装置600の機能構成の例を示す概略ブロック図である。図12に示す構成で、障害物動作予測装置600は、第4通信部610と、第4記憶部680と、第4制御部690とを備える。第4記憶部680は、物体動作情報記憶部681を備える。第4制御部690は、位置予測部691を備える。
第4通信部610は、他の装置と通信を行う。障害物回避制御システム3では、障害物位置特定装置500が、種類識別部591が特定した障害物の種類を示す情報と、位置特定部592が算出した障害物の位置情報とを紐付けて送信する。第4通信部610は、障害物位置特定装置500が送信した情報を受信する。また、第4通信部610は、障害物位置特定装置500から受信した情報に、位置予測部691が予測する障害物の将来の位置情報を加えた情報を、障害物回避制御装置400へ送信する。
第4記憶部680は、各種データを記憶する。第4記憶部680の機能は、障害物動作予測装置600が備える記憶デバイスを用いて実行される。
物体動作情報記憶部681は、物体動作情報を記憶する。物体動作情報記憶部681は、物体の種類を識別する種類識別情報と、物体の動作を予測するための情報とが紐付けられた情報を、物体動作情報として記憶する。
物体動作情報記憶部681が、物体の動作モデルを物体動作情報として記憶するようにしてもよい。あるいは、物体動作情報記憶部681が、障害物として検出された物体の位置の履歴情報を、物体動作情報として記憶するようにしてもよい。
物体動作情報記憶部681は、物体動作情報を記憶する。物体動作情報記憶部681は、物体の種類を識別する種類識別情報と、物体の動作を予測するための情報とが紐付けられた情報を、物体動作情報として記憶する。
物体動作情報記憶部681が、物体の動作モデルを物体動作情報として記憶するようにしてもよい。あるいは、物体動作情報記憶部681が、障害物として検出された物体の位置の履歴情報を、物体動作情報として記憶するようにしてもよい。
第4制御部690は、障害物動作予測装置600の各部を制御して各種処理を実行する。第4制御部690の機能は、障害物動作予測装置600が備えるCPUが、第4記憶部680からプログラムを読み出して実行することで実行される。
位置予測部691は、障害物となっている物体の将来の位置を特定する。例えば、位置予測部691は、障害物位置特定装置500の種類識別部591が特定した物体の種類の識別情報を用いて、その物体の動作を予測するための情報を物体動作情報記憶部681から読み出す。そして、位置予測部691は、読み出した情報を用いて、制御ステップ間における物体の移動量を予測する。位置予測部691は、障害物位置特定装置500の位置特定部592が算出した物体の位置に、予測した移動量を加算することで、次の制御ステップにおける物体の位置を予測する。
位置予測部691は、障害物となっている物体の将来の位置を特定する。例えば、位置予測部691は、障害物位置特定装置500の種類識別部591が特定した物体の種類の識別情報を用いて、その物体の動作を予測するための情報を物体動作情報記憶部681から読み出す。そして、位置予測部691は、読み出した情報を用いて、制御ステップ間における物体の移動量を予測する。位置予測部691は、障害物位置特定装置500の位置特定部592が算出した物体の位置に、予測した移動量を加算することで、次の制御ステップにおける物体の位置を予測する。
図13は、障害物回避制御システム3におけるデータの流れの例を示す図である。
図13を図10と比較すると、図13の障害物回避制御システム3は、障害物動作予測装置600を備える点で、図10の障害物回避制御システム2と異なる。この違いに伴い、図10では、障害物位置特定装置500が、障害物回避制御装置400へ情報を送信するのに対し、図13では、障害物位置特定装置500は、障害物動作予測装置600へ情報を送信し、障害物動作予測装置600が、障害物回避制御装置400へ情報を送信する。
図13を図10と比較すると、図13の障害物回避制御システム3は、障害物動作予測装置600を備える点で、図10の障害物回避制御システム2と異なる。この違いに伴い、図10では、障害物位置特定装置500が、障害物回避制御装置400へ情報を送信するのに対し、図13では、障害物位置特定装置500は、障害物動作予測装置600へ情報を送信し、障害物動作予測装置600が、障害物回避制御装置400へ情報を送信する。
図13では、障害物位置特定装置500は、物体毎に、物体の種類を示す情報と、物体の位置情報とを紐付けた情報を、障害物動作予測装置600へ送信する。障害物動作予測装置600は、物体毎に、物体の種類を示す情報と、物体の位置情報と、物体の位置の予測情報とが紐付けられた情報を、障害物回避制御装置400へ送信する。障害物回避制御装置400では、干渉関数演算部491が、情報取得装置200の状態観測部291が算出した情報および障害物動作予測装置600からの情報を用いて、干渉関数値、干渉関数値の差分、および、ポテンシャル関数値を算出する。
それ以外の点では、図13は図10の場合と同様である。
それ以外の点では、図13は図10の場合と同様である。
以上のように、障害物動作予測装置600は、物体の種類毎に前記物体の動きに関する情報を示す物体動作情報から、識別された種類に応じた情報を読み出し、読み出した情報を用いて障害物の将来の位置を予測する。障害物回避制御装置400の干渉関数演算部491は、予測された障害物の将来の位置に基づいて、干渉関数値の経時変化を示す値を算出する。
障害物回避制御システム3は、障害物の将来の位置の予測に、障害物となっている物体の特性として示されるその物体の動作に関する情報を用いることができ、この点で、障害物の将来の位置を比較的高精度に予測できる。この障害物の将来の位置の予測結果を制御対象機器900の障害物回避に用いる点で、障害物回避制御システム3は、制御対象機器900の障害物回避を比較的高精度に行うことができる。
<第4実施形態>
図14は、第4実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。図14に示す構成で、障害物回避制御システム4は、撮像装置100と、情報取得装置200と、障害物回避制御装置401とを備える。また、障害物回避制御システム2は制御対象機器900を制御する。
図14は、第4実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。図14に示す構成で、障害物回避制御システム4は、撮像装置100と、情報取得装置200と、障害物回避制御装置401とを備える。また、障害物回避制御システム2は制御対象機器900を制御する。
障害物回避制御システム4は、ノミナル制御装置300を備えておらず、障害物回避制御装置400に代えて障害物回避制御装置401を備える点で障害物回避制御装置400と異なる。障害物回避制御装置401では、回避指令値を求める処理で用いる制約条件の1つに、制御対象機器900に設定される目標を制御指令値に反映させるための制約条件が含まれる。
それ以外の点では、障害物回避制御システム4は障害物回避制御システム1と同様である。障害物回避制御システム4について、障害物回避制御システム1と同様の点の説明を省略し、必要に応じて図2に示す符号を引用する。
それ以外の点では、障害物回避制御システム4は障害物回避制御システム1と同様である。障害物回避制御システム4について、障害物回避制御システム1と同様の点の説明を省略し、必要に応じて図2に示す符号を引用する。
以下では、第1実施形態に基づいて第4実施形態を実施する場合を例に説明するが、第2実施形態または第3実施形態に基づいて第4実施形態を実施するようにしてもよい。特に、障害物回避制御システム4が、第2実施形態にかかる障害物位置特定装置500を備えていてもよい。障害物回避制御システム4が、第3実施形態にかかる障害物位置特定装置500および障害物動作予測装置600を備えていてもよい。
図15は、障害物回避制御装置401の機能構成の例を示す概略ブロック図である。図15に示す構成で、障害物回避制御装置401は、第2通信部410と、第2記憶部480と、第2制御部490とを備える。第2記憶部480は、構造情報記憶部481と、動的モデル記憶部482と、制御関数記憶部483とを備える。第2制御部490は、干渉関数演算部491と、機器制御部492とを備える。機器制御部492は、回避指令値演算部495と、仮想力場反映部496とを備える。
障害物回避制御装置401は、第2記憶部480が制御関数記憶部483を備える点、および、機器制御部492がノミナル指令値取得部493と回避要否判定部494とを備えていない点で障害物回避制御装置400と異なる。この違いは、制御対象機器900に設定される目標を制御指令値に反映させる方法の違いに起因する。
障害物回避制御装置400では、回避指令値演算部495が、評価関数を用いて回避指令値をノミナル指令値に近付けることで、制御対象機器900に設定される目標を回避指令値に反映させる。一方、障害物回避制御装置401では、回避指令値演算部495が回避指令値を求める処理で用いる制約条件の1つに、制御対象機器900に設定される目標を制御指令値に反映させるための制約条件が含まれる。
それ以外の点では、障害物回避制御装置401は障害物回避制御装置400と同様である。
それ以外の点では、障害物回避制御装置401は障害物回避制御装置400と同様である。
第4実施形態で回避指令値演算部495が解く最適化問題における制約条件は、3種類の式で示される。
3種類の式のうち1種類目は、第1実施形態の場合と同様、制御対象機器900が障害物に接触しないための十分条件を示す式である。3種類の式のうち1種類目は、例えば上述した式(3)のように示される。
3種類の式のうち2種類目は、第1実施形態の場合と同様、制御指令値が、制御対象機器900の可動部の仕様により規定される上下限値の範囲内で設定されるという制約条件を示す式である。3種類の式のうち2種類目は、例えば上述した式(7)のように示される。
3種類の式のうち1種類目は、第1実施形態の場合と同様、制御対象機器900が障害物に接触しないための十分条件を示す式である。3種類の式のうち1種類目は、例えば上述した式(3)のように示される。
3種類の式のうち2種類目は、第1実施形態の場合と同様、制御指令値が、制御対象機器900の可動部の仕様により規定される上下限値の範囲内で設定されるという制約条件を示す式である。3種類の式のうち2種類目は、例えば上述した式(7)のように示される。
一方、3種類の式のうち3種類目は、制御対象機器900に設定される目標が制御指令の実行によって安定に達成される度合いに関する条件を示す。
例えば、リアプノフ関数をVで示し、制御ステップ間におけるリアプノフ関数値の変化量をΔVで示して、制約条件を示す3種類目の式を式(11)のようにしてもよい。
例えば、リアプノフ関数をVで示し、制御ステップ間におけるリアプノフ関数値の変化量をΔVで示して、制約条件を示す3種類目の式を式(11)のようにしてもよい。
「d」は、制約条件を緩和して解を得易くするために設けられている。
d=0として解を得られた場合、その解は、制御対象機器900に設定される目標を厳密に達成するための制御指令値である。一方、d=0の場合、解をピンポイントで探索することになり、解を得られないことが懸念される。
そこで、d≧0とすることで、制御指令値に基づく制御対象機器900の動作結果と目標とのずれを許容して、解の探索範囲を広げることができる。以下では、制御対象機器900の動作結果と目標とのずれを誤差と称する。「d」の値が大きくなるほど、許容する誤差が大きくなり、解を得易くなる。
d=0として解を得られた場合、その解は、制御対象機器900に設定される目標を厳密に達成するための制御指令値である。一方、d=0の場合、解をピンポイントで探索することになり、解を得られないことが懸念される。
そこで、d≧0とすることで、制御指令値に基づく制御対象機器900の動作結果と目標とのずれを許容して、解の探索範囲を広げることができる。以下では、制御対象機器900の動作結果と目標とのずれを誤差と称する。「d」の値が大きくなるほど、許容する誤差が大きくなり、解を得易くなる。
以下では、「d」がd≧0の変数である場合の例を説明する。この場合、後述するように最適化問題における評価関数に、「d」の値の大きさの評価のための項を設けるようにしてもよい。
第4実施形態で回避指令値演算部495が解く最適化問題における制約条件を示す3種類目の式は、式(11)に限定されず、制御対象機器900に設定される目標の達成度合いと、解の得易さとのトレードオフを調整可能ないろいろな式とすることができる。
例えば、第4実施形態で回避指令値演算部495が解く最適化問題における制約条件を示す3種類目の式を、式(12)のようにしてもよい。
例えば、第4実施形態で回避指令値演算部495が解く最適化問題における制約条件を示す3種類目の式を、式(12)のようにしてもよい。
関数Dは、制御対象機器900に設定される目標値と実際値とのユークリッド距離を算出する関数である。制御対象機器900に設定される目標値がベクトルで示されているとして、回避指令値演算部495は、関数Dの演算で、そのベクトルの全ての要素について目標値と実際値との差の2乗を足し合わせて、その平方根を演算する。
関数ΔDは、制御対象機器900に設定される目標値と実際値とのユークリッド距離の、制御ステップ間における変化量を算出する関数である。例えば、制御ステップtにおける関数値D(x)をD(xt)と表記し、制御ステップt+1における関数値D(x)をD(xt+1)と表記する。また、制御ステップtから制御ステップt+1までの時間における関数値D(x)の変化量ΔD(x,u)は、ΔD(xt,ut)=D(xt+1)-D(xt)と示される。
式(12)は、制御対象機器900の安定性を保証する条件を緩和した制約式である。例えば、回避指令値演算部495で、ΔD(x,u)<0を満たす制御指令uが常に求まれば、制御対象機器900は安定に目標値を達成することができる。
第4実施形態で回避指令値演算部495が解く最適化問題における目的関数は、例えば式(13)のように示される。
式13の「uTu」の項は、制御指令値uのベクトルとしてのノルムの2乗を示す。ここでいうノルムは、ユークリッドノルムまたはユークリッド距離とも称される。
式13の「uTu」の項は、制御対象機器900が無駄な動作をしないよう、制御指令値の大きさをなるべく小さくするために設けられている。
式13の「uTu」の項は、制御対象機器900が無駄な動作をしないよう、制御指令値の大きさをなるべく小さくするために設けられている。
式13の「p・d2」の項は、式(11)または式(12)における「d」の大きさを評価するための項である。「p・d2」の「p」は、「uTu」と「d2」の重み付けを調整するための重みを示す。「p」は、例えばp>0の定数として設定される。
この最適化問題の解の候補が2つ検出された場合、2つの解の候補の「uTu」の項の値が同じであれば、「p・d2」の項の値が小さい方が最適解として採用される。
式(13)は、評価関数の例に該当する。式(13)における最小解となる制御指令値uは、制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値の例に該当する。
この最適化問題の解の候補が2つ検出された場合、2つの解の候補の「uTu」の項の値が同じであれば、「p・d2」の項の値が小さい方が最適解として採用される。
式(13)は、評価関数の例に該当する。式(13)における最小解となる制御指令値uは、制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値の例に該当する。
図16は、障害物回避制御システム4におけるデータの流れの例を示す図である。
図16を図4と比較すると、図16では障害物回避制御システム4がノミナル制御装置300を備えていない。これに伴い図16では、情報取得装置200からの制御対象に関する値が機器制御部492へ入力されている。
それ以外の点では、図16は図4の場合と同様である。
図16を図4と比較すると、図16では障害物回避制御システム4がノミナル制御装置300を備えていない。これに伴い図16では、情報取得装置200からの制御対象に関する値が機器制御部492へ入力されている。
それ以外の点では、図16は図4の場合と同様である。
図17は、障害物回避制御装置401が制御対象機器900に対する制御指令値を取得する処理手順の例を示すフローチャートである。障害物回避制御装置401は、制御対象機器900に対する制御の制御ステップ毎に図17の処理を行う。
図17のステップS311における処理は、図5のステップS111における処理と同様である。
ステップS311の後、回避指令値演算部495は、上述した最適化問題の演算による制御指令値の取得を試みる(ステップS312)。
機器制御部492は、回避指令値演算部495がステップS312で最適化問題の解を得られたか否かを判定する(ステップS313)。
図17のステップS311における処理は、図5のステップS111における処理と同様である。
ステップS311の後、回避指令値演算部495は、上述した最適化問題の演算による制御指令値の取得を試みる(ステップS312)。
機器制御部492は、回避指令値演算部495がステップS312で最適化問題の解を得られたか否かを判定する(ステップS313)。
解を得られたと機器制御部492が判定した場合(ステップS313:YES)、処理がステップS321へ進む。ステップS321における処理は、図5のステップS141における処理と同様である。ステップS321の後、処理がステップS341へ進む。ステップS341における処理は、図5のステップS161における処理と同様である。ステップS341の後、障害物回避制御装置401は図17の処理を終了する。
一方、ステップS313で解を得られなかったと機器制御部492が判定した場合(ステップS313:NO)、処理がステップS3331へ進む。ステップS331における処理は、図5のステップS151における処理と同様である。ステップS331の後、処理がステップS341に進む。
以上のように、障害物回避制御装置400の場合と同様、障害物回避制御装置401でも、制御対象機器900と障害物との接触を回避するための制約条件を設けることができる。制御対象機器900に、障害物との接触を回避するための基準点が複数設定されている場合、基準点毎に制約条件を設けることができる。このように障害物回避制御装置401によれば、制御対象機器900と障害物との接触を回避するための基準点を複数設定することができ、制御対象機器900のいろいろな部分について、障害物を回避するように制御することができる。
また、障害物回避制御装置401では、障害物が複数ある場合、障害物毎に制約条件を設けることができる。この点で、障害物回避制御装置401によれば、障害物が複数ある場合に比較的容易に対応できる。
また、障害物回避制御装置401では、障害物が複数ある場合、障害物毎に制約条件を設けることができる。この点で、障害物回避制御装置401によれば、障害物が複数ある場合に比較的容易に対応できる。
<第5実施形態>
図18は、第5実施形態に係る障害物回避制御装置の構成の例を示す図である。図18に示す障害物回避制御装置10は、回避指令値演算部11と、機器制御部12とを備える。
かかる構成で、回避指令値演算部11は、制御対象機器に対する制御指令値であって、制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める。機器制御部12は、回避指令値演算部11による処理の結果に基づいて、制御対象機器を制御する。
図18は、第5実施形態に係る障害物回避制御装置の構成の例を示す図である。図18に示す障害物回避制御装置10は、回避指令値演算部11と、機器制御部12とを備える。
かかる構成で、回避指令値演算部11は、制御対象機器に対する制御指令値であって、制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める。機器制御部12は、回避指令値演算部11による処理の結果に基づいて、制御対象機器を制御する。
障害物回避制御装置10では、制御対象機器と障害物との接触を回避するための制約条件を設けることができる。制御対象機器と障害物との接触を回避するための基準点が制御対象物に複数設定されている場合、基準点毎に制約条件を設けることができる。このように障害物回避制御装置10では、制御対象機器について障害物を回避するための基準点は1か所に限定されず、制御対象機器のいろいろな部分について、障害物を回避するように制御することができる。
また、障害物回避制御装置10では、障害物が複数ある場合、障害物毎に制約条件を設けることができる。この点で、障害物回避制御装置10によれば、障害物が複数ある場合に比較的容易に対応できる。
また、障害物回避制御装置10では、障害物が複数ある場合、障害物毎に制約条件を設けることができる。この点で、障害物回避制御装置10によれば、障害物が複数ある場合に比較的容易に対応できる。
<第6実施形態>
図19は、第6実施形態に係る障害物回避制御方法における処理の手順の例を示す図である。図19に示す障害物回避制御方法は、制御対象機器に対する制御指令値であって、制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める工程(ステップS11)と、ステップS11での処理の結果に基づいて、制御対象機器を制御する工程(ステップS12)とを含む。
図19は、第6実施形態に係る障害物回避制御方法における処理の手順の例を示す図である。図19に示す障害物回避制御方法は、制御対象機器に対する制御指令値であって、制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める工程(ステップS11)と、ステップS11での処理の結果に基づいて、制御対象機器を制御する工程(ステップS12)とを含む。
この障害物回避制御方法では、制御対象機器と障害物との接触を回避するための制約条件を設けることができる。制御対象機器と障害物との接触を回避するための基準点が制御対象物に複数設定されている場合、基準点毎に制約条件を設けることができる。このように障害物回避制御装置10では、制御対象機器について障害物を回避するための基準点は1か所に限定されず、制御対象機器のいろいろな部分について、障害物を回避するように制御することができる。
図20は、少なくとも1つの実施形態に係るコンピュータの構成を示す概略ブロック図である。
図20に示す構成で、コンピュータ700は、CPU710と、主記憶装置720と、補助記憶装置730と、インタフェース740とを備える。
上記の情報取得装置200、ノミナル制御装置300、障害物回避制御装置400、障害物回避制御装置401、障害物位置特定装置500、および、障害物動作予測装置600のうち何れか1つ以上が、コンピュータ700に実装されてもよい。その場合、上述した各処理部の動作は、プログラムの形式で補助記憶装置730に記憶されている。CPU710は、プログラムを補助記憶装置730から読み出して主記憶装置720に展開し、当該プログラムに従って上記処理を実行する。また、CPU710は、プログラムに従って、上述した各記憶部に対応する記憶領域を主記憶装置720に確保する。各装置と他の装置との通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
図20に示す構成で、コンピュータ700は、CPU710と、主記憶装置720と、補助記憶装置730と、インタフェース740とを備える。
上記の情報取得装置200、ノミナル制御装置300、障害物回避制御装置400、障害物回避制御装置401、障害物位置特定装置500、および、障害物動作予測装置600のうち何れか1つ以上が、コンピュータ700に実装されてもよい。その場合、上述した各処理部の動作は、プログラムの形式で補助記憶装置730に記憶されている。CPU710は、プログラムを補助記憶装置730から読み出して主記憶装置720に展開し、当該プログラムに従って上記処理を実行する。また、CPU710は、プログラムに従って、上述した各記憶部に対応する記憶領域を主記憶装置720に確保する。各装置と他の装置との通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
情報取得装置200がコンピュータ700に実装される場合、第1制御部290およびその各部の動作は、プログラムの形式で補助記憶装置730に記憶されている。CPU710は、プログラムを補助記憶装置730から読み出して主記憶装置720に展開し、当該プログラムに従って上記処理を実行する。
また、CPU710は、プログラムに従って、第1記憶部280に対応する記憶領域を主記憶装置720に確保する。第1通信部210が行う通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
また、CPU710は、プログラムに従って、第1記憶部280に対応する記憶領域を主記憶装置720に確保する。第1通信部210が行う通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
障害物回避制御装置400がコンピュータ700に実装される場合、第2制御部490およびその各部の動作は、プログラムの形式で補助記憶装置730に記憶されている。CPU710は、プログラムを補助記憶装置730から読み出して主記憶装置720に展開し、当該プログラムに従って上記処理を実行する。
また、CPU710は、プログラムに従って、第2記憶部480およびその各部に対応する記憶領域を主記憶装置720に確保する。第2通信部410が行う通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
また、CPU710は、プログラムに従って、第2記憶部480およびその各部に対応する記憶領域を主記憶装置720に確保する。第2通信部410が行う通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
障害物回避制御装置401がコンピュータ700に実装される場合、第2制御部490およびその各部の動作は、プログラムの形式で補助記憶装置730に記憶されている。CPU710は、プログラムを補助記憶装置730から読み出して主記憶装置720に展開し、当該プログラムに従って上記処理を実行する。
また、CPU710は、プログラムに従って、第2記憶部480およびその各部に対応する記憶領域を主記憶装置720に確保する。第2通信部410が行う通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
また、CPU710は、プログラムに従って、第2記憶部480およびその各部に対応する記憶領域を主記憶装置720に確保する。第2通信部410が行う通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
障害物位置特定装置500がコンピュータ700に実装される場合、第3制御部590およびその各部の動作は、プログラムの形式で補助記憶装置730に記憶されている。CPU710は、プログラムを補助記憶装置730から読み出して主記憶装置720に展開し、当該プログラムに従って上記処理を実行する。
また、CPU710は、プログラムに従って、第3記憶部580およびその各部に対応する記憶領域を主記憶装置720に確保する。第3通信部510が行う通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
また、CPU710は、プログラムに従って、第3記憶部580およびその各部に対応する記憶領域を主記憶装置720に確保する。第3通信部510が行う通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
障害物動作予測装置600がコンピュータ700に実装される場合、第4制御部690およびその各部の動作は、プログラムの形式で補助記憶装置730に記憶されている。CPU710は、プログラムを補助記憶装置730から読み出して主記憶装置720に展開し、当該プログラムに従って上記処理を実行する。
また、CPU710は、プログラムに従って、第4記憶部680およびその各部に対応する記憶領域を主記憶装置720に確保する。第4通信部610が行う通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
また、CPU710は、プログラムに従って、第4記憶部680およびその各部に対応する記憶領域を主記憶装置720に確保する。第4通信部610が行う通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
なお、情報取得装置200、ノミナル制御装置300、障害物回避制御装置400、障害物回避制御装置401、障害物位置特定装置500、および、障害物動作予測装置600の全部または一部の機能を実現するためのプログラムをコンピュータ読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピュータシステムに読み込ませ、実行することにより各部の処理を行ってもよい。ここでいう「コンピュータシステム」とは、OS(オペレーティングシステム)や周辺機器等のハードウェアを含む。
「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ROM(Read Only Memory)、CD-ROM(Compact Disc Read Only Memory)等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。また上記プログラムは、前述した機能の一部を実現するためのものであっても良く、さらに前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるものであっても良い。
「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ROM(Read Only Memory)、CD-ROM(Compact Disc Read Only Memory)等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。また上記プログラムは、前述した機能の一部を実現するためのものであっても良く、さらに前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるものであっても良い。
以上、本発明の実施形態について図面を参照して詳述してきたが、具体的な構成はこの実施形態に限られるものではなく、この発明の要旨を逸脱しない範囲の設計変更等も含まれる。
本発明の実施形態は、障害物回避制御装置、障害物回避制御システム、障害物回避制御方法および記録媒体に適用してもよい。
1、2、3、4 障害物回避制御システム
10、400、401 障害物回避制御装置
11、495 回避指令値演算部
12、492 機器制御部
100 撮像装置
200 情報取得装置
210 第1通信部
280 第1記憶部
290 第1制御部
291 状態観測部
292 三次元計測部
300 ノミナル制御装置
410 第2通信部
480 第2記憶部
481 構造情報記憶部
482 動的モデル記憶部
483 制御関数記憶部
490 第2制御部
491 干渉関数演算部
493 ノミナル指令値取得部
494 回避要否判定部
496 仮想力場反映部
500 障害物位置特定装置
510 第3通信部
580 第3記憶部
581 物体情報記憶部
590 第3制御部
591 種類識別部
592 位置特定部
600 障害物動作予測装置
610 第4通信部
680 第4記憶部
681 物体動作情報記憶部
690 第4制御部
691 位置予測部
10、400、401 障害物回避制御装置
11、495 回避指令値演算部
12、492 機器制御部
100 撮像装置
200 情報取得装置
210 第1通信部
280 第1記憶部
290 第1制御部
291 状態観測部
292 三次元計測部
300 ノミナル制御装置
410 第2通信部
480 第2記憶部
481 構造情報記憶部
482 動的モデル記憶部
483 制御関数記憶部
490 第2制御部
491 干渉関数演算部
493 ノミナル指令値取得部
494 回避要否判定部
496 仮想力場反映部
500 障害物位置特定装置
510 第3通信部
580 第3記憶部
581 物体情報記憶部
590 第3制御部
591 種類識別部
592 位置特定部
600 障害物動作予測装置
610 第4通信部
680 第4記憶部
681 物体動作情報記憶部
690 第4制御部
691 位置予測部
Claims (13)
- 制御対象機器に対する制御指令値であって、前記制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める回避指令値演算部と、
前記回避指令値演算部による処理の結果に基づいて、前記制御対象機器を制御する機器制御部と、
を備える障害物回避制御装置。 - 前記制御対象機器による障害物回避を考慮しない場合の制御指令値であるノミナル指令値を取得するノミナル指令値取得部をさらに備え、
前記回避指令値演算部は、求める制御指令値と前記ノミナル指令値との相違の度合いが所定の条件を満たすという前記終了条件を用いて前記回避指令値を求め、
前記機器制御部は、前記ノミナル指令値および前記回避指令値演算部による処理の結果のうち何れか一方に基づいて、前記制御対象機器を制御する、
請求項1に記載の障害物回避制御装置。 - 前記制御対象機器が、前記障害物に近いほど前記障害物から離れる動作を、前記ノミナル指令値および前記回避指令値のうち少なくとも何れか一方に反映させる仮想力場反映部をさらに備え、
前記機器制御部は、前記動作が反映された前記ノミナル指令値、および、前記動作が反映された前記回避指令値のうち何れか一方に基づいて、前記制御対象機器を制御する、請求項2に記載の障害物回避制御装置。 - 前記制御対象機器と前記障害物との位置関係に応じた値を示す干渉関数値、および、前記干渉関数値の経時変化を示す値を算出する干渉関数演算部をさらに備え、
前記回避指令値演算部は、前記干渉関数値、および、前記干渉関数値の経時変化を示す値を用いて前記回避指令値を求める、
請求項1から3の何れか一項に記載の障害物回避制御装置。 - 前記制御対象機器と前記障害物との位置関係に基づいて、前記回避指令値を取得するための処理を行う必要の有無を判定する回避要否判定部をさらに備え、
前記回避指令値演算部は、前記回避指令値を取得するための処理を行う必要があると前記回避要否判定部が判定した場合に、前記回避指令値を求める
請求項1から4の何れか一項に記載の障害物回避制御装置。 - 前記機器制御部は、前記回避指令値演算部が前記回避指令値の取得に失敗したと判定した場合、前記制御対象機器を減速させる、請求項1から5の何れか一項に記載の障害物回避制御装置。
- 前記回避指令値演算部は、前記制御対象機器に設定された基準点毎に、その基準点が前記障害物に接触しないための十分条件を満たす前記回避指令値を求める、請求項1から6の何れか一項に記載の障害物回避制御装置。
- 前記回避指令値演算部は、障害物毎に、前記制御対象機器がその障害物に接触しないための十分条件を満たす前記回避指令値を求める、請求項1から7の何れか一項に記載の障害物回避制御装置。
- 前記回避指令値演算部は、前記制御対象機器と前記障害物とが最も近い位置における前記制御対象機器と前記障害物との距離に基づいて判定される、前記制御対象機器が前記障害物に接触しないための十分条件を満たす前記回避指令値を求める、請求項1から6の何れか一項に記載の障害物回避制御装置。
- 請求項4に記載の障害物回避制御装置と、
前記障害物となっている物体の種類を識別する種類識別部と、
前記物体の種類毎に前記物体の大きさおよび形状を示す物体情報から、識別された種類に応じた情報を読み出し、読み出した情報を用いて前記障害物の位置を特定する位置特定部と、
をさらに備え、
前記干渉関数演算部は、特定された前記障害物の位置に基づいて、前記干渉関数値、および、前記干渉関数値の経時変化を示す値を算出する、
障害物回避制御システム。 - 前記物体の種類毎に前記物体の動きに関する情報を示す物体動作情報から、識別された種類に応じた情報を読み出し、読み出した情報を用いて前記障害物の将来の位置を予測する位置予測部をさらに備え、
前記干渉関数演算部は、予測された前記障害物の将来の位置に基づいて、前記干渉関数値の経時変化を示す値を算出する、
請求項10に記載の障害物回避制御システム。 - 制御対象機器に対する制御指令値であって、前記制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める工程と、
前記回避指令値を求める工程での処理の結果に基づいて、前記制御対象機器を制御する工程と、
を含む障害物回避制御方法。 - コンピュータに、
制御対象機器に対する制御指令値であって、前記制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める工程と、
前記回避指令値を求める工程での処理の結果に基づいて、前記制御対象機器を制御する工程と、
を実行させるためのプログラムを記録した記録媒体。
Priority Applications (4)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2020569234A JP7180695B2 (ja) | 2019-01-30 | 2019-01-30 | 障害物回避制御装置、障害物回避制御システム、障害物回避制御方法およびプログラム |
US17/426,240 US20220097231A1 (en) | 2019-01-30 | 2019-01-30 | Obstacle avoidance control device, obstacle avoidance control system, obstacle avoidance control method, and recording medium |
EP19914139.1A EP3920001A4 (en) | 2019-01-30 | 2019-01-30 | OBSTACLE AVOIDANCE CONTROL DEVICE, OBSTACLE AVOIDANCE CONTROL SYSTEM, OBSTACLE AVOIDANCE CONTROL METHOD AND RECORDING MEDIA |
PCT/JP2019/003177 WO2020157862A1 (ja) | 2019-01-30 | 2019-01-30 | 障害物回避制御装置、障害物回避制御システム、障害物回避制御方法および記録媒体 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
PCT/JP2019/003177 WO2020157862A1 (ja) | 2019-01-30 | 2019-01-30 | 障害物回避制御装置、障害物回避制御システム、障害物回避制御方法および記録媒体 |
Publications (1)
Publication Number | Publication Date |
---|---|
WO2020157862A1 true WO2020157862A1 (ja) | 2020-08-06 |
Family
ID=71841442
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
PCT/JP2019/003177 WO2020157862A1 (ja) | 2019-01-30 | 2019-01-30 | 障害物回避制御装置、障害物回避制御システム、障害物回避制御方法および記録媒体 |
Country Status (4)
Country | Link |
---|---|
US (1) | US20220097231A1 (ja) |
EP (1) | EP3920001A4 (ja) |
JP (1) | JP7180695B2 (ja) |
WO (1) | WO2020157862A1 (ja) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113119119A (zh) * | 2021-03-30 | 2021-07-16 | 广州大学 | 一种机器人动作的执行方法、装置和存储介质 |
CN113625702A (zh) * | 2020-10-28 | 2021-11-09 | 北京科技大学 | 基于二次规划的无人车同时路径跟踪与避障方法 |
CN114782759A (zh) * | 2022-06-22 | 2022-07-22 | 鲁东大学 | 一种基于YOLOv5网络对密集遮挡鱼类的检测方法 |
WO2023047496A1 (ja) * | 2021-09-22 | 2023-03-30 | 日本電気株式会社 | 制約条件取得装置、制御システム、制約条件取得方法および記録媒体 |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116755562B (zh) * | 2023-07-04 | 2024-04-05 | 深圳市仙瞬科技有限公司 | 一种避障方法、装置、介质及ar/vr设备 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2012011498A (ja) * | 2010-06-30 | 2012-01-19 | Toshiba Corp | ロボットアーム操作システムおよびその操作方法 |
JP2012011494A (ja) * | 2010-06-30 | 2012-01-19 | Yaskawa Electric Corp | ロボットの角度データ変換方法及びこの方法を実施するための制御装置 |
JP2018167333A (ja) | 2017-03-29 | 2018-11-01 | セイコーエプソン株式会社 | ロボット制御装置、ロボットおよびロボットシステム |
JP2018190241A (ja) * | 2017-05-09 | 2018-11-29 | オムロン株式会社 | タスク実行システム、タスク実行方法、並びにその学習装置及び学習方法 |
Family Cites Families (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
SE456048B (sv) * | 1982-02-24 | 1988-08-29 | Philips Norden Ab | Sett och anordning for att bestemma kollisionsrisken for tva inbordes rorliga kroppar |
US5347459A (en) * | 1993-03-17 | 1994-09-13 | National Research Council Of Canada | Real time collision detection |
JP5112666B2 (ja) * | 2006-09-11 | 2013-01-09 | 株式会社日立製作所 | 移動装置 |
JP4730440B2 (ja) * | 2009-01-01 | 2011-07-20 | ソニー株式会社 | 軌道計画装置及び軌道計画方法、並びにコンピューター・プログラム |
JP6378783B2 (ja) * | 2014-12-25 | 2018-08-22 | 川崎重工業株式会社 | アーム型のロボットの障害物自動回避方法及び制御装置 |
JP7058126B2 (ja) * | 2018-01-12 | 2022-04-21 | 株式会社日立製作所 | ロボット制御装置および自動組立システム |
TWI664573B (zh) * | 2018-05-11 | 2019-07-01 | 國立交通大學 | 動作計算裝置、機器人系統及機器人控制方法 |
-
2019
- 2019-01-30 EP EP19914139.1A patent/EP3920001A4/en active Pending
- 2019-01-30 WO PCT/JP2019/003177 patent/WO2020157862A1/ja unknown
- 2019-01-30 JP JP2020569234A patent/JP7180695B2/ja active Active
- 2019-01-30 US US17/426,240 patent/US20220097231A1/en active Pending
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2012011498A (ja) * | 2010-06-30 | 2012-01-19 | Toshiba Corp | ロボットアーム操作システムおよびその操作方法 |
JP2012011494A (ja) * | 2010-06-30 | 2012-01-19 | Yaskawa Electric Corp | ロボットの角度データ変換方法及びこの方法を実施するための制御装置 |
JP2018167333A (ja) | 2017-03-29 | 2018-11-01 | セイコーエプソン株式会社 | ロボット制御装置、ロボットおよびロボットシステム |
JP2018190241A (ja) * | 2017-05-09 | 2018-11-29 | オムロン株式会社 | タスク実行システム、タスク実行方法、並びにその学習装置及び学習方法 |
Non-Patent Citations (1)
Title |
---|
See also references of EP3920001A4 |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113625702A (zh) * | 2020-10-28 | 2021-11-09 | 北京科技大学 | 基于二次规划的无人车同时路径跟踪与避障方法 |
CN113625702B (zh) * | 2020-10-28 | 2023-09-22 | 北京科技大学 | 基于二次规划的无人车同时路径跟踪与避障方法 |
CN113119119A (zh) * | 2021-03-30 | 2021-07-16 | 广州大学 | 一种机器人动作的执行方法、装置和存储介质 |
CN113119119B (zh) * | 2021-03-30 | 2022-07-05 | 广州大学 | 一种机器人动作的执行方法、装置和存储介质 |
WO2023047496A1 (ja) * | 2021-09-22 | 2023-03-30 | 日本電気株式会社 | 制約条件取得装置、制御システム、制約条件取得方法および記録媒体 |
CN114782759A (zh) * | 2022-06-22 | 2022-07-22 | 鲁东大学 | 一种基于YOLOv5网络对密集遮挡鱼类的检测方法 |
CN114782759B (zh) * | 2022-06-22 | 2022-09-13 | 鲁东大学 | 一种基于YOLOv5网络对密集遮挡鱼类的检测方法 |
US11790640B1 (en) | 2022-06-22 | 2023-10-17 | Ludong University | Method for detecting densely occluded fish based on YOLOv5 network |
Also Published As
Publication number | Publication date |
---|---|
JP7180695B2 (ja) | 2022-11-30 |
EP3920001A1 (en) | 2021-12-08 |
JPWO2020157862A1 (ja) | 2021-11-11 |
EP3920001A4 (en) | 2022-01-26 |
US20220097231A1 (en) | 2022-03-31 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
WO2020157862A1 (ja) | 障害物回避制御装置、障害物回避制御システム、障害物回避制御方法および記録媒体 | |
US10189162B2 (en) | Model generation apparatus, information processing apparatus, model generation method, and information processing method | |
Rusu et al. | Laser-based perception for door and handle identification | |
Janabi-Sharifi et al. | Automatic selection of image features for visual servoing | |
CN114072254A (zh) | 不确定环境中利用静态和动态碰撞规避的机器人路径规划方法 | |
JP2003271975A (ja) | 平面抽出方法、その装置、そのプログラム、その記録媒体及び平面抽出装置搭載型ロボット装置 | |
WO2020181257A1 (en) | Collaborative determination of a load footprint of a robotic vehicle | |
JP7180696B2 (ja) | 制御装置、制御方法およびプログラム | |
JP5370122B2 (ja) | 移動***置推定装置及び移動***置推定方法 | |
Sadeghian et al. | Visual servoing with safe interaction using image moments | |
Yu et al. | Indoor Localization Based on Fusion of AprilTag and Adaptive Monte Carlo | |
US20240149458A1 (en) | Robot remote operation control device, robot remote operation control system, robot remote operation control method, and program | |
US20220148119A1 (en) | Computer-readable recording medium storing operation control program, operation control method, and operation control apparatus | |
US20220143836A1 (en) | Computer-readable recording medium storing operation control program, operation control method, and operation control apparatus | |
US11372475B2 (en) | Information processing apparatus, information processing method, and floor modeling system | |
US12017372B2 (en) | Robot control device | |
CN111971149A (zh) | 记录介质、信息处理设备和信息处理方法 | |
US20230202026A1 (en) | Robot Training System | |
CN117207190B (zh) | 基于视觉与触觉融合的精准抓取机器人*** | |
US20220324105A1 (en) | Gripping position determination device, gripping position determination system, gripping position determination method, and recording medium | |
Lloyd et al. | A pose and shear-based tactile robotic system for object tracking, surface following and object pushing | |
Saleem et al. | Obstacle detection by multi-sensor fusion of a laser scanner and depth camera | |
Rowlands | Robotic Control for the Manipulation of 3D Deformable Objects | |
Nisa | People Detection from Laser Range Finder Data | |
KR20230069484A (ko) | 로봇 센서 캘리브레이션 방법 및 이를 구현하는 로봇 |
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: 19914139 Country of ref document: EP Kind code of ref document: A1 |
|
ENP | Entry into the national phase |
Ref document number: 2020569234 Country of ref document: JP Kind code of ref document: A |
|
NENP | Non-entry into the national phase |
Ref country code: DE |
|
ENP | Entry into the national phase |
Ref document number: 2019914139 Country of ref document: EP Effective date: 20210830 |