JP2022013038A - Robot and control device thereof, and movement range estimation device and program thereof - Google Patents

Abstract

To efficiently avoid interference with a moving obstacle by assuming a future fluctuation in an expected position of the moving obstacle.SOLUTION: A robot 10 comprises: a detection device 12 that detects position information and speed information of a nearby target pedestrian H; and a control device 13 that executes an action for avoiding future interference with the target pedestrian H based on the detection result of the detection device 12. The control device 13 comprises: action determination means 19 for determining a necessity of the action; and action execution means 20 that executes the action when it is determined that the action is necessary. The action determination means 19 comprises: a prediction distribution area deriving unit 22 for obtaining a predicted distribution area G which represents a probability distribution of a position where the target pedestrian H exists at a predetermined time in the future; an interference probability identifying unit 23 that identifies the interference probability using the predicted distribution area G from a future relative positional relationship between the target pedestrian H and the robot 10; and a necessity determining unit 24 which determines the necessity of the action by comparing the interference probability with a predetermined threshold value.SELECTED DRAWING: Figure 1

Description

The present invention relates to a robot and its control device, and a movement range estimation device and its program, and more particularly, a robot and its control device that efficiently avoid future interference with a moving obstacle such as a human being. It relates to a movement range estimation device and a program thereof, which are applied to them and predict the future movement range of a movement obstacle.

Recently, robots that can move autonomously in a coexisting environment with humans have appeared. Some robots move autonomously from a predetermined start point to a goal point while sensing the situation of obstacles including surrounding humans and avoiding future interference with the obstacles. .. In such a robot, it is required not only to avoid humans unilaterally, but also to consider human movements and other obstacles, and to cooperate with humans to effectively avoid interference. .. That is, when moving in the environment, in addition to passive movements such as interference avoidance movements and stop movements, the robot's action intention is transmitted to humans through the movements, and the human own interference avoidance behaviors. It is necessary to actively work on. Therefore, as a result of their own research, the present inventors have proposed various robots capable of autonomous movement based on a route plan based on the above-mentioned action (see Patent Documents 1, 2, etc.).

In order to generate the movement path of the robot when performing the above-mentioned action, it is necessary to detect the existence of a pedestrian or the like moving around the robot, its trajectory, the moving speed, and the like. The detection of the moving speed is estimated by providing the robot with a sensor such as a laser range finder (LRF) that acquires the position information of the surrounding environment and performing an operation based on the position information. Therefore, the present inventors improve the position estimation accuracy of the pedestrian by suppressing the estimation error due to the periodic vibration of the pedestrian, and suppress the phase delay and improve the responsiveness by the arithmetic processing with less burden. We have already proposed a method for estimating the speed of a pedestrian that can be used (see Patent Document 3).

Japanese Unexamined Patent Publication No. 2019-84641 Japanese Unexamined Patent Publication No. 2020-46759 Japanese Unexamined Patent Publication No. 2020-91235

When the robot route is generated in Patent Documents 1 and 2, the possibility of future interference between the pedestrian and the robot is determined from the position and speed vector of the pedestrian at the current time, and the interference is predicted. In that case, the route of the robot is planned so as to work on pedestrians. Here, on the assumption that the current velocity vector of the pedestrian will be maintained in the future, the movement path of the robot for avoiding interference is generated by using the prediction model of the velocity vector of the pedestrian as a constant velocity linear motion. .. At this time, in the robot, the future predicted position of the pedestrian is uniquely specified in time series, and when there is a possibility of the interference, the future interference points are estimated at one place.

However, the actual human velocity vector has many disturbances that cannot be represented by such a simple model. As the first disturbance, since the sensor mounted on the robot at a position away from the pedestrian is used, the detected value includes a measurement error. The error range of this measurement error increases as time elapses from the current time, and the future position of the pedestrian becomes ambiguous. Also, as a second disturbance, there is a possibility of holding the future velocity vector corresponding to the probability of maintaining the current value. That is, for example, in the case where a pedestrian is traveling straight for a long distance at a constant speed, it is considered that the speed vector predicted in the future does not fluctuate significantly from the current value. On the other hand, in the case where the pedestrian walks in a zigzag or turns a corner and has not progressed in a certain direction so far, the velocity vector predicted in the future is considered to fluctuate greatly from the current value. Be done. Therefore, the future velocity vector of the pedestrian will change the size of the pedestrian's movement range predicted at the future time due to the above-mentioned measurement error and the transition state so far.

As described above, in the current velocity vector calculated based on the current position information of the pedestrian, the future predicted position of the pedestrian is one point in consideration of the above-mentioned measurement error and holdability factors. The predicted position cannot be determined, and the predicted position extends over a predetermined range. In the algorithms in Patent Documents 1 and 2, when the possibility of future interference between a human and a robot is assumed, a movement path is generated in order to avoid the interference. However, since the prediction model of the pedestrian's velocity vector is a constant velocity linear motion, if the path of the robot is planned with the prediction model, the larger the future movement range of the pedestrian, the more the predetermined time thereafter. In the same processing for each, replanning of the movement route occurs frequently. That is, even if a route plan for avoiding interference is made based on the previous speed vector which is uncertain information, when the speed vector of the pedestrian changes due to a change of course or the like, the pedestrian and the robot interfere again. The possibility of this has emerged, and it is necessary to re-plan the route again. Therefore, as the speed vector of the pedestrian in the future is uncertain, the work of resetting and resetting the previous route plan is repeated many times, which makes the process of route planning inefficient. In addition, in the conventional algorithm described above, the robot performs only the minimum necessary avoidance, so if the human speed vector changes even a little with respect to the assumption, there is a possibility that it will interfere again when it gets closer to the pedestrian. It may occur and may require greater avoidance of the robot.

The present invention has been devised focusing on such a problem, and an object of the present invention is to assume a blurring of a future predicted position of a moving obstacle and to efficiently avoid interference with the moving obstacle. It is an object of the present invention to provide a robot, a control device thereof, a movement range estimation device applicable to the robot, and a program thereof.

In order to achieve the above object, the present invention mainly describes a detection device that detects the position information and speed information of a moving obstacle existing in the surroundings, and the future of the moving obstacle from the detection result of the detection device. In a robot that is provided with a control device that works to avoid interference and can move autonomously while avoiding the interference, the control device has a working necessity determining means for determining the necessity of the working and the working. When the necessity determination means determines that the action is necessary, the action execution means for controlling the operation of the robot so as to execute the action is provided, and the action necessity determination means is said to be said at a predetermined time in the future. From the predictive distribution area derivation unit that obtains the predicted distribution area that represents the probability distribution of the position where the moving obstacle exists, and the future relative positional relationship between the moving obstacle and the robot, the interference occurs using the predicted distribution area. It is configured to include an interference probability specifying unit that specifies an interference probability that is a probability of occurrence, and a necessity determining unit that determines the necessity of the action by comparing the interference probability with a predetermined threshold value.

In the present invention, the predicted distribution area estimates the future movement range of the moving obstacle at a predetermined future time, and specifies the existence probability at each point within the moving range. From this existence probability, the interference probability according to the disturbance in the future movement of the moving obstacle is derived, and the necessity of working is determined according to the magnitude of the interference probability. As a result, even if it is estimated that there is no possibility of interference with the robot in the future on the prediction model in which the moving obstacle moves linearly at a constant velocity, if the interference probability is higher than the threshold value, efficient movement is performed. It is possible to work early to do this. On the other hand, even if it is estimated that there is a possibility of interference on the prediction model, if the interference probability is lower than the threshold value, it is assumed that the moving obstacle will be greatly shaken in the future. It will be possible to see off and wait for a while. As described above, according to the present invention, it is possible to cope with the fluctuation of the future existence range of the moving obstacle, minimize the replanning at the time of generating the moving path of the robot, and efficiently avoid the interference with the moving obstacle. It is possible to make the movement control system robust by minimizing the influence of disturbance on the movement of moving obstacles.

In addition, the avoidance amount adjustment unit can add a margin in consideration of the future movement range of the moving obstacle as the avoidance amount required to avoid the interference between the robot and the moving obstacle. Therefore, it is possible to minimize the replanning when the robot's movement path is generated.

It is a block diagram schematically showing only the configuration related to the movement control of the robot which concerns on this embodiment. It is a conceptual diagram for explaining a predicted distribution area. It is a figure for demonstrating the relative coordinate system and the predicted distribution area in a relative coordinate system. It is a conceptual diagram for explaining a personal space. (A) is a conceptual diagram for explaining the procedure for calculating the condition value in the first case, and (B) is a conceptual diagram for explaining the procedure for calculating the condition value in the second case. (A) to (C) are conceptual diagrams for explaining a procedure for obtaining a deviation width.

Hereinafter, embodiments of the present invention will be described with reference to the drawings.

FIG. 1 shows a block diagram schematically showing only the configuration related to the movement control of the robot 10 according to the present embodiment. In this figure, the robot 10 functions as a moving body that autonomously moves from a preset movement start point (start point) to a predetermined target point (goal point). In this robot 10, when future interference with an object such as a wall or an obstacle consisting of human beings is expected during movement, the robot 10 travels to surrounding human beings as a moving route while avoiding these obstacles. A movement route that can be moved efficiently with reduced influence is searched for, and operation control is performed along the movement route.

Obstacles here include fixed obstacles that are always stationary in the environment, such as walls existing around the movement path of the robot 10, and various types of obstacles that can move in the environment, such as humans, animals, and robots. There are moving obstacles consisting of moving bodies. In the present embodiment, a walking human (pedestrian) is used as a movement obstacle, and motion control of the robot 10 for avoiding future interference between the pedestrian and the robot 10 will be described, but other movements will be described. The operation of the robot 10 can be controlled for the body by the same processing as described later. In this robot 10, when a pedestrian moves in an approaching direction, a movement path is generated so as to work to avoid interference with the pedestrian, and motion control of various parts is performed. Although not particularly limited, this action is performed with the intention of encouraging the coordinated movement of the pedestrian for the interference avoidance, but is a mere interference avoidance operation of the robot 10 that does not expect the coordinated movement. Is also included.

As shown in FIG. 1, the robot 10 includes an operation unit 11 composed of mechanisms and devices configured to enable various operations, a detection device 12 for detecting environmental information around the robot 10, and a detection device 12. Based on the detection result of the above, a control device 13 that performs autonomous movement control of the robot 10 while considering the situation of the obstacle is provided.

The moving unit 11 includes a moving device 14 including a mechanism for autonomously moving the robot 10 within a predetermined range and a power source thereof. Since the operating unit 11 including the moving device 14 is composed of all known members, mechanisms, devices, and the like and is not an essential part of the present invention, detailed illustration and description of each configuration will be omitted.

The detection device 12 detects the speed information of a pedestrian based on the position detection unit 16 that detects the position information of pedestrians and other obstacles existing around the robot 10 and the detection result of the position detection unit 16. It is equipped with a speed detection unit 17 made of a computer, and can acquire the position information and the speed information at predetermined time intervals.

The position detection unit 16 is not particularly limited, but is based on the reflection state of an object including a human being by irradiation of a laser beam from the robot 10 to its surroundings, and is a surface portion of each surface portion of the object around the robot 10. It is composed of a range sensor such as a known laser range finder that detects position information. In this range sensor, the two-dimensional position coordinates of each point in the point cloud constituting the surface part of a human being or the like in a plan view are detected at predetermined time intervals, and from the detection results at each time, a known method is used. The position of the human virtual trunk at the time is specified, and the position information in the virtual trunk is used as the observed value. Here, when the displacement of the position information of the virtual trunk in the time series data is less than a predetermined threshold value, it is assumed that they are the same pedestrian and the same ID is assigned. Then, for each pedestrian, the speed detection unit 17 obtains the speed information from the change of the acquired time-series position information.

The position detection unit 16 is not limited to the above-mentioned laser range finder and other range sensors, but may be composed of various sensors and devices as long as it can detect the position information of a pedestrian. You can also.

In the speed detection unit 17, the corresponding pedestrian's position information is obtained from the position information at each time when the pedestrian's position information is acquired by the method already proposed by the present inventors (Japanese Patent Laid-Open No. 2020-91235). Velocity information at the position is calculated. Since the algorithm for deriving the speed information is not an essential element of the present invention, detailed description thereof will be omitted. Further, in the present invention, if the speed information can be obtained from the position information of the pedestrian detected by the position detection unit 16, the speed information is not limited to the above-mentioned method, and the speed information can be obtained by another known method or the like. Other derivable sensors, devices, systems, etc. can be applied.

As the position information in the present embodiment detected by the detection device 12, a position vector composed of coordinate components in two orthogonal axes in a plan view is used, and as the same velocity information, a component of a rotation angle indicating a traveling direction and a component of a rotation angle indicating a traveling direction are used. A velocity vector consisting of a velocity component is used.

As described above, the movement information including the position information and the speed information of the pedestrian is sequentially detected by the detection device 12, transmitted to the control device 13 at the detection timing, and the control device 13 corresponds to the acquired time. It is recorded as log data in.

The control device 13 detects that the robot 10 can autonomously move from a preset start point to a predetermined goal point based on its own position and speed vector while avoiding interference with pedestrians. It is determined whether or not it is necessary to work at each acquisition timing of the movement information in the device 12. Then, when it is determined that the action is necessary, the optimum movement route is searched for, and the operation command is given to the operation unit 11.

The control device 13 is provided integrally with the robot 10 or as a separate body, and is composed of a computer including an arithmetic processing unit such as a CPU and a storage device such as a memory and a hard disk. A program for functioning as each of the following means is installed in the computer.

Next, a specific configuration of the control device 13 will be described.

The control device 13 needs to work to determine whether or not the robot 10 needs to work on the pedestrian to avoid the interference when the robot 10 may interfere with the pedestrian moving around the robot 10 in the future. It is provided with a rejection determination means 19 and an action execution means 20 that controls the operation of the robot 10 so as to execute an action suitable for avoiding interference when the action necessity determination means 19 determines that the action is necessary. There is.

The action necessity determination means 19 obtains a prediction distribution region representing a probability distribution of a position where a pedestrian (hereinafter referred to as “target pedestrian”) to be determined for interference exists at a predetermined time in the future. Using the distribution area derivation unit 22 and the predicted distribution area at a future time when the target pedestrian approaches the robot 10, the interference probability specifying unit 23 that specifies the interference probability, which is the probability that they interfere with each other, and the interference. It is provided with a necessity determination unit 24 that determines the necessity of the above-mentioned action by comparing the probability with a predetermined threshold value.

Here, the predicted distribution region will be described. As a premise, the position prediction of the target pedestrian at a predetermined time in the future has the highest probability of the coordinates calculated by the constant velocity linear motion model based on the velocity vector detected at the present time (current time), and moves away from that point. The lower the probability. Therefore, as conceptually shown in FIG. 2, in the predicted distribution region G, the center coordinates of the target pedestrian H at a predetermined future time are calculated by a constant velocity linear motion model, and the range of the region is defined as the traveling direction velocity V. The existence probability of the target pedestrian H is expressed as a bivariate Gaussian distribution with the rotation angular velocity θ as two axes.

In the following various processes in the action necessity determination means 19, as shown in FIG. 3, a relative coordinate system representing the relative position of the robot 10 with respect to the target pedestrian H is adopted. In this relative coordinate system, the position of the target pedestrian H calculated by the constant velocity linear motion model is set as the origin at a predetermined time, and the traveling (straight) direction (arrow direction in the figure) in the model is the y-axis (m). The direction orthogonal to the traveling direction is defined as the x-axis (m).

上式において、Ｐは、前記相対座標系における対象歩行者Ｈの座標内位置である任意のｘ，ｙ座標であり、Σは共分散行列を意味する。共分散行列Σは、確率変数の散らばり具合を表すものであり、一次元のガウス分布の分散を多次元に拡張したものである。ｘ軸、ｙ軸に対して分散をとると、現時刻からｔ_ｎ時間後の共分散行列Σは、次式のように表される。

上式において、σ_ｘは、ｘ軸方向における標準偏差（ｍ／ｓ）であり、σ_ｙは、ｙ軸方向における標準偏差（ｍ／ｓ）である。つまり、σ_ｘ ^２は、進行時の方向の変化量に対応して変化し、σ_ｙ ^２は、速さの変化量に対応して変化する。 In the predicted distribution area derivation unit 22, a probability density function f _nt (P, Σ) according to the following equation is used as a bivariate Gaussian distribution at a future time in _tun time (seconds) from the current time, and the concept is shown in FIG. As shown in the above, the predicted distribution region G of the target pedestrian H after _tun hours (seconds) from the present time is specified. It should be noted that the outside of the outer edge portion in the predicted distribution region G is a portion where the existence probability of the target pedestrian H is infinitely close to zero.

In the above equation, P is an arbitrary x, y coordinate that is a position within the coordinates of the target pedestrian H in the relative coordinate system, and Σ means a covariance matrix. The covariance matrix Σ represents the degree of dispersion of random variables, and is a multidimensional extension of the variance of a one-dimensional Gaussian distribution. When the variance is taken with respect to the x-axis and the y-axis, the covariance matrix Σ after _TN hours from the current time is expressed by the following equation.

In the above equation, σ _x is the standard deviation (m / s) in the x-axis direction, and σ _y is the standard deviation (m / s) in the y-axis direction. That is, σ _x ² changes according to the amount of change in the traveling direction, and σ _y ² changes according to the amount of change in speed.

上式において、σ_ｓθは、測定誤差のｘ軸方向における標準偏差（ｍ／ｓ）であり、σ_ｓｖは、測定誤差のｙ軸方向における標準偏差（ｍ／ｓ）である。また、σ_Δθは、保持可能性のｘ軸方向における標準偏差（ｍ／ｓ）であり、σ_ΔＶは、保持可能性のｙ軸方向における標準偏差（ｍ／ｓ）である。 Here, the range of the predicted distribution region G is an element related to the measurement error on the device of the position detection unit 16 and an element related to the holdability corresponding to the probability that the future velocity vector of the target pedestrian H maintains the current value. It changes according to. That is, the standard deviations (σ _x , σ _y ) that determine the range of the predicted distribution region G are the standard deviations related to the measurement error of the velocity vector and the standard deviations related to the holdability of the target pedestrian H, as shown in the following equation. Is determined by the total sum of.

In the above equation, σ _{s θ} is the standard deviation (m / s) of the measurement error in the x-axis direction, and σ _sv is the standard deviation (m / s) of the measurement error in the y-axis direction. Further, σ _Δθ is the standard deviation (m / s) of the holdability in the x-axis direction, and σ _ΔV is the standard deviation (m / s) of the holdability in the y-axis direction.

上式において、Ｅ_θは、予め行った実験によって特定された特定値（例えば、５．６４）であり、Ｅ_Ｖは、予め行った実験によって特定された特定値（例えば、０．１４）であり、これらＥ_θ、Ｅ_Ｖは、ロボット１０の周囲の環境中の人間等の移動状況に応じて定まる値となっている。また、Ｖ_Ｈｔ０は、現時刻ｔ_０における対象歩行者Ｈの速さである。 Here, the standard deviation (σ _sθ , σ _sv ) of the measurement error is obtained by the following equation.

In the above equation, E _θ is a specific value (for example, _5.64 ) specified by a prior experiment, and EV is a specific value (for example, 0.14) specified by a prior experiment. Yes, these E _θ and _EV are values that are determined according to the movement situation of a human being or the like in the environment around the robot 10. Further, V _Ht 0 is the speed of the target pedestrian H at the current time t ₀ .

上式において、Ｎは、ログデータの窓長であり、この窓長は、検出装置１２での移動情報の検出タイミングとなる各ステップのうち、現在での演算処理がなされる現時刻ｔ_０でのステップから過去に遡るステップ数を意味する。以下、現時刻ｔ_０でのステップｋからその１つ前の直前ステップを、ステップｋ－１と表し、その更に１つ前のステップをｋ－２と表し、現時刻ｔ_０でのステップｋからｉ個前のステップをステップｋ－ｉと表す。なお、特に限定されるものではないが、本実施形態の窓長Ｎは、人間の１歩行周期の１／２の時間に相当する２０ステップに設定される。また、θ_{Ｈ（ｋ－ｉ）}は、現時刻ｔ_０でのステップｋからｉステップ前の対象歩行者Ｈの回転角速度（ｒａｄ／ｓ）であり、Ｖ_{Ｈ（ｋ－ｉ）}は、現時刻ｔ_０でのステップｋからｉステップ前の対象歩行者Ｈの速さ（ｍ／ｓ）である。また、θ_ａｖは、Ｎステップ分のログデータにおける平均回転角速度（ｒａｄ／ｓ）を表し、Ｖ_ａｖは、Ｎステップ分のログデータにおける平均速さ（ｍ／ｓ）を表す。 Further, the standard error of the retainability (σ _Δθ , σ _ΔV ) is based on the transition state of the movement information of the target pedestrian H acquired at each predetermined timing in the past from the current time, and the log data of the movement information is used. It is calculated by the following formula using it.

In the above equation, N is the window length of the log data, and this window length is the current time t ₀ at which the current arithmetic processing is performed in each step that is the detection timing of the movement information in the detection device 12. It means the number of steps that go back to the past from the step of. Hereinafter, the step immediately before the step k at the current time t ₀ is referred to as step k-1, and the step immediately before the step k is referred to as k-2, and the step immediately before the step k at the current time t ₀ is represented as k-2. The step i before is represented as step k-i. Although not particularly limited, the window length N of the present embodiment is set to 20 steps corresponding to half the time of one human walking cycle. Further, θ _{H (ki)} is the rotational angular velocity (rad / s) of the target pedestrian H before step k to i step at the current time t ₀ , and V _{H (ki)} is the current time. It is the speed (m / s) of the target pedestrian H before step k to i step at t ₀ . Further, θ _av represents the average rotational angular velocity (rad / s) in the log data for N steps, and V _av represents the average speed (m / s) in the log data for N steps.

As shown in FIG. 1, the interference probability specifying unit 23 specifies the predicted distribution region G from the condition value calculation unit 26 that calculates various condition values for obtaining the interference probability and the condition values. It is provided with a probability calculation unit 27 that calculates the interference probability using a mathematical formula.

As will be described later, the condition value calculation unit 26 estimates the future approach state of the relatively moving robot 10 and the target pedestrian H, and the position information of a predetermined approach point in the approach state and its approach. The time information, which is the elapsed time from the current time until the future approach time in the state, is mathematically calculated as the condition value.

Here, when specifying the approaching state, as shown in FIG. 4, a comprehensive personal space PS virtually set around the position of the robot 10 is used. This comprehensive personal space PS is a virtual circular personal space SR, SH having _{radii L RP} , L _HP multiplied by a constant K (K> 1) with respect to the width of the robot 10 and the width of the human trunk, respectively. It is a circular area that is the sum of (the broken line part in the figure). That is, the radius L _PS of the total personal space PS is a value obtained by _summing the radii L _RP and _{L HP} of each personal space SR, SH, and is not particularly limited, but the radius L _PS of the present embodiment is The radii L _RP and L _HP are set to 0.5 m, respectively, and the total is set to 1 m. In this comprehensive personal space PS, when the target pedestrian H exists in the inner region at a predetermined time, the target _{pedestrian H} and the robot 10 which is a boundary for determining the interference / non-interference of each personal space SR, SH and the target pedestrian. The relative distance of person H is less than or equal to _LPS , and it is judged that they interfere with each other.

In the condition value calculation unit 26, the condition value is obtained by the following procedure. That is, first, the future movement state of the total personal space PS is estimated using the relative position vector and the relative velocity vector of the robot 10 at the current time in the relative coordinate system. Then, according to the future movement state, the position information of the approaching point at a predetermined timing when the comprehensive personal space PS approaches the target pedestrian H and the time information which is the approaching time at the timing are obtained as condition values. ..

Here, the calculation of the condition value is always non-interfering in the relative coordinate system in which the target pedestrian H exists at the origin, and the target pedestrian H at the origin does not exist within the range of the total personal space PS at any future timing. The first case is divided into the first case and the second case where the target pedestrian H at the origin may exist within the range of the total personal space PS at a predetermined timing in the future. That is, in the first case, it is estimated that the relative distance between the target pedestrian H and the robot 10 based on the future relative movement prediction does not always fall below the preset radius LPS of the total personal space _PS . Is. On the other hand, the second case is a case where it is estimated that the relative distance may be less than the radius L _PS of the total personal space PS.

In the first case, from the concept of the predicted distribution region described above, the existence probability of the target pedestrian H decreases as the position is farther from the origin of the relative coordinate system at a certain future time. Then, the interference probability becomes maximum when the total personal space PS is closest to the origin, as shown by the broken line in FIG. 5 (A). Therefore, the relative position information and the time information in this closest state are obtained as follows.

That is, in the closest state in which the robot 10 moves relative to the position of the total personal space PS of the broken line in FIG. 5 from the relative position of the robot 10 having the total personal space PS of the solid line in FIG. The center point C (relative position of the robot 10) of the personal space PS is specified. Here, in the closest approach state, the straight line l connecting the origin of the relative coordinate system and the center point C is orthogonal to the relative movement locus M of the robot 10 represented by the straight line, and from the condition, the straight line is straight. The length of l, that is, the linear distance _dRH between the center point C and the origin in the closest state is obtained. This linear distance d _RH is mathematically calculated from the relative position vector of the robot 10 at the current time in the relative coordinate system and the relative rotation angle between the target pedestrian H and the robot 10 at the current time.

Then, in the relative coordinate system, the point closest to the origin in the region of the total personal space PS in the closest approach state is defined as the approach point CP, and the position vector P _CPA is obtained as the position information. This position vector P _CPA is mathematically calculated by the difference between the linear distance d _RH and the radius L _PS of the total personal space PS and the relative rotation angles of the target pedestrian H and the robot 10 at the current time.

Further, as the time information, the elapsed time _TCPA from the current time to the approach time when the robot 10 reaches the position of the total personal space PS in the closest approach state is the relative position vector of the robot 10 at the current time and the approach time. And the relative speed of the robot 10 with the target pedestrian H at the current time, which is calculated mathematically.

In the second case, as shown in FIG. 5B, the earliest approach state is when the peripheral edge of the comprehensive personal space PS touches the origin of the relative coordinate system, which is the earliest approach. The predicted position C of the robot 10 in this state is set as the approach point CP. Therefore, the position vector P _CPA of the approach point CP, which is the position information, becomes the position vector of the robot 10 in the earliest approach state in the relative coordinate system. Further, the elapsed time _TCPA , which is time information, is the time from the current time to the future time when the earliest approaching state is reached. These position vectors P _CPA and elapsed time T _CPA are mathematically calculated from the position vector of the robot 10 at the current time in the relative coordinate system and the velocity vector (relative rotation angle and relative speed).

In the probability calculation unit 27, the numerical value obtained by substituting the position vector P _CPA of the approach point CP and the elapsed time T _CPA into the above-mentioned probability density function for specifying the predicted distribution region is obtained as the interference probability. That is, the interference probability is when the total personal space PS of the robot 10 is in the closest state of the first case to the target pedestrian H, or when it is in the earliest approach state of the second case. It is the probability in the predicted distribution region of the target pedestrian H at each approach point CP. Therefore, the above equations (1) to (5) are used, and the position vector P _CPA of the approaching point CP is substituted into these equations as the position vector P, and the elapsed time t _n from the current time. The time T _CPA is substituted and the interference probability is obtained.

As shown in FIG. 1, the necessity determination unit 24 includes a threshold value determination unit 29 that determines a threshold value that serves as a reference for the timing of action according to the relative relationship between the robot 10 at the current time and the target pedestrian H. When the interference probability specified by the interference probability specifying unit 23 is larger than the threshold value obtained by the threshold value determining unit 29, the robot 10 needs to work for interference avoidance, and the execution means 20 is encouraged to perform the work. It is provided with a working command unit 30 for commanding.

The threshold value is set to the minimum interference probability that it is determined that the robot 10 needs to work, and is as follows at each timing when the interference probability is obtained by the interference probability specifying unit 23 so as to be variable according to various situations. It is determined.

That is, for example, when the target pedestrian H is assumed to collide with the robot 10 from the front side, even if the predicted position of the target pedestrian H is slightly displaced, the personal spaces of the target pedestrian H interfere with each other. There is a high possibility that it will be done. Therefore, in such a case, unless the deviation width of the predicted position of the target pedestrian H is extremely large, it is unlikely that the interference of each personal space will be avoided, and there is a high possibility that action will be required. Therefore, it is necessary to lower the threshold value to avoid interference faster. On the other hand, due to the relative relationship between the target pedestrian H and the robot 10, if it is delicate whether or not they collide, if the predicted position of the target pedestrian H deviates even a little, their personal spaces do not interfere with each other. Probability is high. Therefore, in such a case, it is unlikely that the action is required at the current time, and if the action is made, the interference state will change sensitively to the deviation of the predicted position of the target pedestrian H. It is useful to set a high threshold value so as to delay the timing of the action, and to wait and see once in the current movement path of the robot 10.

Therefore, the threshold value determination unit 29 avoids the interference in the above-mentioned second case in which each of the personal spaces is predicted to interfere in the future based on the relative relationship between the target pedestrian H and the robot 10 at the current time. The minimum deviation width ΔD required for this is calculated, and the threshold value is calculated as follows according to the deviation width ΔD. According to the above-mentioned idea, the threshold value is calculated lower so that the larger the deviation width ΔD, the easier it is to work. On the other hand, in the case of the above-mentioned first case where it is predicted that the personal spaces of the target pedestrian H and the robot 10 will not interfere in the future, since the deviation width ΔD is zero, the calculation described later can be performed. Instead, a predetermined value set in advance according to the request is adopted as the threshold value.

The deviation width ΔD is a direction in which the relative positional relationship between the target pedestrian H and the robot 10 in the future separates the personal spaces from the interference state in which the personal spaces overlap each other to the state in which the peripheral edges thereof intersect with each other. It is used as a movement vector when shifting. In other words, if each personal space is shifted to the deviation width ΔD or more, the interference state thereof is avoided, and the minimum movement vector for the avoidance becomes the deviation width ΔD.

Specifically, the deviation width ΔD is obtained by mathematical calculation from the relative position information and the relative velocity information of the robot 10 in the relative coordinate system at the current time through the procedure in each of the following steps.

First, as a first step, as shown in FIG. 6A, the center point C of the robot 10 in the state where the robot 10 is closest to the target pedestrian H at the origin in the relative coordinate system and their relatives. The linear distance d _RH is calculated.

Next, as a second step, as shown in FIG. 6B, the boundary point AP of the robot 10 which is the boundary between the target pedestrian H and each personal space of the robot 10 in the relative coordinate system. The position vector of is calculated. That is, assuming that the boundary point AP is orthogonal to the straight line l connecting the origin and the position of the robot 10 at the current time in the relative coordinate system and exists on the straight line passing through the origin, the radius centered on the boundary point AP. The outer edge of the total personal space _PS of LPS is set at the point where it passes through the origin. This boundary point AP will be located at two points in the relative coordinate system as shown in the figure.

Further, as a third step, as shown in FIG. 6C, the position vector of the center point C of the robot 10 when the robot 10 is closest to the target pedestrian H and the position vector of each boundary point AP are set. The difference from each is obtained, and the smaller difference is defined as the deviation width ΔD.

上式において、Δｘは、ズレ幅ΔＤのｘ軸成分であり、Δｙは、ズレ幅ΔＤのｙ軸成分である。 Then, the threshold value in the second case is calculated by applying the probability density functions of the above equations (1) and (2) using the deviation width ΔD. That is, in the probability density function, the position vector P of the target pedestrian H is set as the origin (0,0), and the standard deviation with respect to each of the x-axis and y-axis which are diagonal components of the covariance matrix Σ of the above equation (2). Is a deviation width ΔD expressed as a vector, and the threshold value T is calculated by the following equation in which these are substituted into the above equation (1).

In the above equation, Δx is the x-axis component of the deviation width ΔD, and Δy is the y-axis component of the deviation width ΔD.

When the interference probability specified by the interference probability specifying unit 23 is larger than the threshold value obtained by the threshold value determining unit 29, the working execution means 20 executes the operation of the robot 10 regarding the working by a command from the working command unit 30. It works to do.

As shown in FIG. 1, the working execution means 20 is based on an avoidance amount adjusting unit 32 that adjusts an avoidance amount for avoiding interference between the robot 10 and the target pedestrian H when working, and the avoidance amount. It is provided with a movement control unit 33 that controls the movement of the robot 10.

In the avoidance amount adjusting unit 32, the predicted distribution has a constant interference avoidance width that is the radius LPS of the total personal space _PS , which is the minimum relative distance between the robot 10 and the target pedestrian H to avoid interference. By adding the safety distance that changes according to the size of the region, the avoidance amount considering the deviation of the future predicted position of the target pedestrian H can be obtained.

上式において、σ_ｘ、σ_ｙは、前述の式（３）～（５）により求められる値であり、ｔ_ｎは、現時刻からロボット１０と対象歩行者Ｈの各パーソナルスペースが干渉するまでにかかる時間であり、前述と同様にして算出される。 The safety distance _LA is calculated by the following equation based on the diagonal component of the above equation (2), corresponding to the variance for each axis, which is a parameter that determines the range of the predicted distribution region.

In the above equation, σ _x and σ _y are values obtained by the above equations (3) to (5), and t _n is from the current time until the personal spaces of the robot 10 and the target pedestrian H interfere with each other. It is the time required for the operation, and is calculated in the same manner as described above.

In the movement control unit 33, the operation of the robot 10 corresponding to various actions is selected by using the method and the like disclosed in Japanese Patent Application Laid-Open No. 2020-46759 already proposed by the present inventors. Here, when the movement path of the robot 10 corresponding to the action suitable for interference avoidance is generated, the avoidance amount adjusted by the avoidance amount adjusting unit 32, that is, the radius LPS which is the width of the total personal space _PS . In addition, an extended avoidance amount is used by adding a safety distance _LA in consideration of the movement error of the target pedestrian H. Since the operation selection of the robot 10 according to the action is not an essential part of the present invention, detailed description thereof will be omitted.

As described above, the predicted distribution area derivation unit 22 obtains a predicted distribution area G that specifies the existence probability at each point within the future movement range of the target pedestrian H at a predetermined future time. .. Therefore, the control device 13 including the predicted distribution area derivation unit 22 functions as a movement range estimation device for estimating the future movement range of the target pedestrian H at a predetermined time.

The robot according to the present invention is not limited to the autonomously moving robot 10 described in the above embodiment, and can autonomously move in a predetermined space such as an automatic vehicle, a ship, or a flying object. In addition to the moving body, a manipulator such as a robot arm that moves within a predetermined range of space may be used, and it can be applied as a method for avoiding interference with the target pedestrian H during these movements.

In addition, the configuration of each part of the device in the present invention is not limited to the illustrated configuration example, and various changes can be made as long as substantially the same operation is achieved.

10 Robot 12 Detection device 13 Control device (movement range estimation device)
19 Working necessity judgment means 20 Working execution means 22 Prediction distribution area derivation unit 23 Interference probability specification unit 24 Necessity determination unit 27 Probability calculation unit 29 Threshold determination unit 30 Working command unit 32 Avoidance amount adjustment unit 33 Movement control unit CP approach point G Predicted distribution area H Target pedestrian (moving obstacle)

Claims (9)

A detection device that detects the position information and speed information of a moving obstacle existing in the surroundings, and a control device that works to avoid future interference with the moving obstacle from the detection result of the detection device. In a robot that can move autonomously while avoiding the interference
The control device controls the operation of the action necessity determining means for determining the necessity of the action and the robot so as to execute the action when the action necessity determination means determines that the action is necessary. Equipped with means of working and execution,
The action necessity determination means is a predictive distribution area deriving unit that obtains a predictive distribution area representing a probability distribution of a position where the moving obstacle exists at a predetermined time in the future, and a future relative relationship between the moving obstacle and the robot. From the positional relationship, the predicted distribution region is used to determine the necessity of the action by comparing the interference probability specifying unit that specifies the interference probability, which is the probability of occurrence of the interference, with a predetermined threshold. A robot characterized by having a necessity determination unit. In the predicted distribution area derivation unit, the center coordinates are calculated from the detection result of the detection device by the constant velocity linear motion model of the moving obstacle, and the two axes corresponding to the speed and the angle of rotation of the moving obstacle are used. The robot according to claim 1, wherein the predicted distribution region having a probability density function having a variable Gaussian distribution as a region range is obtained, and the probability that the moving obstacle exists at a predetermined position at a predetermined time is estimated. The standard deviation of the predicted distribution region is determined by integrating the factors related to the measurement error of the detector and the factors related to the holdability corresponding to the probability that the future velocity information will maintain the current state.
The robot according to claim 2, wherein the retainability is specified according to a transition state of past log data detected by the detection device. The interference probability specifying unit includes a condition value calculation unit that calculates various condition values for obtaining the interference probability, and a probability calculation unit that calculates the interference probability from the condition values using a mathematical formula representing the predicted distribution region. And with
In the condition value calculation unit, the time information which is the elapsed time from the current time until the future approach time when the moving obstacle is predicted to be in the approaching state approaching the robot from the detection result of the detection device at the current time. And the position information of the predetermined approach point in the approach state are obtained as the condition value.
When it is estimated that the relative distance between the moving obstacle and the robot based on the future relative movement prediction does not always fall below a preset predetermined value, the time when the moving obstacle and the robot come closest to each other is said. The approach time is defined as the approach point on the straight line connecting the moving obstacle and the robot at the approach time, and the point separated from the robot by a predetermined value is the approach point, while the relative distance is the future. When it is estimated that there are times when the relative distance falls below the predetermined value, the earliest time when the relative distance matches the predetermined value is set as the approach time, and the predicted position of the robot at that time is set as the approach point.
The robot according to claim 1, wherein the probability calculation unit calculates the existence probability of the moving obstacle at the approaching point as the interference probability by substituting the condition value into the mathematical formula. The necessity determination unit performs the action with the threshold value determination unit that determines the threshold value according to the relative relationship between the moving obstacle and the robot at the current time, and when the interference probability is larger than the threshold value. Is equipped with a working command unit that commands the working execution means.
In the threshold determination unit, when it is predicted that there is a future time in which the relative distance between the moving obstacle and the robot becomes less than a preset predetermined value, the relative distance at the future time becomes equal to or more than the predetermined value. Therefore, the minimum deviation width corresponding to the movement vector when the robot is further moved relative to the robot is calculated, and the deviation width is substituted into the probability density function as the standard deviation to calculate the threshold value. The robot according to claim 2. The action executing means includes an avoidance amount adjusting unit that adjusts an avoidance amount for avoiding the interference at the time of the action, and a movement control unit that controls the movement of the robot based on the avoidance amount.
The avoidance amount adjusting unit is characterized in that the avoidance amount is obtained by adding a safety distance that changes according to the size of the predicted distribution region to a certain interference avoidance width that is the minimum required for interference avoidance. The robot according to claim 1. Based on the detection result of the detection device that detects the position information and speed information of the moving obstacles existing in the surroundings, the robot autonomously moves so as to work to avoid future interference with the moving obstacles. In the control device to control
The action necessity determination means for determining the necessity of the action and the action execution means for controlling the operation of the robot so as to execute the action when the action necessity determination means determines the action is necessary. Prepare,
The action necessity determination means is a predictive distribution area deriving unit that obtains a predictive distribution area representing a probability distribution of a position where the moving obstacle exists at a predetermined time in the future, and a future relative relationship between the moving obstacle and the robot. From the positional relationship, it is necessary to use the predicted distribution region to determine the necessity of the action by comparing the interference probability specifying unit that specifies the interference probability, which is the probability of occurrence of the interference, with a predetermined threshold. A control device characterized by having a rejection unit. The robot's detection device that can move to a predetermined target point detects the position information and speed information of the moving obstacles that exist around the robot, and based on the detection results, at a predetermined time in the future of the moving obstacles. A device that estimates the range of movement,
A predictive distribution area derivation unit for obtaining a predictive distribution area representing a probability distribution of a position where the moving obstacle exists at the predetermined time is provided.
In the predicted distribution area derivation unit, the center coordinates are calculated from the detection result of the detection device by the constant velocity linear motion model of the moving obstacle, and the two axes corresponding to the speed and the angle of rotation of the moving obstacle are used. The predicted distribution region having the probability density function consisting of a variable Gaussian distribution as the region range is obtained, and the probability that the moving obstacle exists at a predetermined position at a predetermined time is estimated.
The standard deviation of the predicted distribution region is determined by integrating the factors related to the measurement error of the detector and the factors related to the holdability corresponding to the probability that the future velocity information will maintain the current state.
The retentability is a movement range estimation device, characterized in that it is specified according to a transition state of past log data detected by the detection device. The robot's detection device that can move to a predetermined target point detects the position information and speed information of the moving obstacles that exist around the robot, and based on the detection results, at a predetermined time in the future of the moving obstacles. It is a program of a device that estimates the range of movement.
A computer is made to function as a predictive distribution area derivation unit for obtaining a predictive distribution area representing the probability distribution of the position where the moving obstacle exists at the predetermined time.
In the predicted distribution area derivation unit, the center coordinates are calculated from the detection result of the detection device by the constant velocity linear motion model of the moving obstacle, and the two axes corresponding to the speed and the angle of rotation of the moving obstacle are used. The predicted distribution region having the probability density function consisting of a variable Gaussian distribution as the region range is obtained, and the probability that the moving obstacle exists at a predetermined position at a predetermined time is estimated.
The standard deviation of the predicted distribution region is determined by integrating the factors related to the measurement error of the detector and the factors related to the holdability corresponding to the probability that the future velocity information will maintain the current state.
The retentability is a program of a movement range estimation device, characterized in that it is specified according to a transition state of past log data detected by the detection device.

Images