Motion planning device

Abstract

The present disclosure relates to an apparatus for planning motion of a dynamic system using a sampling method, the apparatus having an action planning unit that outputs a mathematical model expressing the motion of the dynamic system based on a goal to be achieved by the dynamic system and state constraints of the dynamic system that should be considered in advance, and an action plan generating unit that limits an input sampling range based on the mathematical model and the state constraints, and generates a motion plan for the dynamic system by performing a state estimation calculation within the input sampling range based on the mathematical model.

Description

[Technical field]

[0001] The present disclosure relates to a motion planning device, and more particularly to a motion planning device for a dynamic system using a sampling technique. [Background technology]

[0002] Several methods have been proposed for motion planning problems of dynamic systems, one of which is a sampling method. In motion planning problems, there are constraints that the target must satisfy, but the sampling method has the advantage of being able to easily handle complex constraints, since it creates a motion plan after determining for each particle whether it is valid particles that satisfy the constraints (valid particles) or invalid particles that do not (invalid particles).

[0003] For example, in the vehicle control system of Patent Document 1, the control input is sampled, and then the time evolution of a physical model is calculated, and effective particles are determined for state constraints, thereby controlling the vehicle so as to optimize the probability of achieving the desired motion using only effective particles.

[0004] In addition, in the autonomous driving system of Patent Document 2, the control input is sampled within the constraints of road surface slippage, and the vehicle is controlled to optimize the probability of achieving the desired motion using only effective particles for the road surface slippage. [Prior art documents] [Patent documents]

[0005] [Patent Document 1] Patent No. 6494872 [Patent Document 2] Patent No. 6594589 [Non-patent literature]

[0006] [Non-Patent Document 1] AD Ames X. Xu, JW Grizzle and P. Tabuada. “Control barrier function based quadratic programs for safety critical systems.” IEEE Transactions on Automatic Control, Vol. 62, No. 8, pp.3861-3876, 2016. [Non-Patent Document 2] Q. Nguyen and K. Sreenath. “Exponential control barrier functions for enforcing high relative-degree safety-critical constraints.” 2016 American Control Conference. 2016. Summary of the Invention [Problem to be solved by the invention]

[0007] In the sampling method, a large amount of sampling calculation is required to guarantee the approximation accuracy of the probability density distribution approximated based on the sampling. When calculating the time evolution of a physical model after performing input sampling and determining effective particles that satisfy the constraints as in Patent Document 1, if the number of effective particles is small, the approximation accuracy of the probability density distribution cannot be guaranteed. In other words, the possibility of the performance of the method deteriorating due to a decrease in effective particles increases.

[0008] In addition, in the method of Patent Document 2, since the input sampling is limited in advance, it is easy to ensure the number of effective particles. However, since it is limited to the slipperiness of the road surface, it is difficult to apply it to other applications, and when other state constraints are taken into account, the results are similar to those of Patent Document 1.

[0009] The present disclosure has been made to solve the problems described above, and aims to provide a motion planning device using a sampling method that is unlikely to cause a degradation in the functionality of the method due to a decrease in effective particles, and that does not limit the state constraints that must be considered in advance in the system to which the method is applied. [Means for solving the problem]

[0010] The motion planning device according to the present disclosure is a motion planning device for a dynamic system using a sampling method, the motion planning device including: an action planning unit that outputs a mathematical model expressing the motion of the dynamic system based on a goal to be achieved by the dynamic system and a state constraint to be considered in advance of the dynamic system; In advance The system further includes an operation plan generating unit that limits an input sampling range and generates an operation plan for the dynamic system by performing a state estimation operation within the limited input sampling range. Effect of the Invention

[0011] According to the motion planning device disclosed herein, input sampling is performed within the range of state constraints that should be considered in advance, thereby improving the approximation accuracy of the probability density distribution approximated based on the sampling, reducing the possibility of the performance of the method deteriorating due to a decrease in effective particles, and there is no limit to the state constraints that should be considered in advance in the system to which the method is applied. [Brief description of the drawings]

[0012] [Figure 1] 1 is a schematic diagram illustrating a configuration of a moving body equipped with a motion planning device according to a first embodiment of the present disclosure. [Diagram 2] 1 is a schematic diagram illustrating a configuration of a moving body equipped with a motion planning device according to a first embodiment of the present disclosure. [Diagram 3] FIG. 2 is a diagram illustrating an example of a situation in which a moving object equipped with the motion planning device according to the first embodiment of the present disclosure moves. [Figure 4]1 is a block diagram showing a configuration of a motion planning device according to a first embodiment of the present disclosure. [Diagram 5] FIG. 2 is a diagram illustrating a coordinate system used in the first embodiment according to the present disclosure. [Figure 6] 5 is a flowchart showing a calculation process of a particle filter used in the motion planning device according to the first embodiment of the present disclosure. [Figure 7] FIG. 2 is a diagram illustrating a schematic result of generating a motion plan in the motion planning device according to the first embodiment of the present disclosure. [Figure 8] FIG. 11 is a diagram illustrating a method of correcting input sampling values ​​in a motion planning device according to a third embodiment of the present disclosure. [Figure 9] FIG. 13 is a diagram illustrating a method for limiting an input sampling range in a motion planning device according to a fourth embodiment of the present disclosure. [Figure 10] FIG. 1 is a diagram illustrating a hardware configuration for implementing a motion planning device according to first to fourth embodiments. [Figure 11] FIG. 1 is a diagram illustrating a hardware configuration for implementing a motion planning device according to first to fourth embodiments. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0013] <Embodiment 1> <System configuration for mobile devices> 1 and 2 are schematic diagrams showing a system configuration of a moving body 1, which is a dynamic system equipped with a motion planning device according to a first embodiment of the present disclosure. FIG. 1 is a perspective view of the moving body 1 as seen from above, and FIG. 2 is a side view of the moving body 1. As shown in FIG. 1, the moving body 1 includes wheels 101, an actuator 102, and a battery 103 as driving devices. The actuator 102 converts electric power obtained from the battery 103 into driving force and inputs it to the wheels 101. The actuator 102 is controlled by a control device 10. The control device 10 includes a storage unit and a calculation unit, and controls the actuator 102 according to a set control program to control the movement of the moving body 1.

[0014] In this embodiment, an example is shown in which the motion planning device is mounted on a mobile body 1 that can move in all directions using three wheels and three actuators, but the present invention is not limited to this and can also be mounted on, for example, a differential two-wheel mobile body or a leg-type mobile robot. Here, a differential two-wheel mobile body is a mobile body that has two wheels that can rotate independently and moves straight and turns by utilizing the difference in rotation speed between the wheels.

[0015] Furthermore, the dynamic system to which the present disclosure is applicable is not limited to a moving body, but may be any dynamic system in which the motion of a robot arm or an overhead crane is expressed as a mathematical model (differential equation).

[0016] When a dynamic system is treated as a moving body, it becomes possible to set a complex target trajectory toward a target location.

[0017] The moving body 1 is equipped with, as observation devices, a wheel angle sensor 111 shown in Fig. 1, an optical range sensor 112 shown in Fig. 2, and a depth camera 113. The observation devices are connected to a control device 10.

[0018] The wheel angle sensor 111 is provided on each wheel 101 and detects the amount of rotation of the wheel 101. The control device 10 calculates the amount of movement of the mobile object 1 based on the amount of rotation of the wheel 101. The wheel angle sensor 111 is formed of, for example, a rotary encoder.

[0019] The optical range sensor 112 is, for example, a LiDAR (Light Detection and Ranging) and is provided on the upper surface of the moving object 1. The optical range sensor 112 measures physical shape data of the space in the surrounding environment of the moving object 1 along a scanning plane. Based on the measured shape data, the control device 10 creates a map of the surrounding environment. By referring to the created map, the position of the moving object 1 on the map plane is estimated. At this time, it is also possible to create a map in advance and estimate the position of the moving object 1 by referring to the map information.

[0020] Fig. 3 is a diagram showing a schematic example of a situation in which the moving body 1 moves. In Fig. 3, the physical shape data of the space obtained by the optical range sensor 112 includes the shapes of a person 500 and an obstacle 600 that are obstacles to the movement of the moving body 1. The control device 10 recognizes these shape data as objects that the moving body 1 should avoid. In the following description, all objects that obstruct the movement of the moving body 1, such as a person, a wall, and another moving body, are referred to as obstacles.

[0021] The front depth camera 113 acquires physical shape data of the space in front of the vehicle together with an image. The front depth camera 113 acquires range information within the camera angle of view, and the optical range sensor 112 supplements the shape data on the scanning plane (data cut from a plane in a three-dimensional space).

[0022] However, the above-mentioned observation device is just one example, and the method of obtaining shape data of obstacles in the environment in which the moving object 1 moves is not particularly limited.

[0023] <Device configuration> FIG. 4 is a block diagram showing the configuration of the motion planning apparatus 300 according to the first embodiment of the present disclosure, and a block diagram showing the configurations of the drive device 100 and the observation device 200 connected to the motion planning apparatus 300.

[0024] 4, the driving device 100 includes the wheels 101, the actuator 102, and the battery 103 described above. The observation device 200 includes the wheel angle sensor 111, the optical range sensor 112, and the depth camera 113 described above.

[0025] 4 is included in the control device 10, and includes a behavior planning unit 310 and a motion plan generating unit 320. The control device 10 is a device that controls the moving object 1 according to a target trajectory, and is installed, for example, as an embedded computer.

[0026] The behavior planning unit 310 has a moving object state estimating unit 311 , a moving target computing unit 312 , and a state constraint computing unit 313 .

[0027] The moving object state estimation unit 311 executes self-position estimation of the moving object 1 based on information from a global positioning sensor (GPS), for example. The self-position information estimated by the moving object state estimation unit 311 is output to the operation plan generation unit 320 and the movement target calculation unit 312.

[0028] The moving target calculation unit 312 calculates a goal to be achieved by the moving body 1 based on the self-position of the moving body 1 output from the moving body state estimation unit 311 and the obstacle information output from the observation device 200, and outputs the goal to the state constraint calculation unit 313. The goal is, for example, a reference trajectory, a target point, information on an obstacle to be avoided, and the like.

[0029] A state constraint calculation unit 313 in the action planning unit 310 calculates and outputs a mathematical model of the moving body 1 and state constraint information that should be considered in advance, based on the goal to be achieved by the moving body 1 output from the movement target calculation unit 312. In the first embodiment, the state constraint information that should be considered in advance is relative position information between the moving body 1 and an obstacle, and is output based on information including the position of the moving body 1, the obstacle position, and the obstacle shape.

[0030] By having the state constraint calculation unit 313 within the action planning unit 310 and making it independent from the action plan generation unit 320, it is possible to handle constraints in a unified manner without relying on the mathematical model and sampling method that represent the behavior and state constraints of the dynamic system.

[0031] The relative position information output from the state constraint calculation unit 313 is input to the operation plan generation unit 320. For example, the relative distance between the moving body 1 and the surrounding environment is obtained from the optical range sensor 112 of the observation device 200. However, the relative distance is not limited to information obtained directly from the sensor, but can be a value calculated based on values ​​from one or more sensors. For example, the positions of the system to be controlled and the object to be avoided can be obtained from a GPS, and the relative distance can be calculated from each position. Also, two camera image sensors can be used to obtain image data of each, and the relative distance can be calculated using the parallax in each image data.

[0032] It is also possible to use only software calculations based on values ​​that have been mathematically transformed into phenomena without using sensors. For example, even if a person or robot is not detected, a process is performed that constantly predicts a person or robot jumping out using a mathematical formula. This is a process that uses simulation to predict the jumping out of an undetected person or a virtual wall that indicates a no-entry area for moving objects, or to predict and calculate the probability that an obstacle will appear from outside the sensor range.

[0033] The motion plan generating unit 320 has a target trajectory generating unit 321 and a target trajectory storage unit 322. The target trajectory generating unit 321 generates a target trajectory for moving while achieving a goal, taking into consideration avoiding the obstacle in advance, based on relative position information between the moving object 1 and the obstacle, for example, a relative distance, output from the action planning unit 310. A sampling method is applied to generate this target trajectory. The generated target trajectory is output to the target trajectory storage unit 322.

[0034] The target trajectory storage unit 322 stores the target trajectory obtained from the target trajectory generation unit 321, selects the necessary amount of target trajectory information, and outputs it as a motion plan to the driving device 100. The driving device 100 operates the moving body 1 according to the received motion plan.

[0035] <Moving object coordinate system> FIG. 5 is a diagram showing a coordinate system used in the first embodiment. The X-axis and Y-axis in FIG. 5 are inertial coordinate systems, and x r and y r represents the center of gravity of the moving object 1 in the inertial coordinate system. r and y r is not limited to the center of gravity as long as it can represent the position of the moving object 1, and can be, for example, the shape center point, the depth camera installation point, and the range finder installation point. x and v y are the velocities of the moving body 1 in the X and Y directions in the inertial coordinate system. o and y ois the representative point position of the obstacle 600, and is also expressed in the XY inertial coordinate system. The representative point position of the obstacle 600 can be one or more points, and can be, for example, the point at the shortest distance between the moving body 1 and the obstacle 600, the center of the shape of the obstacle 600, the center of gravity of the obstacle 600, etc.

[0036] <Sampling method> In this embodiment, the target trajectory generating unit 321 generates a target trajectory, which is an index for the control device 10 to control the moving body 1, as a motion plan by a sampling method based on information obtained from the observation device 200. The motion planning device 300 of the first embodiment performs a time evolution calculation of the state using a mathematical model f that mathematically represents the motion of the moving body 1, and generates a target trajectory by solving a designed optimization problem after taking into consideration state constraints in advance. In this embodiment, a particle filter, which is a type of sampling method, is used. The effect of the present disclosure applies to all cases in which calculations related to the time evolution of a dynamic system are included in the formulation of the sampling method, so the sampling method is not limited to the particle filter. Details of this effect will be described in a specific formulation described later.

[0037] <Formulation of motion plan generation using sampling method> In this embodiment, the state quantity x and the input u of the moving object used in the target trajectory generating unit 321 are set as shown in the following formula (1).

[0038]

number

[0039] Here, the coordinate system is the one shown in Figure 4, and v x is the velocity in the x-direction, v y is the velocity in the Y direction.

[0040] The mathematical model f representing the motion of a moving object is expressed by the following mathematical formula (2) and is linear with respect to the input. This simplifies the extraction of the input sampling range and reduces the calculation load.

[0041]

number

[0042] Note that, since formulas (1) and (2) are examples of mathematical models, the state quantity x, input u, and mathematical model f can be selected according to the characteristics of the system. The coordinate system is not limited to the Cartesian coordinate system, and can be defined, for example, in the route coordinate system.

[0043] In this embodiment, as a state constraint to be considered in advance, a state constraint h(x)≧0 that prevents the moving object 1 from colliding with an obstacle is expressed as the following formula (3).

[0044]

number

[0045] where r m is a value that represents a certain distance from the obstacle. As long as the state constraint h(x) ≥ 0 is satisfied, the obstacle and r m This means that the object moves while maintaining a distance equal to or greater than this. Also, this state constraint h(x) ≧ 0 can be considered as h(x) ≦ 0. However, since this embodiment is concerned with the state constraint h(x) ≧ 0, attention should be paid to the positive and negative signs.

[0046] In addition, the state constraint h(x) is a scalar value. Since the state constraint h(x) is a scalar value, the extraction of the input sampling range can be simplified and the calculation load can be reduced.

[0047] Note that the position of the obstacle is x o ,y o may be included in the mathematical model, and the obstacle position x o ,y o may be defined as a dynamical system.

[0048] Putting all of this together, the optimization problem is set as shown in Equation (4) below.

[0049]

number

[0050] Here, J(x,u) is the evaluation function. The evaluation function can be designed according to the desired evaluation value. For example, the time integral value of the distance to the target point or the time integral value of the magnitude of the input can be used.

[0051] Equation (4) expresses that the optimization is performed so that the time integral of the distance to the target point and the time integral of the magnitude of the input are minimized. Note that the evaluation function is a term used in optimization theory in the field of mathematics.

[0052] In this embodiment, formulation has been described for a mobile object that can move in all directions, but the present invention is not limited to this as long as the object can express the mathematical model f of a dynamic system and the state constraint h(x). For example, a differential two-wheel model or a four-wheel vehicle model may be used.

[0053] <Motion plan generation using sampling method> In this embodiment, a particle filter is used as a sampling method. A particle filter is a method for predicting time series data using a probability density distribution. By executing a state estimation calculation using this particle filter, the optimization problem of Equation (4) is solved sequentially.

[0054] A particle filter as a state estimation calculation approximates the probability density distribution of a state by using a plurality of particles. For example, if there are many particles indicating a certain state quantity, the probability density of that state is high. In this case, a large number of particles are required to guarantee the approximation accuracy of the probability density distribution. That is, in the method that is the subject of the present disclosure, a decrease in effective particles causes a degradation of the method's performance. This degradation occurs when a particle calculated in the algorithm violates a constraint and is deleted. The technology according to the present disclosure maintains the number of effective particles and suppresses the degradation of the performance by taking into consideration in advance the constraints that cause this degradation.

[0055] <Particle filter calculation flow> FIG. 6 is a flowchart showing the particle filter calculation process executed by the desired trajectory generating unit 321 in the motion planning apparatus 300 of this embodiment.

[0056] When the calculation process starts, the target trajectory generating unit 321 first obtains information on state constraints to be considered in advance (step S101). A probability density distribution is approximated so that the state constraints are satisfied for each particle.

[0057] The target trajectory generating unit 321 is p N particles are initialized (step S102). p is an integer equal to or greater than 2. In this case, N p Each of the particles can have a different state quantity. They can also be initialized based on the current state quantity of the moving object 1. Here, the initialization of the particles means that p This is a process for preparing particles on software, and is a necessary advance preparation for executing the processes from step S102 onwards.

[0058] In this embodiment, the state quantity P of a particle is defined by the formula (1). In addition, the state quantity of the n-th particle is defined as P n It is written as follows.

[0059] In this embodiment, the initial values ​​of the variables are the same for all particles, and x r =0,y r =0, v x =0, v y = 0. A weight w is defined for each particle, and the initial value is the same for all particles and set as shown in the following formula (5). Time t is also defined, and the initial value is set to 0.

[0060]

number

[0061] Next, the target trajectory generating unit 321 extracts an input sampling range based on a state constraint h(x)≧0 that should be considered in advance (step S103).

[0062] The extraction method will be described below. First, the time derivative of the function h(x) representing the state constraint is calculated. In this embodiment, this is specifically calculated using the following formula (6).

[0063]

number

[0064] Here, the state quantity x and the input u are defined by the formula (1), and the formula model f is expressed by the formula (2).

[0065] Then, particles are generated within the input sampling range that satisfies the inequality expressed by the following formula (7).

[0066]

number

[0067] In this embodiment, the inequality is specifically expressed by the following formula (8).

[0068]

number

[0069] Here, α(h) is called the extended class κ function, which is monotonically increasing and α(0) = 0. For example, α(h) = α · h is used. · " is a positive constant.

[0070] When the state constraint h(x) ≧ 0 is satisfied at a certain time t ≧ 0, the state quantity x that evolves over time by the input sampling that satisfies the inequality in Equation (8) and the mathematical model f continues to satisfy h(x) ≧ 0 even after time t. The theoretical proof is disclosed in Non-Patent Document 1. Non-Patent Document 1 discloses a method for calculating a speed range in which a moving object does not collide with an obstacle, and more specifically, this is disclosed in Sections II.B and III.B of Non-Patent Document 1.

[0071] In this embodiment, the formulas (6) and (8) are expressed in continuous time, but they can also be expressed in discrete time by using a defined discrete time width Δt.

[0072] As described above, in the input sampling range in which the state constraint h(x) ≧ 0 is considered in advance under the condition of formula (8), the state quantity x after a discrete time span of Δt seconds is calculated by the above-mentioned system formula model f. + This makes it possible to predict the state of particles while taking into account constraints.

[0073] The state quantity P of the particle generated in the input sampling range that satisfies formula (8) n satisfies the state constraint h(x) ≥ 0. Therefore, all generated particles are valid, and the approximation accuracy of the probability density distribution can be guaranteed. Here, the approximation accuracy of the probability density distribution is defined as the number of valid particles. p The accuracy of the approximation of the probability density distribution decreases by the number of particles that is reduced, but since all generated particles are valid, the accuracy of the approximation can be guaranteed. Since the accuracy of the approximation of the probability density distribution is guaranteed, post-processing is not required, which is efficient.

[0074] Next, the target trajectory generating unit 321 predicts the state after a discrete time width Δt seconds based on the state constraint (step S104). In this embodiment, the particle state prediction uses the mathematical model of Equation (2). Here, particles are generated using random numbers in the input sampling range extracted in step S103, so that particles can be generated based on the state constraint h(x)≧0.

[0075] The particle state quantity P is the predicted state quantity x + and is updated using the input u, which is the input sampling value, and expressed as the following equation (9).

[0076]

number

[0077] Here, the particle state quantity x and the predicted state quantity x + ,The input u, which is the input sampling value, is a column vector, and the transpose is used for simplification.

[0078] Let P be the particle state quantity P. p After performing calculations y times, the observation value of each particle is obtained (step S105). The observation variables are designed based on the goal of the motion plan. The goal of the motion plan is determined by the surrounding environment of the moving object 1 or a user setting. In this embodiment, the goals are to maintain an ideal movement path, to maintain a movement speed, and to maintain a distance from an obstacle. Based on these goals, the lateral deviation y d The observation variables φ for the vehicle speed v and the distance d from the obstacle are expressed by the following equation (10).

[0079]

number

[0080] Here, e represents the natural logarithm. Since each value can be expressed using the particle's state quantity x, the observation variable can also be considered as a function φ(x) of the state quantity x. In the following discussion, for simplicity, it will be written as φ. The observation variable is the lateral deviation y d , the moving speed v, and the distance d to the obstacle.

[0081] Next, the observed value φ of each particle and the ideal observed value φ i The weight w of each particle is updated based on the difference between the ideal observation value φ iis an observation value for the moving body 1 in a virtually designed ideal state, and is determined from the target of the driving plan. Therefore, when the moving body 1 satisfies the target of the operation plan, the moving body 1 is in the ideal state. In this embodiment, the ideal observation value φ i is expressed by the following formula (11).

[0082]

number

[0083] Here, each value corresponds to the observation variable φ. Therefore, in this embodiment, the lateral deviation is 0, and the ideal moving speed is v i The ideal state is to keep the distance d from the obstacle large, that is, to make d approach infinity (∞) so that the value represented by the natural logarithm e in equation (10) approaches 0.

[0084] Based on the theory of particle filters, the weight w of each particle is updated. The update is performed by using the weight w of the nth particle as shown in the following formula (12). n and the likelihood γ, and the accumulated weights of all particles are set to 1.

[0085]

number

[0086] Here, the likelihood γ of each particle is calculated using the following formula (13) using a covariance matrix Q related to the state quantity x of the particle and a covariance matrix R related to the observation value φ, which are set in advance.

[0087]

number

[0088] Here, the det operator represents calculation of the determinant of a square matrix, and the matrix S is expressed by the following formula (14).

[0089]

number

[0090] Here, the matrix H is a differential coefficient obtained by differentiating the observation variable φ with respect to the state quantity x when the state quantity x has a certain value (x), and is defined by the following formula (15).

[0091]

number

[0092] Next, the target trajectory generating unit 321 resamples the particles based on the weight w of each particle (step S106). However, in this embodiment, in order to prevent a large variation in the particles, the virtual effective particle number N eff is the threshold N th Resampling is performed only if the following is true; otherwise, nothing is done in this step. Here, the virtual effective particle number N eff is calculated using the following formula (16). Note that resampling can also be performed every time.

[0093]

number

[0094] Here, the term "virtual effective particle number" is used because the number of particles is virtually calculated using the weight of each particle. When the weights of each particle are equal, the calculation result of equation (16) coincides with the number of particles.

[0095] The resampling method is to sample at equal intervals from the empirical distribution function, as in the case of a normal particle filter. When resampling is performed, the weights of each particle are initialized as equal based on equation (5).

[0096] Next, the target trajectory generating unit 321 calculates a weighted average value based on the weight w for the state quantity P of the particle obtained by the above-mentioned processing, stores the state quantity x and the input u in the target trajectory generating unit 321 as an operation plan (step S107), and updates the time as t+Δt.

[0097] Next, the target trajectory generating unit 321 calculates a planning horizon value τ h It is determined whether t<τ h If t≧τ, the process from step S103 onward is repeated. h If this is the case (if Yes), the data of the state quantity x and the input u stored as the motion plan are output as the target trajectory and the target input data, and the calculation for generating the motion plan is terminated.

[0098] FIG. 7 is a diagram showing the result of generating a motion plan by the above-mentioned sampling method. For simplicity, the number of particles is set to N p = 10, but in reality, sampling is performed with a target of 10 times the number of states to be estimated.

[0099] In Fig. 7, the initial values ​​of all particles to be estimated refer to node 330, and in this embodiment, calculation is started with the same value as node 330. The state transitions of these particles are predicted through the process described using formulas (1) to (8), and the state quantities of the particles are updated using formula (9) to obtain updated values ​​of the particles. These have a variation as shown as particle 331 when node 330 in Fig. 7 is set as the initial value, for example, so as to satisfy the state constraint h(x) ≧ 0 expressed in formula (3) while following formula (2) of the mathematical model.

[0100] For each updated particle 331, through the process described using formulas (10) to (16), weights are calculated according to the relationship between the target trajectory and the target input and the surrounding environment, and resampling according to the weights is performed, resulting in, for example, a new node 332. Here, information on the ideal path 400 and the obstacle 600 (or person 500) is acquired to calculate the observed value of formula (10).

[0101] <Effects> According to the configuration of the motion plan generating unit 320 described above, when a motion plan based on a sampling method represented by a particle filter is applied to a dynamic system, particles are generated within an input sampling range that takes into consideration in advance the state constraint h(x)≧0, thereby making it possible to implement an efficient motion plan that can guarantee the accuracy of the probability density distribution.

[0102] If the state constraint h(x) ≧ 0 is not considered in advance, particles determined to collide with an obstacle are subjected to post-processing to maintain the safety of the system or the consistency of the simulation. For example, if it is set that particles determined to collide are deleted, the accuracy of the probability density distribution cannot be guaranteed. For example, when adjusting the weights as post-processing, it is necessary to maintain consistency with the mathematical model f of the dynamic system, and the calculation load for the adjustment may occur.

[0103] In this embodiment, a particle filter has been described as an example of a sampling method, but the sampling method may be a different method having a technical background of, for example, the Monte Carlo method. A feature of the present disclosure is that the input sampling range is limited in advance based on a state constraint expressed by a mathematical model, and can be introduced regardless of the form of the sampling method itself.

[0104] In addition, as shown in formula (3), the state constraint is limited to a constraint that considers avoiding collisions with obstacles, but is not limited to this. Examples of state constraints include constraints that consider lane departure, constraints that consider entry into a dangerous area, constraints that consider tipping over due to sudden steering, constraints that consider the range of motion in the case of a robot arm, and constraints that consider singular postures.

[0105] <Embodiment 2> Next, a description will be given of the motion planning apparatus 300 according to a second embodiment of the present disclosure. Note that the configuration of the motion planning apparatus 300 according to the second embodiment is the same as the configuration of the motion planning apparatus 300 according to the first embodiment shown in FIG.

[0106] In the relationship between the mathematical model (mathematical formula (1) and mathematical formula (2)) and the state constraint (mathematical formula (3)) in the first embodiment described above, the input term appears in the first-order time derivative of the function h(x) representing the state constraint, but second- or higher-order time derivatives can also be used.

[0107] For example, the state quantity x of the moving body 1 is redefined as the position and velocity in the XY coordinate system, and the input u is redefined as the acceleration in the XY coordinate system, as shown in the following equation (17).

[0108]

number

[0109] The mathematical model f of the moving object 1 is also redefined as shown in the following mathematical expression (18).

[0110]

number

[0111] The state constraint h(x)≧0 is to prevent collision with an obstacle, for example, as in the first embodiment, and is expressed as in formula (3).

[0112] Here, when the time derivatives of the first and second derivatives of equation (3) are calculated based on the redefined mathematical model f of the moving body, the calculations are given by the following equations (19) and (20), respectively.

[0113]

number

[0114]

number

[0115]

number

[0116] In order to limit the input sampling range using this vector-valued function η(x) so as to take into account in advance the state constraint h(x)≧0, the condition of equation (7) is expanded to the condition of equation (22) below.

[0117]

number

[0118] Here, K b =[k b1 ,k b2 ] represents a vector whose elements are all positive.

[0119] By generating particles based on random numbers within the input sampling range expressed by Equation (22), the state quantity x of the generated particles continues to satisfy the state constraint h(x). That is, as in the result of the first embodiment, no invalid particles are generated, and sampling can be performed efficiently. A mathematical proof of the satisfaction of the state constraint h(x) is disclosed in Non-Patent Document 2. Non-Patent Document 2 discloses a method of calculating an acceleration range in which a moving object does not collide with an obstacle, and more specifically, this is disclosed in Section III of Non-Patent Document 2.

[0120] <Embodiment 3> Next, a description will be given of the motion planning apparatus 300 according to a third embodiment of the present disclosure. Note that the configuration of the motion planning apparatus 300 according to the second embodiment is the same as the configuration of the motion planning apparatus 300 according to the first embodiment shown in FIG.

[0121] In the above-described first embodiment, the input sampling range is restricted by obtaining an input value based on a random number from the input sampling range restricted by equation (7). Regarding this restriction method, it is also possible to first obtain an input value based on a random number in an unrestricted input sampling range, and then correct the input value so as to satisfy the constraint condition of equation (7). As a correction method, a correction method based on an optimization problem in which the input correction amount is an evaluation function can be used.

[0122] 8 is a diagram showing a schematic diagram of a method for correcting an input value in the third embodiment. Here, the case of the first embodiment is shown as an example.

[0123] In FIG. 8, when an input value is obtained based on a random number in an original input sampling range 804, which is an unrestricted input sampling range, the input values ​​can be divided into, for example, an input value 801 that satisfies formula (7) and an input value 802 that does not satisfy formula (7), i.e., violates formula (7), based on a boundary 806 where the positive and negative signs of the left side of formula (7) change.

[0124] For example, a correction amount 805 is introduced for each of the violating input values ​​802 to obtain corrected input values ​​803 that satisfy the original input sampling range 804 and the input sampling range restricted by formula (7). Then, the processing from step S104 onward shown in FIG. 6 is executed using the input values ​​801 and the corrected input values ​​803.

[0125] In this embodiment, by limiting the input sampling range, for example, if it is difficult to obtain an input value based on a random number from the range limited by formula (7), the input value can be obtained based on a random number using a different optimization problem and then corrected.

[0126] In addition, by using the amount of input correction for restricting the input value obtained based on the random number so that it satisfies equation (7) as an evaluation function, the designer can design the input sampling range to the range desired by the designer, and the amount of correction of the input value can be directly evaluated.

[0127] When the input correction amount is used as the evaluation function, for example, the evaluation function can be set as the square of the input correction amount. In this case, if, for example, formula (7) is linear with respect to the input, an analytical solution of the quadratic programming problem can be applied, improving the calculation efficiency.

[0128] <Fourth embodiment> Next, a description will be given of the motion planning apparatus 300 according to a fourth embodiment of the present disclosure. Note that the configuration of the motion planning apparatus 300 according to the fourth embodiment is the same as the configuration of the motion planning apparatus 300 according to the first embodiment shown in FIG.

[0129] The input sampling range restriction described in embodiment 1 or embodiment 3 requires that all input values ​​obtained based on random numbers satisfy equation (7). However, in embodiment 4, by adjusting the input sampling range in advance based on information about the state constraint h(x), it is also acceptable for some or all of the input values ​​obtained based on random numbers to not satisfy equation (7).

[0130] In other words, the input sampling range is approximately restricted by the probability density distribution function and adjusted so that the number of input values ​​that satisfy formula (7) is equal to or greater than a certain number. The calculation load can be reduced by applying the probability density distribution information to the state estimation calculation of the particle filter.

[0131] For example, when the input sampling range is to be limited symmetrically, a Gaussian distribution is used, and the mean and standard deviation that characterize the Gaussian distribution are used as parameters for adjusting the input sampling range. Alternatively, the mean and / or one of the mean and standard deviation that characterize the Gaussian distribution based on an optimization problem are used as parameters for adjusting the input sampling range. Note that when it is desired to limit the input sampling range in a specific direction, a gamma distribution is used.

[0132] The computational load can be reduced by applying Gaussian distribution information to the state estimation calculation of the particle filter. In addition, by specifying the input sampling range with a Gaussian distribution, the parameters for adjusting the range can be narrowed down to the mean value and standard deviation, which can also reduce the computational load.

[0133] Moreover, the adjustment of the range by the Gaussian distribution can be adjusted mechanically by adjusting it based on an optimization problem.

[0134] In the following description, the input sampling range is limited to a Gaussian distribution, and an example of the limiting method is shown diagrammatically in Fig. 9. Here, the case of the first embodiment is shown as an example.

[0135] 9, the original input sampling range, which is the adjusted input sampling range, is defined as a Gaussian distribution and characterized by a mean value 811 and a standard deviation 812. In this case, if an input value is obtained based on a random number, many values ​​that violate formula (7) are extracted. For example, if the mean value 811 violates formula (7), the probability that the input value obtained based on the random number satisfies formula (7) is less than 0.5.

[0136] Here, based on a boundary 806 where the sign of the left side of the formula (7) changes, for example, the average value 811 is corrected by a correction amount 813 to obtain a corrected average value 815, and the standard deviation 812 is corrected by a correction amount 814 to obtain a corrected standard deviation 816. As a correction method, a correction method based on an optimization problem in which the input correction amount is an evaluation function can be used, as in the third embodiment.

[0137] Compared with input values ​​acquired from the original input sampling range, input values ​​acquired based on random numbers from a Gaussian distribution characterized by these corrected mean value 815 and corrected standard deviation 816 have more extracted values ​​that satisfy formula (7). Using these input sampling values, the processes from step S104 onward in Fig. 6 are executed. For example, if the mean value 811 is corrected by the correction amount 813 to obtain corrected mean value 815, the probability that the input value acquired based on random numbers satisfies formula (7) can be adjusted to 0.5 or more.

[0138] By approximating the input sampling range with a preset probability density distribution function, it is only necessary to modify the parameters that characterize the probability density distribution, which simplifies the calculation and reduces the calculation load.

[0139] <Hardware configuration> Each of the components of the motion planning apparatus 300 according to the first to fourth embodiments described above can be configured using a computer, and is realized by the computer executing a program. That is, for example, it is realized by a processing circuit 1000 shown in FIG. 10. A processor such as a CPU (Central Processing Unit) or a DSP (Digital Signal Processor) is applied to the processing circuit 1000, and the function of each part is realized by executing a program stored in a storage device. It is noted that the target trajectory storage unit 322 is realized by a storage device included in the computer.

[0140] Dedicated hardware may be applied to the processing circuit 1000. When the processing circuit 1000 is dedicated hardware, the processing circuit 1000 corresponds to, for example, a single circuit, a composite circuit, a programmed processor, a parallel programmed processor, an ASIC (Application Specific Integrated Circuit), an FPGA (Field-Programmable Gate Array), or a combination of these.

[0141] In the motion planning device 300, the functions of each of the components can be realized by individual processing circuits, or these functions can be realized collectively by one processing circuit.

[0142] 11 shows a hardware configuration in the case where the processing circuit 1000 is configured using a processor. In this case, the functions of each part of the motion planning device 300 are realized by a combination of software, etc. (software, firmware, or software and firmware). The software, etc. are described as a program and stored in the memory 1002. The processor 1001 functioning as the processing circuit 1000 realizes the functions of each part by reading and executing the program stored in the memory 1002 (storage device). In other words, it can be said that this program causes a computer to execute the procedure and method of the operation of the components of the motion planning device 300.

[0143] Here, the memory 1002 may be, for example, a non-volatile or volatile semiconductor memory such as RAM, ROM, flash memory, EPROM (Erasable Programmable Read Only Memory), EEPROM (Electrically Erasable Programmable Read Only Memory), HDD (Hard Disk Drive), magnetic disk, flexible disk, optical disk, compact disk, mini disk, DVD (Digital Versatile Disc) and its drive device, or any storage medium to be used in the future.

[0144] The above describes a configuration in which the functions of the components of the motion planning device 300 are realized by either hardware or software, etc. However, the present invention is not limited to this, and some components of the motion planning device 300 can be realized by dedicated hardware, and other components can be realized by software, etc. For example, it is possible to realize the functions of some components by the processing circuit 1000 as dedicated hardware, and realize the functions of other components by the processing circuit 1000 as the processor 1001 reading and executing a program stored in the memory 1002.

[0145] As described above, the motion planning apparatus 300 can realize the above-mentioned functions by hardware, software, or a combination of these.

[0146] Although the present disclosure has been described in detail, the above description is illustrative in all respects and does not limit the present disclosure. It is understood that countless variations not illustrated can be assumed without departing from the scope of the present disclosure.

[0147] In addition, within the scope of the present disclosure, it is possible to freely combine the respective embodiments, and to appropriately modify or omit the respective embodiments.

Claims

1. An operation planning device for a dynamic system by a sampling method, comprising: a behavior planning unit that outputs a mathematical model representing the motion of the dynamic system based on a target to be achieved by the dynamic system and state constraints to be considered in advance for the dynamic system; an operation plan generation unit that restricts an input sampling range based on the mathematical model and the state constraints, and generates an operation plan for the dynamic system by state estimation calculation within the restricted input sampling range.

2. The operation plan generation unit acquires an input value based on a random number within the unrestricted input sampling range, and then corrects the input value so as to satisfy the state constraint. The operation planning device according to claim 1.

3. The operation plan generation unit adjusts the input sampling range so that the number of input values satisfying the input sampling range is equal to or more than a certain number. The operation planning device according to claim 1.

4. The operation plan generation unit corrects the input value based on an optimization problem. The operation planning device according to claim 2 or claim 3.

5. The operation plan generation unit uses the correction amount when correcting the input value as an evaluation function of the optimization problem. The operation planning device according to claim 4.

6. The mathematical model is linear with respect to the input. The operation planning device according to claim 1.

7. The state constraint is a constraint that becomes a scalar value with respect to input sampling by a first-order derivative. The operation planning device according to claim 1.

8. The operation plan generation unit defines the input sampling range by a probability density distribution so that the input value satisfying the input sampling range is equal to or more than the certain number. The operation planning device according to claim 3.

9. The operation plan generation unit sets the probability density distribution as a Gaussian distribution, and uses the mean value and the standard deviation characterizing the Gaussian distribution as parameters for adjusting the input sampling range. The operation planning device according to claim 8.

10. The operation plan generation unit sets the probability density distribution as a Gaussian distribution, and uses both or one of the mean value and the standard deviation characterizing the Gaussian distribution as parameters for adjusting the input sampling range based on an optimization problem. The operation planning device according to claim 8.

11. The sampling method is a particle filter that approximates the probability density distribution of a state by a plurality of particles. The operation planning device according to claim 1.

12. The dynamic system according to any one of claims 1 to 3, wherein the dynamic system is a moving body.

Images