JP2021077089A - Multiple vehicle movement control method, movement controller, movement control system, program, and recording medium - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a movement control method for coordinating the movement of a plurality of vehicles while suitably maintaining the observation of a search target by a vehicle. SOLUTION: This is a method of controlling the movement of a plurality of vehicles while observing at least one search target by a plurality of vehicles B1, B2, B3, and a step of acquiring the position and orientation of each of the plurality of vehicles, and a search. Based on the step of specifying the position of the target, the position and orientation of each of the plurality of vehicles, and the position of the search target, the step of calculating the movement locus for the look-ahead step of the plurality of vehicles, and the calculated movement locus. When multiple vehicles move in, a step to determine whether the observation condition of the search target is satisfied, a step to return to the step to calculate the movement trajectory when the observation condition is not satisfied, and a movement trajectory satisfying the observation condition. When the calculation is performed, the step of determining the driving condition of the vehicle from the present time to the unit time lapse based on the movement locus and controlling the movement of the vehicle is included. [Selection diagram] Fig. 1

Description

The present disclosure relates to a plurality of vehicle movement control methods, movement control devices, movement control systems, programs and recording media.

As one of the technologies for moving a plurality of vehicles (Swarm technology), a method is known in which areas in charge that do not interfere with each other are set, the moving bodies are independently controlled, and observations are performed in each vehicle (for example,). Patent Document 1).

JP-A-2019-74918

Here, as the movement control of the vehicle, there is a control of moving the vehicle with respect to the search target while observing the search target by the vehicle. When such control is performed, if the area in charge is divided, the search target may not be measured by the vehicle. In the system of Patent Document 1, in order to move the vehicle appropriately, for example, weighting processing of the area in charge is required. It is a burden on the worker to execute the weighting process according to the situation. In addition, in order to satisfy the conditions for observing the search target with the vehicle, movement control including movements that the vehicle cannot execute may occur, which causes a contradiction in control, and deadlock may make the movement of the vehicle uncontrollable. is there.

The present disclosure has been made in view of the above circumstances, and is a movement control method and a movement control device for a plurality of vehicles capable of coordinating the movement of vehicles while suitably maintaining the observation of the search target by the vehicle. It is an object of the present invention to provide a mobile control system, a program and a recording medium.

A method for controlling the movement of a plurality of vehicles, wherein the plurality of vehicles are observed while observing at least one search target by the plurality of vehicles in order to solve the above-mentioned problems and achieve the purpose. Based on the step of acquiring each position and posture of the search target, the step of specifying the position of the search target, the position and posture of each of the plurality of vehicles, and the position of the search target of the plurality of vehicles. A step of calculating the movement locus for the look-ahead step, a step of determining whether or not the observation conditions of the search target are satisfied when the plurality of vehicles move according to the calculated movement locus, and a case of not satisfying the observation conditions. When the step of returning to the step of calculating the movement locus and the movement locus satisfying the observation conditions are calculated, the driving conditions of the vehicle from the present time to a unit time later are determined based on the movement locus. Provided is a movement control method including a step of controlling the movement of the vehicle.

Further, it is a movement control device that controls the movement of a plurality of vehicles, communicates with each of the plurality of vehicles, acquires information on the current position and attitude of the vehicle, and searches at least one. The vehicle that satisfies the observation condition of the search target based on the communication unit that acquires the target position information, the acquired current position information and posture information of the vehicle, and at least one search target position information. The control unit includes a control unit for calculating the movement locus of the plurality of vehicles, and the control unit includes a pre-reading step of the plurality of vehicles based on the position and orientation of each of the plurality of vehicles and the position of the search target. The movement locus is calculated, and when the plurality of vehicles move according to the calculated movement locus, it is determined whether or not the observation condition of the search target is satisfied, and if the observation condition is not satisfied, the movement locus is calculated again. When a movement locus satisfying the observation conditions is calculated, a movement control device for controlling the movement of the vehicle by determining the driving conditions of the vehicle from the present time to a unit time after the movement locus is provided. ..

Further, a movement control system including a plurality of vehicles and a movement control device for moving the plurality of vehicles while observing at least one search target by the plurality of vehicles, wherein the movement control device includes the plurality of vehicles. A position detection unit that communicates with each of the vehicles and acquires information on the current position and attitude of the vehicle, an object detection unit that acquires information on the position of at least one search target, and the acquired object detection unit. The control includes a control unit that calculates the movement locus of the vehicle that satisfies the observation condition of the search target based on the information of the current position and the posture of the vehicle and the information of the position of at least one search target. The unit calculates the movement loci for the look-ahead steps of the plurality of vehicles based on the positions and postures of the plurality of vehicles and the positions of the search targets, and uses the calculated movement trajectories to calculate the movement loci of the plurality of vehicles. If it moves, it is determined whether the observation condition of the search target is satisfied, if the observation condition is not satisfied, the movement locus is calculated again, and if the movement locus satisfying the observation condition is calculated, the movement locus is described. Provided is a movement control system that determines a driving condition of the vehicle from the present time to a unit time after the movement locus based on the movement locus and controls the movement of the vehicle.

Further, a program for moving the plurality of vehicles while observing at least one search target by a plurality of vehicles, the step of acquiring the position and orientation of each of the plurality of vehicles on a computer, and the search target. Based on the step of specifying the position of the plurality of vehicles, the position and orientation of each of the plurality of vehicles, and the position of the search target, the step of calculating the movement locus for the look-ahead steps of the plurality of vehicles was calculated. When the plurality of vehicles move in the movement locus, a step of determining whether or not the observation condition of the search target is satisfied, a step of returning to the step of calculating the movement locus if the observation condition is not satisfied, and the observation. When the movement locus satisfying the condition is calculated, the step of determining the driving condition of the vehicle from the present time to the unit time after the current time and controlling the movement of the vehicle based on the movement locus is executed. Provide a program.

Further, it is a computer-readable recording medium that records a program for moving the plurality of vehicles while observing at least one search target by the plurality of vehicles, and the position and orientation of each of the plurality of vehicles are stored in the computer. The movement of the plurality of vehicles by the look-ahead step based on the step of acquiring the search target, the step of specifying the position of the search target, the position and orientation of each of the plurality of vehicles, and the position of the search target. A step of calculating a locus, a step of determining whether or not the observation conditions of the search target are satisfied when the plurality of vehicles move according to the calculated movement locus, and a step of calculating the movement locus when the observation conditions are not satisfied. When the step of returning to the step of returning to the step and the movement locus satisfying the observation condition are calculated, the driving condition of the vehicle from the present time to a unit time later is determined based on the movement locus, and the movement of the vehicle is controlled. A computer-readable recording medium on which a program for executing a vehicle is recorded is provided.

According to at least one embodiment of the present disclosure, it is possible to coordinate the movement of the vehicle while suitably maintaining the observation of the search target by the vehicle.

FIG. 1 is a block diagram showing a main configuration of a movement control system including the movement control device of the first embodiment. FIG. 2 is a table showing an example of the relationship between the vehicle and the search target. FIG. 3 is a schematic diagram showing an example of the observation range of one vehicle. FIG. 4 is a schematic diagram showing an example in which a plurality of vehicles individually move to their respective target positions. FIG. 5 is a schematic diagram showing an example in which a plurality of vehicles individually move to their respective target positions. FIG. 6 is a flowchart showing a flow of processing performed by the movement control system in the first embodiment. FIG. 7 is a schematic diagram showing an example of processing performed by the movement control system in the second embodiment. FIG. 8 is a schematic diagram showing an example of processing performed by the movement control system in the fourth embodiment. FIG. 9 is a schematic diagram showing an example of processing performed by the movement control system in the fourth embodiment. FIG. 10 is a schematic diagram showing an example of processing performed by the movement control system in the fifth embodiment.

Hereinafter, some embodiments of the present disclosure will be described in detail with reference to the drawings. The present invention is not limited to this embodiment. In addition, the components in the embodiment include those that can be easily replaced by those skilled in the art, or those that are substantially the same. Furthermore, the components described below can be combined as appropriate.

[First Embodiment]
FIG. 1 is a block diagram showing a main configuration of a movement control system 1 including the movement control device 10 of the first embodiment. The movement control system 1 is a system that controls the movement paths of a plurality of vehicles B. The movement control device 10 controls the movement of the plurality of vehicles B. In FIG. 1 and the like, reference numerals B1, B2, B3 ... Are attached for the purpose of distinguishing each of the plurality of vehicles B. The vehicle B may be a moving body traveling on the ground, a moving body flying in the air, or a moving body moving in water. Therefore, although the vehicle B includes a moving body that can move in three dimensions, the case of moving in a two-dimensional plane will be described below for the sake of explanation.

Each of the plurality of vehicles B includes a position detection unit 51, a target detection unit 52, a communication unit 53, and a power unit 54. The position detection unit 51 detects the position of the vehicle B provided with the position detection unit 51. As a specific configuration example of the position detection unit 51, a positioning device for detecting a position by using a positioning system such as a global positioning system (GPS) can be mentioned. The position detection unit 51 may be an inertial navigation system that detects a position with respect to a predetermined starting point.

The target detection unit 52 detects the search target to be observed. The object detection unit 52 detects an object with an image, temperature, sound waves, and a radio tower. The target detection unit 52 preferably uses a sensor that acquires information output from the target object, a so-called passive sensor. As a result, the target detection unit 52 does not need to output an output wave in order to detect the search target, and the device configuration can be simplified.

The communication unit 53 communicates with the movement control device 10. A wireless communication device can be mentioned as a specific configuration example of the communication unit 53. The communication unit 53 may be configured to perform wired communication with the movement control device 10.

The power unit 54 functions as a power source for moving the vehicle B. The specific configuration of the power unit 54 depends on the operation mode of the vehicle B. As an example, when the vehicle B is a vehicle traveling on the ground, the power unit 54 includes a plurality of wheels and a prime mover for driving a part or all of the plurality of wheels. The specific configuration of the power unit 54 illustrated here is merely an example and is not limited to this. The power unit 54 may function as a power source for making the vehicle B movable.

The movement control device 10 includes a communication unit 20 and a control unit 30. The communication unit 20 communicates with a plurality of vehicles B. Communication between the movement control device 10 and the vehicle B is performed by communication between the position detection unit 51 and the communication unit 20. The specific configuration of the communication unit 20 is the same as that of the position detection unit 51.

The control unit 30 includes a calculation unit 31 and a storage unit 32. The calculation unit 31 includes a calculation circuit such as a CPU (Central Processing Unit) and performs various processes related to movement control of a plurality of vehicles B. The storage unit 32 stores a software program (hereinafter, simply referred to as a program) and data used for processing of the calculation unit 31. This program may be stored in the storage unit 32, or may be recorded on a recording medium that can be read by the movement control device 10 which is a computer. In this case, the movement control device 10 includes a reading device for reading a program from the recording medium. Further, the storage unit 32 stores information about the vehicle B acquired via the communication unit 20. For example, the constraint conditions, observation conditions, and the like used when controlling the movement of a plurality of vehicles B, which will be described later, are stored. Further, information indicating a search target (for example, search target T1, T2, ..., Tm) of a plurality of vehicles B, which will be described later, is stored in the storage unit 32. The search target is the target to be observed by the vehicle B or the target position. The search target may be fixed or may be moved.

Next, the movement of the plurality of vehicles by the movement control system 1 will be described with reference to FIGS. 2 to 6. FIG. 2 is a table showing an example of the relationship between the vehicle and the search target. FIG. 3 is a schematic diagram showing an example of the observation range of one vehicle. 4 and 5 are schematic views showing an example in which a plurality of vehicles move individually to their respective target positions. FIG. 6 is a flowchart showing a flow of processing performed by the movement control system in the first embodiment.

The movement control system 1 of the present embodiment controls the movement of a plurality of vehicles M while capturing at least one or more search targets (designated place, designated position, designated object) by the plurality of vehicles M. Each of the plurality of vehicles B transmits information indicating the position acquired by the position detection unit 51 to the movement control device 10 via the communication unit 53. Further, each of the plurality of vehicles B transmits the information indicating the search target acquired by the target detection unit 52 to the movement control device 10 via the communication unit 53. The vehicle B may include information on whether the search target has been captured and the distance from the search target as information indicating the search target, or may include information on the position of the search target T. The storage unit 32 cumulatively stores information indicating the positions of the plurality of vehicles B and supplementary information of the search target T. The calculation unit 31 calculates and evaluates the movement path of each of the plurality of vehicles B based on the information indicating the position of each of the plurality of vehicles B, the information indicating the search target, the constraint condition, and the observation condition. The movement route is determined based on the evaluation result, the control input of each of the plurality of vehicles B is calculated based on the determined movement route, and the control input is individually transmitted to the plurality of vehicles B via the communication unit 20. To do. Here, the control input functions as information indicating a moving direction and a moving speed for moving the plurality of vehicles B while satisfying a predetermined condition in a unit time. The vehicle B operates the power unit 54 so as to move according to the control input. Further, the storage unit 32 cumulatively stores the control inputs transmitted to each of the plurality of vehicles B.

The movement control system 1 of the present embodiment controls the movement of the vehicle B by maintaining a state in which the number of vehicles B observing the search target T is equal to or more than the set number. When there are a plurality of search targets T, each search target is set. The number of vehicles B observing the search target T can be set by the user. If the search target T can be identified, the number of vehicles B observing the search target T may be set for each search target T. For example, if the number of vehicles B observing the search target T is set to 2 as a constraint condition, it is sufficient that the number of vehicles B observing the search target T is 2 or more, and even 3 generations have 4 vehicles. But it may be.

Further, when the vehicle B includes a plurality of search targets T within the range that can be detected by the target detection unit 52, the plurality of observable search targets T can be set as search targets that can be observed by the vehicle B. When two or more vehicles observe all the search targets at the same time, for example, the search target T1 is in the field of view (detectable range) of both the vehicle B1 and the vehicle B2, and the search target T1 is in the field of view of both the vehicle B1 and the vehicle B3 (for example). There is a search target T2 in the detectable range). Here, when there are two search targets and there are other vehicles and the above relationship is satisfied, for example, the vehicle B4 may not have a search target in the field of view (detectable range).

と表すことができる。ここで，Ｍは、制御対象であるビークル群のビークルＢの台数を表し、ｋは離散時間のインデクスを表す。 Next, the relationship between the positions and postures of the vehicle B and the vehicle B to be controlled, the position of the search target T, and the control input which is a movement instruction input to the vehicle B will be described. First, the position and orientation of the _{vehicle B m in the two-dimensional plane is}

It can be expressed as. Here, M represents the number of vehicles B in the vehicle group to be controlled, and k represents the discrete-time index.

と表すことができる。 Next, the vector of only the position of the vehicle Bm in the two-dimensional plane is

It can be expressed as.

と表すことができる。なお、本説明では、簡単のためビークルＢと、捜索対象Ｔの数を同じＭとし、識別文字をｍとしているが、数は、特に限定されない。ビークルＢは、２台以上あればよく、捜索対象Ｔは１つ以上あればよい。 Next, the position of the search target T _{m on} the two-dimensional plane is

It can be expressed as. In this description, for the sake of simplicity, the number of the vehicle B and the search target T is the same M, and the identification character is m, but the number is not particularly limited. Vehicle B may be two or more, and search target T may be one or more.

Next, the control input to the _{vehicle B m is u m} (k). As an example of the control input, there is a combination of a speed command value v _m (k) [m / sec] and a yaw rate command value φ _m (k) [rad / sec]. The control input may be any command value that can control the movement of the vehicle B, and the parameters are not necessarily limited to these.

とする。 In the following, the control input _u m a (k),

And.

となる。 If the current position and orientation of the vehicle _{B m} is a _p'm (k), the control input _u m (k) is applied, the position of the vehicle _{B m} at k + 1 time, the function h representing the dynamics,

Will be.

となる。ここで、Ｔ_ｃは、速度などを制御する際の制御周期を表す。上記は一例であり、空中ドローンや水中ビークルなど、ビークルの種類によって異なる。なお、技術は、ダイナミクスを示す関数ｈは、対象のビークルに基づいて、取得して適用すればよい。 Here, since the function h indicating this dynamics is a two-dimensional plane in this embodiment, for example, in the case of a vehicle traveling on land.

Will be. Here, T _c represents a control cycle for controlling speed and the like. The above is just an example and depends on the type of vehicle, such as an aerial drone or an underwater vehicle. In the technique, the function h indicating the dynamics may be acquired and applied based on the target vehicle.

Movement control system 1, the position and orientation information _p'm of the vehicle B (k), m = 1,2,3 , ···, M and the position information of the search target _{T q m (k), m} = 1, 2, 3, ..., as input M, for non-holonomic vehicle group covers the search target group by the target detection unit 52, an appropriate control input _{u m (k), m =} 1,2,3 , ..., M is calculated.

In the movement control device 10, the vehicle B has information in a range that can be observed by the sensor of the target detection unit 52. In the present embodiment, as shown in FIG. 3, in the vehicle B, the field of view A of the target detection unit 52 is in the shape of a cone, and the viewing angle thereof is ± β. For example, when the field of view A includes the search target T ₁ , the vehicle B ₁ is a search target within a range in which the search target T _{1 can be detected.}

（式１）
となる。なお、本実施形態では、下限値のみ不等号で示すが、等号で示したり、上限値を設定したりしてもよい。 At this time, the constraint condition (observation condition) that all the search target Ts are always in the field of view of two or more vehicles B at the same time is

(Equation 1)
Will be. In the present embodiment, only the lower limit value is indicated by an inequality sign, but it may be indicated by an equal sign or an upper limit value may be set.

である。また、Ｐ´（ｋ＋ｎ）＝［ｐ_１´^Ｔ（ｋ＋ｎ）・・・ｐｍ´^Ｔ（ｋ＋ｎ）］^Ｔであり、ｃはパラメータである。角度ψ_ｍｌは、ビークルＢ_ｍから見た捜索対象Ｔ_ｌの方位を表す。 Here, the relationship between each vehicle B _m and each search target T _m is as shown in FIG. 2, and is summarized below.

Is. Further, a ^{_{P'(k + n) = [}} p 1 'T (k + n) ··· pm' T (k + n)] T, c is a parameter. The angle ψ _ml represents the direction of the search target T _l as seen from the vehicle B _m.

として、ビークルＢ_ｍの将来の状態を予測する。関数ｆは、捜索対象Ｔ_ｌに対するビークル群全体の観測精度となる。個々の評価値Ｓ_ｍｌは、ビークルＢ_ｍが捜索対象Ｔ_ｌを観測できているかどうかを表している。観測できている（視野内に捜索対象が入っている）場合はＳ_ｍｌ＝１となり、観測できていない場合Ｓ_ｍｌ＝０となる。本実施形態体は、２台以上のビークルＢ_ｍが捜索対象Ｔ_ｌ，ｌ＝１，２，３，・・・，Ｍを同時に観測できている必要がある。図２において、各列に２台以上の“１”がある場合、条件を満足する。その条件を不等式の形で表現すると、上記制約条件の（式１）となる。 Movement control device 10 of the present embodiment includes a look-ahead and look-ahead step number (prediction horizon) and N _PH. The movement control device 10 uses the above-mentioned dynamics model.

As a result, the future state _{of the vehicle B m is predicted.} The function f is the observation accuracy of the entire vehicle group with respect to _{the search target T l.} The individual evaluation value S _ml indicates whether or not the vehicle B _m can observe the search target T _l. If it can be observed (the search target is in the field of view), S _ml = 1, and if it cannot be observed, S _ml = 0. In the present embodiment, _{it is necessary that two or more vehicles B m} can simultaneously observe the search target T _l , l = 1, 2, 3, ..., M. In FIG. 2, when there are two or more "1" s in each row, the condition is satisfied. When the condition is expressed in the form of an inequality, it becomes (Equation 1) of the above constraint condition.

となる。なお、捜索対象が検出できる範囲にあるビークルの台数であるＤは、全てのビークルの数Ｍ以下、つまりＭ≧Ｄである。 This embodiment is a case where all search targets are observed by two or more vehicles at the same time. The observation condition (constraint condition), which is a constraint that generalizes the constraint condition and causes vehicles of D or more to observe all search targets at the same time, is

Will be. Note that D, which is the number of vehicles within the range in which the search target can be detected, is the number M or less of all vehicles, that is, M ≧ D.

Here, the movement control device 10 solves the constrained optimization problem and inputs the control input so that all the search targets can be detected by the set number of vehicles in the vehicle group with (Equation 1) as the constraint condition. Find the optimal solution u _m (k). Here, in the movement control system 1 shown in FIG. 4, all the search targets can be observed by two or more vehicles, and the constraint condition is satisfied. On the other hand, the movement control system 1 shown in FIG. 5 does not satisfy the constraint condition because the _{search target T 3 has only one observable vehicle.} When the movement of the vehicle B is restricted, for example, when the turning angle is restricted, the movement control device 10 will be in the state shown in FIG. 5 after a predetermined time due to the movement restriction when it is in the position shown in FIG. , It becomes a deadlock state in which the control input satisfying the constraint condition cannot be calculated.

（式２）
を解き，制御入力の最適解の系列ｕ_ｍ（ｋ），ｕ_ｍ（ｋ＋１），・・・，ｕ_ｍ（ｋ＋Ｎ_ＰＨ−１），ｍ＝１，２，３，・・・，Ｍを求める。ここで、将来の予測位置姿勢Ｐ´（ｋ＋ｎ），ｎ＝１，２，３，・・・，Ｎ_ＰＨ−１は、制御入力の関数である。なお、この最適化問題の解法としては，逐次二次計画法や内点法などの公知技術を用いることができる。 Movement control device 10 of the present embodiment, using the preceding読量N _PH, the optimization problem based on the idea of model predictive control

(Equation 2)
The solved, sequence _u m of the optimal solution of the control input _{(k), u m (k} + 1), ···, u m (k + N PH -1), m = 1,2,3, ···, seeking M .. Here, the future predicted position / attitude P'(k + n), n = 1, 2, 3, ..., N _PH -1 is a function of the control input. As a method for solving this optimization problem, a known technique such as a sequential quadratic programming method or an interior point method can be used.

Thus, the movement control unit 10, using the preceding読量(preliminary read-out step min) N _PH, so as to satisfy the constraints of the above formula (1), based on the position of the search target T, pre-reading The movement route of the vehicle B for the quantity is calculated, the movement of the vehicle B for the unit time is calculated based on the calculated movement route, and the control input for the vehicle B is calculated based on the calculation result.

Hereinafter, an example of the process executed by the movement control device 10 will be described with reference to FIG. The movement control device 10 detects the positions and postures of the plurality of vehicles B (step S12). The position control device 10 acquires the position and orientation information detected by the position detection unit 51 from the vehicle B via the communication unit 70. Next, the movement control device 10 measures the positions of a plurality of search targets (step S14). The movement control device 10 processes the information of the search target detected by the target detection units 52 of the plurality of vehicles B, and measures the position of the search target. When the position of the search target is known, the movement control device 10 may use the known information to measure the position of the search target. Further, the movement control device 10 may acquire information on the position of the search target T from a device other than the vehicle B and use it as a measurement result.

Next, the movement control device 10 determines whether l <M (step S16). Here, l is an identifier of the search target. M is the total number of search targets in this embodiment. l becomes 1 at the start of processing. When the movement control device 10 determines that l <M (Yes in step S16), that is, the processing of all the search targets is not completed, the vehicle number is initialized, that is, m = 1 (step). S18). Next, the movement control device 10 determines whether m <M (step S20). When the movement control device 10 determines that m <M (Yes in step S20), that is, the processing of all the vehicles is not completed, the movement control device 10 calculates or measures _{the direction ψ ml of the search target l as seen from the vehicle m.} (Step S22), the success or failure of the observation of the search target l by the vehicle m is measured (step S24), and the vehicle number is updated, that is, m = m + 1 (step S26). Here, whether the vehicle can observe the search target can be _{calculated by s ml} = tanh {c (β-ψ _ml )}.

Next, when the movement control device 10 determines that m <M (No in step S20), that is, that all vehicle processing has been completed for the target search target, the entire group observes the search target. The success / failure calculation is calculated (step S30). That is, it calculates how many vehicles are within the range where the search target can be detected. When the calculation is completed, the movement control device 10 updates the search target number, that is, sets l = l + 1, and returns to step S16. The movement control device 10 calculates the relationship between the search target and the vehicle by repeatedly calculating the processes from step S12 to step S32.

When the movement control device 10 determines that l <M (No in step S16), that is, the processing of all the search targets is completed, the movement control device 10 solves the restricted optimization problem of (Equation 2) and is optimal. The control input U (k) is calculated (step S40). Movement control unit 10 allows the preliminary read-out step number N _PH, calculates the movement of satisfying the constraints vehicle. That is, the set number of each search target, and in the present embodiment, the movement locus of the vehicles that can be continuously measured by the two vehicles is calculated. The movement control device 10 calculates the control input of the vehicle for a unit time based on the calculation result. The unit time is shorter than the time corresponding to the number of read-ahead steps. The movement control device 10 calculates a movement route for a time (number of read-ahead steps − unit time) longer than the time actually moved by the control input, and calculates the movement control input for the unit time from the calculation result.

The movement control device 10 transmits the calculated control input U (k) as speed and yaw rate control command values to each vehicle (step S42). Each vehicle controls the speed and attitude of the vehicle based on the input control command value (step S44), and updates the time after the lapse of a unit time (step S46). The movement control device 10 determines whether the processing is completed (step S48), and if it is determined that the processing is not completed (No in step S48), the movement control device 10 returns to step S12 and determines that the processing is completed (Yes in step S48). If so, this process ends.

Movement control system 1, using the idea of model predictive control, the preliminary read-out step number (prediction horizon) and N _PH, predict satisfies the number of constraints of the vehicle that can be observed search target in future position of the vehicle group ( Look-ahead), calculate the movement route, and determine the movement for a unit time based on the calculated movement route. As a result, when the vehicle moves and then moves next time, it is possible to prevent the movement of the movable area of the vehicle from causing a deadlock that does not satisfy the constraint condition. In addition, by using the number of vehicles in the range where the search target can be observed as the constraint condition, the user can easily determine the number of vehicles required to observe one search target simply by adjusting the constraint condition. Can be specified.

Further, in the present embodiment, the position of the search target is detected at the time of the optimization calculation, but the movement of the search target may be predicted and the position of the search target may be moved according to the elapsed time. In this case, the prediction may be made based on the movement trajectory of the search target in the past, or the movement may be made under predetermined conditions. Moreover, the search target may be fixed.

[Second Embodiment]
Next, the second embodiment will be described. The same components as those in the first embodiment are designated by the same reference numerals, and the description thereof will be omitted. Except for special matters, the second embodiment is the same as the first embodiment. FIG. 7 is a schematic diagram showing an example of processing performed by the movement control system in the second embodiment.

（式３）
を用いる。 In the movement control device 10 of the second embodiment, the vehicle group simultaneously reaches each search target as the evaluation function J (U (k), U (k), ..., U (k + N _{PH +1)).} Is a condition. In the movement control device 10 of the second embodiment, the search target of each vehicle can be reached at the same time while the number of vehicles under the constraint condition is observing the search target. In order for the vehicle to reach the search target, the relative distance between the vehicle and the search target must be zero at the target time. The movement control device 10 is used as an evaluation function.

(Equation 3)
Is used.

Evaluation function of model predictive control of the present embodiment, like the first embodiment, for performing the read ahead processing comprises the preliminary read-out step N _PH. Specifically, Tm (k + N _PH ) is a function of U (k), U (k), ..., U (k + N _PH +1).

（式４）
を含む。ここで、Ｔ_ｍ，Ａ（ｋ＋ｍ）は、ビークルＢ_ｍの捜索対象Ｔ_ｍへの到達予想時刻である。Ｔ_ｍ，Ａ（ｋ＋ｍ）は、現在時刻ｔ_ｎｏｗを用いて、

となる。Ｔ_ｓｅｔは、ユーザが指定したビークル群の捜索対象への同時到達の目標時刻である。 Further, the movement control device 10 of the present embodiment is set as a constraint condition because the arrival time of each vehicle at the search target is also arbitrated.

(Equation 4)
including. Here, T _{m and A} (k + m) are estimated arrival times of _{the vehicle B m to} the search target T _m. For T _{m and A} (k + m), use the current time to _now .

Will be. T _set is a target time for simultaneous arrival at the search target of the vehicle group specified by the user.

The movement control system 1 of the present embodiment uses (Equation 3) as an evaluation function, and sets the constraint condition of (Equation 1) and the constraint condition of (Equation 4) regarding the arrival timing of the first embodiment to vehicle. The entire group will be able to reach the search target position at the specified time while constantly observing the search target and suppressing deadlock.

The evaluation function of the present embodiment is a function that evaluates only the deviation between the final arrival point (target time point) of the look-ahead and the search target position without evaluating the route in the middle. As a result, the degree of freedom of the route on the way is increased, and the occurrence of deadlock can be further suppressed. As an example, in the middle route, since the vehicle moves to a position where it is easy to observe the search target, an action such as making a detour is also generated. Specifically, as shown in FIG. 7, if the route in the middle is also evaluated, the evaluation of the route 102 is high in order to approach the search target earlier. However, in the present embodiment, the route in the middle is not evaluated. , The degree of freedom of movement is increased, the search target can be observed more reliably during the look-ahead processing, and the route 104 that can be moved within the range of restrictions on the movement of the vehicle can be calculated.

[Third Embodiment]
Next, the third embodiment will be described. The same components as those in the first embodiment are designated by the same reference numerals, and the description thereof will be omitted. Except for special matters, the third embodiment is the same as the first embodiment.

The movement control system 1 of the present embodiment includes, in addition to the evaluation functions / constraints of the first embodiment and the second embodiment, conditions for avoiding collisions between vehicles and conditions for upper and lower limits of control input as constraint conditions.

が制約条件として、最適化問題に含まれる。 The movement control system 1 of the present embodiment includes a constraint condition that the relative distance is equal to or more than the threshold value d at each time in the future prediction in order to avoid a vehicle collision. In particular,

Is included in the optimization problem as a constraint.

がある。ここで、ｖ_ＭＩＮとｖ_ＭＡＸは、それぞれ速度指令の下限と上限を表す。下限がｖ_ＭＩＮ＞０であれば、ビークルを停止させないように最適な制御入力を生成することができる。また、φ_ＭＩＮとφ_ＭＡＸは、それぞれヨーレート指令の下限と上限を表す。左旋回と右旋回で同等の制約を課す場合は、φ_ＭＩＮ＝−φ_ＭＡＸとすれば良い。 Further, the movement control system 1 of the present embodiment includes a constraint condition for the control input so that the vehicle can move in the calculated locus corresponding to the control input. In the movement control system 1, upper and lower limits such as vehicle speed and yaw rate are set as control input limit values. As a result, an excessive control input can be output to cause the vehicle to break down, and in the case of a flying vehicle, it can be lowered. As a constraint condition for control input,

There is. Here, v _MIN and v _MAX represent the lower limit and the upper limit of the speed command, respectively. If the lower limit is v _MIN > 0, the optimum control input can be generated so as not to stop the vehicle. Further, φ _MIN and φ _MAX represent the lower limit and the upper limit of the yaw rate command, respectively. If the same restrictions are imposed on turning left and turning right, φ _MIN = −φ _MAX may be used.

These constraints are also included in the optimization problem, and by solving the optimization problem based on these constraints, control that satisfies the constraints can be executed. The movement control system 1 can set various constraint conditions based on the relative position of the vehicle and the movement performance of the vehicle. By adding to the constraint condition of the optimization problem, the movement control system 1 can generate a t-tail that calculates the movement locus of the vehicle satisfying the constraint condition when calculating the movement locus of the vehicle by pre-reading.

[Fourth Embodiment]
Next, the fourth embodiment will be described. The same components as those in the first embodiment are designated by the same reference numerals, and the description thereof will be omitted. Except for special matters, the fourth embodiment is the same as the first embodiment. FIG. 8 is a schematic diagram showing an example of processing performed by the movement control system in the fourth embodiment. FIG. 9 is a schematic diagram showing an example of processing performed by the movement control system in the fourth embodiment.

The movement control system 1 of the present embodiment is based on the information between the points of the movement route calculated by using the _{predicted horizon N PH,} and the adjacent points of the route, for example, the movement route calculated by using the _{predicted horizon N PH.} Calculate the route from the current point to the next point in parallel. It is preferable that this embodiment is carried out in combination with the second embodiment. Since the movement control system 1 of the second embodiment includes the constraint condition of simultaneous arrival, it is necessary to generate a complete route to the search target position. Therefore, the predicted horizon N _PH may become large, and the processing time for solving the constrained optimization problem may become long. In order to shorten the processing time, the unit time (interval between one step of the look-ahead step) is set to be long, and it is possible to suppress a decrease in the accuracy of the movement control.

のステップｋ，ｋ＋１，ｋ＋２，・・・の間隔をＴ_ＭＰＣ［ｓｅｃ］とする。本実施形態で用いる時間間隔Ｔ_ｃは、Ｔ_ＭＰＣ＞＞Ｔ_Ｃを満たす。また、ダイナミクスｈを用いて将来の状態を予測する際、Ｔ_ＭＰＣ［ｓｅｃ］間は、同じ制御入力をビークルに印加し続けるものと仮定する。すなわち、

とダイナミクスを設定し、将来位置を予測する。 The movement control system 1 of the present embodiment predicts the future position of each vehicle.

_{Let TMPC} [sec] be the interval between steps k, k + 1, k + 2, .... Time interval _{T c} used in the present _embodiment, satisfies the _T MPC >> T _C. Further, when predicting the future state using the dynamics h, it is assumed that the same control input is continuously applied to the vehicle during _{TMPC [sec].} That is,

And set the dynamics to predict the future position.

Similar to the first to third embodiments, the movement control system 1 plans a route to the search target position by the _{time interval TMPC using the above dynamics.} The route plan to the search target position calculated by the time interval _{TMPC is referred to as "global route plan".} The P'the ^{(k + n) = [T1'} T (k + n) ··· Tm' T (k + n)] T, and "waypoints".

The global route plan satisfies the conditions for observing the search target in (Equation 1) at the waypoints and various constraints, but may include cases where the conditions are not satisfied during the movement between the waypoints.

The movement control system 1 of the present embodiment _{uses the local system that operates at the time interval Tc} described above, creates a control input when moving between waypoints in the global route plan, and controls the vehicle. Hereinafter, the processing of the local system of the movement control device 10 of the movement control system 1 will be described. In the present embodiment, the processing of the local system is executed by the movement control device 10, but the processing of the local system may be executed by each vehicle.

The movement control device 10 calculates the movement route and calculates the control input without performing the process including the read-ahead step used in the global plan. As a result, the amount of calculation processing can be reduced and the calculation processing can be speeded up. The movement control device 10 sets the target position of the local system as the next waypoint instead of the search target position. Waypoints, ^P'(k + n) = as described as [T1' T (k + n) ··· Tm' T (k + n)] T, the target position for each unit time of each vehicle. Therefore, it is a constraint that the movement control device 10 arrives at the waypoint after the lapse of a unit time.

とする。これにより、グローバル経路計画が生成した経路に可能な限り沿ってビークル群を移動させる経路の評価が高くすることができ、ビークルがグローバル経路計画が生成した経路から大きく逸脱することを抑制できる。 The movement control device 10 uses an evaluation function.

And. As a result, it is possible to evaluate the route for moving the vehicle group along the route generated by the global route plan as much as possible, and it is possible to prevent the vehicle from deviating significantly from the route generated by the global route plan.

である。下位制御周期Ｔ_ｃ[ｓｅｃ]毎にτを０からＴ_Ｃ／Ｔ_ＭＰＣに更新していき，次のウェイポイントにビークル群を移動させる。上記式に示す通り、最適化の対象は、次の制御入力のみである。したがって、上述したようにローカルシステムは、先読は行わない。 Where w _m ^τ is

Is. The τ in each lower control period _{T c} [sec] continue to update from 0 to _T C _{/ T MPC,} to move the vehicle group to the next waypoint. As shown in the above equation, the target of optimization is only the following control inputs. Therefore, as mentioned above, the local system does not pre-read.

が観測条件となり制約条件となる。 The movement control device 10 includes the same constraints as the global route plan because the search target is always observed both in the local system and when moving between waypoints. That is, the local system

Becomes an observation condition and a constraint condition.

を制約条件とする。ここで，α＝１，２，・・・，Ｎ_ＰＨは，次のウェイポイントＰ（ｋ＋１）がグローバル経路計画全体の何番目のウェイポイントかを表す自然数である。 The roll system also allows each vehicle to reach waypoint P (k + 1) at the same time.

Is a constraint. Here, α = 1, 2, ..., _NHP is a natural number indicating which waypoint the next waypoint P (k + 1) is in the entire global route plan.

As shown in FIG. 8, the movement control device 10 processes the global plan 120 and the local system 122 in parallel. For example, the global plan 120 requires a required time of 126 to calculate the plan, and when the calculation is started at time k, the calculation is performed until the calculation completion time of 130. The global plan 120 transmits the waypoint information 140 at least at time k + 1 to the local system 122 at time k. Further, the global plan 120 transmits the waypoint information 142 at least at time k + 2 to the local system 122 at time k + 1. _{On the other hand, the local system 122 calculates the target position in the period T c} based on the waypoint information 140 and the information of the current position, and calculates the control input based on the calculated position information. In the local system 122, the time related to the calculation of the next control input is 132, which is shorter than _{the period T c.} The movement control system 1 _{calculates the target position in the cycle T c} , and as shown in FIG. 9, connects the waypoint 150 and the waypoint 160 at the target position 162 calculated in each _{cycle T c.} The movement route 151 can be calculated.

The mobile control system can suppress the occurrence of waiting time for control input and can finely adjust the route by processing in parallel in the local system. In addition, the local system can move according to the global plan while simplifying the calculation by using the waypoint information without performing the pre-reading process.

Further, as shown in FIG. 8, the movement control device 10 suppresses the occurrence of a waiting time for calculating waypoints by calculating the next global route while the vehicle is operating in the local system. can do. In this case, the movement control device 10 starts the calculation before reaching the starting point at the time of calculating the global plan. Therefore, it is assumed that the movement control device 10 sets the position and orientation of the next waypoint of the vehicle as the current position and attitude, that is, the vehicle always reaches the next waypoint for each _{periodic TNPC.} Plan the next global route. As a result, the vehicle group can be continuously moved without causing any waiting time for each waypoint.

[Fifth Embodiment]
Next, the fifth embodiment will be described. The same components as those in the first embodiment are designated by the same reference numerals, and the description thereof will be omitted. Except for special matters, the fifth embodiment is the same as the first embodiment. FIG. 10 is a schematic diagram showing an example of processing performed by the movement control system in the fifth embodiment.

The movement control system 1 of the present embodiment includes a function of increasing the accuracy of the route while optimizing the load of the look-ahead processing according to the movement of the vehicle with respect to the search target. Movement control system 1, the global path planning, the processing cycle (unit time) T _MPC process of calculating the movement trajectory including look-ahead of the present embodiment is fixed and the preliminary read-out step number (prediction horizon) When N _PH also be fixed , There is a possibility that the movement route to reach the target position to be searched is not calculated. Further, when the number of read-ahead steps is constant, the calculation load does not change. Further, even if the processing cycle (unit time) _TMPC is changed, the processing load increases if the number of waypoints does not change.

The movement control system 1 of the present embodiment reduces the predicted horizon by one each time the global route is calculated, for example, every time a unit time elapses or every time a waypoint is passed. That is, the movement control system 1 sets N _PH = max {N _PH- 1,1} after completing the calculation of the global route.

As a result, as shown in FIG. 10, it is possible to prevent the interval between the waypoints 178 of the path 172 after passing and the interval of the waypoints 178 of the path 174 from which the vehicle B1 is going from changing in the movement locus 170. In addition, the fluctuation of the movement locus 170 can be reduced. The predicted horizon value may be gradually decreased, and the timing of the decrease is not particularly limited.

The first embodiment, the second embodiment, the third embodiment, the fourth embodiment, the fifth embodiment, and the modified examples have been described above, but these can be arbitrarily combined. That is, the first embodiment, the second embodiment, the third embodiment, the fourth embodiment, and the fifth embodiment can be used together without competing with or inconsistent with other embodiments. Further, the modified example can be applied to the first embodiment, the second embodiment, the third embodiment, the fourth embodiment and the fifth embodiment, and the embodiment by a combination thereof.

In the above description, the model prediction control is applied to the calculation of the control input of each of the plurality of vehicles B, but the calculation method of the control input is not limited to this. A nonholonomic model, such as an equivalent two-wheeled vehicle model, may be used.

The embodiments and modifications are presented as examples and are not intended to limit the scope of the invention. The embodiments and modifications can be implemented in various other forms, and various omissions, replacements, and changes can be made without departing from the gist of the invention. The embodiments and modifications are included in the scope and gist of the invention, as well as in the scope of the invention described in the scope of patent claims and the equivalent scope thereof.

1 movement control system 10 moves the controller 20,53,70 communication unit 30 control unit 31 operation unit 32 storage unit 51 the position detecting unit 52 target detection unit 54 power section _{B, B 1, B 2,} B 3, B M Vehicle T , T ₁ , T ₂ , T _{3, TM} Search target

Claims (13)

It is a movement control method of a plurality of vehicles that moves the plurality of vehicles while observing at least one search target by the plurality of vehicles.
The step of acquiring the position and posture of each of the plurality of vehicles, and
The step of identifying the position of the search target and
A step of calculating a movement locus for a look-ahead step of the plurality of vehicles based on the position and posture of each of the plurality of vehicles and the position of the search target.
When the plurality of vehicles move according to the calculated movement trajectory, a step of determining whether or not the observation conditions of the search target are satisfied, and
If the observation conditions are not satisfied, the step of returning to the step of calculating the movement locus and the step of returning to the step of calculating the movement locus,
When a movement locus satisfying the observation conditions is calculated, a step of determining the driving condition of the vehicle from the present time to a unit time later based on the movement locus and controlling the movement of the vehicle, and a step of controlling the movement of the vehicle.
How to control the movement of multiple vehicles, including. The vehicle movement control method according to claim 1, wherein the position of the search target is a position detected by the vehicle. The vehicle movement control method according to claim 2, wherein the observation conditions include whether or not the number of vehicles capable of observing the search target is equal to or greater than a set number. In the observation conditions, for each search target, the vehicle within the range in which the search target can be observed is set to the f () unit, the lower limit value of the vehicle capable of observing the search target is set to D units, and the total number of the vehicles is set to M. The vehicle movement control method according to claim 2, wherein the vehicle movement control method includes satisfying f () ≧ -1 (MD) + (1 × D). The vehicle according to any one of claims 1 to 4, wherein the movement locus is a locus in which the vehicle set for the search target arrives at the final target position and a plurality of the vehicles arrive at the time. Movement control method. The vehicle movement control method according to any one of claims 1 to 5, wherein the step of calculating the movement locus is a step of setting the distance between the vehicle and the other vehicle to be equal to or greater than a threshold distance. The step of calculating the movement locus is any one of claims 1 to 6 for calculating the movement locus of the vehicle based on a limit value that limits the fluctuation of the movement distance and direction of the vehicle for a predetermined time. The vehicle movement control method described in the section. The step of controlling the movement of the vehicle is based on the position and orientation of the vehicle after the unit time and the position and orientation of the vehicle at the present time, in a unit cycle shorter than the unit time, of the vehicle. The vehicle movement control method according to any one of claims 1 to 7, wherein the driving conditions are determined and the movement of the vehicle is controlled. The vehicle movement control method according to any one of claims 1 to 8, wherein the step of calculating the movement locus is a step of reducing the number of steps of the look-ahead step based on the movement of the vehicle. A movement control device that controls the movement of multiple vehicles.
A position detection unit that communicates with each of the plurality of vehicles and acquires information on the current position and posture of the vehicle.
An object detection unit that acquires information on the position of at least one search target,
Includes a control unit that calculates the movement locus of the vehicle that satisfies the observation condition of the search target based on the acquired information on the current position and posture of the vehicle and the information on the position of at least one search target. ,
The control unit calculates the movement locus for the look-ahead steps of the plurality of vehicles based on the positions and postures of the plurality of vehicles and the positions of the search targets.
When the plurality of vehicles move according to the calculated movement trajectory, it is determined whether or not the observation conditions of the search target are satisfied.
When the observation condition is not satisfied, the movement locus is calculated again, and when the movement locus satisfying the observation condition is calculated, the driving condition of the vehicle from the present time to a unit time later is based on the movement locus. A movement control device that determines the movement of the vehicle and controls the movement of the vehicle. With multiple vehicles
A movement control device for moving the plurality of vehicles while observing at least one search target by the plurality of vehicles is provided.
The movement control device is
A communication unit that communicates with each of the plurality of vehicles, acquires information on the current position and posture of the vehicle, and acquires information on the position of at least one search target.
Includes a control unit that calculates the movement locus of the vehicle that satisfies the observation condition of the search target based on the acquired information on the current position and posture of the vehicle and the information on the position of at least one search target. ,
The control unit calculates the movement locus for the look-ahead steps of the plurality of vehicles based on the positions and postures of the plurality of vehicles and the positions of the search targets.
When the plurality of vehicles move according to the calculated movement trajectory, it is determined whether or not the observation conditions of the search target are satisfied.
When the observation condition is not satisfied, the movement locus is calculated again, and when the movement locus satisfying the observation condition is calculated, the driving condition of the vehicle from the present time to a unit time later is based on the movement locus. A movement control system that determines the movement of the vehicle and controls the movement of the vehicle. A program for moving a plurality of vehicles while observing at least one search target by a plurality of vehicles.
On the computer
The step of acquiring the position and posture of each of the plurality of vehicles, and
The step of identifying the position of the search target and
A step of calculating a movement locus for a look-ahead step of the plurality of vehicles based on the position and posture of each of the plurality of vehicles and the position of the search target.
When the plurality of vehicles move according to the calculated movement trajectory, a step of determining whether or not the observation conditions of the search target are satisfied, and
If the observation conditions are not satisfied, the step of returning to the step of calculating the movement locus and the step of returning to the step of calculating the movement locus,
When a movement locus satisfying the observation conditions is calculated, the driving conditions of the vehicle from the present time to a unit time later are determined based on the movement locus, and a step of controlling the movement of the vehicle is executed. Program for. A computer-readable recording medium that records a program for moving a plurality of vehicles while observing at least one search target by a plurality of vehicles.
On the computer
The step of acquiring the position and posture of each of the plurality of vehicles, and
The step of identifying the position of the search target and
A step of calculating a movement locus for a look-ahead step of the plurality of vehicles based on the position and posture of each of the plurality of vehicles and the position of the search target.
When the plurality of vehicles move according to the calculated movement trajectory, a step of determining whether or not the observation conditions of the search target are satisfied, and
If the observation conditions are not satisfied, the step of returning to the step of calculating the movement locus and the step of returning to the step of calculating the movement locus,
When the movement locus satisfying the observation conditions is calculated, the driving conditions of the vehicle from the present time to a unit time later are determined based on the movement locus, and the steps of controlling the movement of the vehicle are executed. A computer-readable recording medium on which the program is recorded.

Images