JP7475562B1 - Motion planning device - Google Patents

Abstract

The present disclosure relates to an operation planning device for a dynamic system using a sampling method, the device 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 operation plan generation unit that limits an input sampling range based on the mathematical model and the state constraints, and generates an operation plan for the dynamic system by performing a state estimation calculation within the input sampling range based on the mathematical model.

Description

The present disclosure relates to a motion planning device, and in particular to a motion planning device for dynamic systems using a sampling technique.

Several methods have been proposed for the motion planning problem 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 deal with complex constraints, as it creates a motion plan after determining for each particle whether it is a valid particle that satisfies the constraints (valid particles) or an invalid particle that does not (invalid particles).

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

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 road surface slippage.

Patent No. 6494872 Patent No. 6594589

A. D. Ames, X. Xu, J. W. 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. Q. Nguyen and K. Sreenath. “Exponential control barrier functions for enforcing high relative-degree safety-critical constraints.” 2016 American Control Conference. 2016.

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

In addition, the method of Patent Document 2 limits the input sampling in advance, making it easy to ensure a valid number of particles, but since it is limited to road surface slippage, 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.

The present disclosure has been made to solve the problems described above, and aims to provide an action planning device using a sampling method that is unlikely to cause a degradation in the functionality of the method due to a decrease in valid particles, and that does not limit the state constraints that must be considered in advance in the system to which it is applied.

The motion planning device according to the present disclosure is a motion planning device for a dynamic system using a sampling method, and includes: 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 a motion plan generation unit that limits an input sampling range in advance 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 limited input sampling range.

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 valid 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.

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. 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. FIG. 1 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. 1 is a block diagram showing a configuration of a motion planning device according to a first embodiment of the present disclosure. FIG. 2 is a diagram illustrating a coordinate system used in the first embodiment according to the present disclosure. 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. 1 is a diagram illustrating a schematic diagram of a result of generating a motion plan in the motion planning device according to the first embodiment of the present disclosure. FIG. FIG. 13 is a diagram illustrating a method of correcting input sampling values in a motion planning apparatus according to a third embodiment of the present disclosure. 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. FIG. 2 is a diagram illustrating a hardware configuration for implementing the motion planning device according to the first to fourth embodiments. FIG. 2 is a diagram illustrating a hardware configuration for implementing the motion planning device according to the first to fourth embodiments.

<First embodiment>
<System configuration of a moving object>
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, actuators 102, and a battery 103 as driving devices. The actuators 102 convert power obtained from the battery 103 into driving force and input the power to the wheels 101. The actuators 102 are controlled by a control device 10. The control device 10 includes a storage unit and a calculation unit, and controls the actuators 102 according to a set control program to control the movement of the moving body 1.

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 the motion planning device 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 uses the difference in rotation speed between the wheels to move straight and turn.

In addition, the dynamic systems to which this disclosure applies are not limited to moving bodies, but can be applied to any dynamic system in which the movement of a robot arm or overhead crane, for example, is expressed as a mathematical model (differential equation).

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

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

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 body 1 based on the amount of rotation of the wheel 101. The wheel angle sensor 111 is, for example, configured by a rotary encoder.

The optical range sensor 112 is, for example, a LiDAR (Light Detection and Ranging) sensor, and is provided on the top surface of the moving body 1. The optical range sensor 112 measures physical shape data of the space in the surrounding environment of the moving body 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 body 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 body 1 by referring to the map information.

Figure 3 is a diagram showing a schematic example of a situation in which the moving body 1 moves. In Figure 3, the physical shape data of the space obtained by the optical range sensor 112 includes the shapes of a person 500 and an object 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 explanation, all objects that impede the movement of the moving body 1, such as a person, a wall, or another moving body, are referred to as obstacles.

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

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 mobile body 1 moves is not particularly limited.

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

As shown in Figure 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.

The motion planning device 300 shown in Figure 4 is included in the control device 10 and has an action planning unit 310 and a motion plan generating unit 320. The control device 10 is a device that controls the moving body 1 according to a target trajectory, and is installed, for example, as an embedded computer.

The action planning unit 310 has a moving body state estimation unit 311, a moving target calculation unit 312, and a state constraint calculation unit 313.

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

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 calculated goal to the state constraint calculation unit 313. The goal is, for example, a reference trajectory, a target point, and information on an obstacle to be avoided.

The 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 present embodiment 1, 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.

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.

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. In addition, two camera image sensors can be used to obtain image data of each, and the relative distance can be calculated using the parallax in the image data of each.

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 when no people or robots are detected, a process is performed that constantly predicts, using a mathematical formula, whether a person or robot will suddenly jump out. This is a process that uses simulation to place virtual walls that indicate undetected people or areas where moving objects are prohibited from entering, or to predict and calculate the probabilistic appearance of obstacles from outside the sensor range.

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 obstacles in advance, based on relative position information between the moving body 1 and obstacles, such as 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.

The target trajectory memory unit 322 stores the target trajectory obtained from the target trajectory generation unit 321, selects the necessary target trajectory information, and outputs it as an operation plan to the driving device 100. The driving device 100 operates the moving body 1 according to the received operation plan.

<Moving object coordinate system>
FIG. 5 is a diagram showing a schematic diagram of a coordinate system used in the first embodiment. The X-axis and Y-axis in FIG. 5 are inertial coordinate systems, and _xr and _yr represent the center of gravity of the moving body 1 in the inertial coordinate system. However, _xr and _yr are not limited to the center of gravity as long as they can represent the position of the moving body 1, and can be, for example, the shape center point, the depth camera installation point, and the range sensor installation point. _vx and _vy are the X-direction and Y-direction velocities of the moving body 1 in the inertial coordinate system. In addition, _xo and _yo are the representative point positions of the obstacle 600, and are 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 that is the shortest distance between the moving body 1 and the obstacle 600, the shape center of the obstacle 600, and the center of gravity of the obstacle 600.

<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 an optimization problem designed in advance after taking into account state constraints. 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 the specific formulation described later.

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

Here, the coordinate system is that shown in FIG. 4, _{v_x} is the velocity in the X direction, and _{v_y} is the velocity in the Y direction.

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 computational load.

Note that formulas (1) and (2) are examples of formula models, so the state quantity x, input u, and formula 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 path coordinate system.

In this embodiment, the state constraint to be considered in advance is the state constraint h(x)≧0, which indicates that the moving body 1 does not collide with an obstacle, and is expressed as the following equation (3).

Here, r _m is a value that indicates a certain distance from an obstacle. This means that while the state constraint h(x) ≧ 0 is satisfied, the robot moves while maintaining a distance of r _m or more from the obstacle. This state constraint h(x) ≧ 0 can also 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.

In addition, the state constraint h(x) is a scalar value, and since the state constraint h(x) is a scalar value, the extraction of the input sampling range can be simplified and the computational load can be reduced.

The positions x _o and y _o of the obstacle may be included in the mathematical model, and the positions x _o and y _o of the obstacle may be defined as a dynamic system.

To summarise all of the above, we set the optimization problem as shown in equation (4) below.

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 of the distance to the target point or the time integral of the magnitude of the input can be used.

Equation (4) expresses the optimization that minimizes the time integral of the distance to the target point and the time integral of the magnitude of the input. Note that the term "evaluation function" is used as a term in optimization theory in the field of mathematics.

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

<Motion plan generation using sampling method>
In this embodiment, a particle filter is used as a sampling method. The 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.

A particle filter as a state estimation calculation uses multiple particles to approximate the probability density distribution of a state; 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 ensure the accuracy of the approximation of the probability density distribution. In other words, in the method that is the subject of this disclosure, a decrease in effective particles causes a degradation of the method's performance. This degradation occurs when a particle calculated within the algorithm violates a constraint and is deleted. The technology disclosed herein maintains the number of effective particles and suppresses degradation of performance by taking into account in advance the constraints that cause this degradation.

<Particle filter calculation flow>
FIG. 6 is a flowchart showing the particle filter calculation process executed by the target trajectory generating unit 321 in the motion planning apparatus 300 according to this embodiment.

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

The target trajectory generating unit 321 initializes _Np particles (step S102). Here, _Np is an integer equal to or greater than 2. At this time, the _Np particles can each have a different state quantity. Alternatively, the particles can be initialized based on the current state quantity of the moving body 1. Here, the initialization of the particles is a process for preparing _Np particles on software, and is a preparatory step required for executing the processes in and after step S102.

In this embodiment, the state quantity P of a particle is defined by the following equation (1): The state quantity of the n-th particle is represented as _Pn .

In this embodiment, the initial values of variables are the same for all particles, i.e., _xr = 0, _yr = 0, _vx = 0, and _vy = 0. A weight w is defined for each particle, and the initial value is the same for all particles and is set as shown in the following formula (5). Time t is also defined, and the initial value is set to 0.

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

The extraction method is explained 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).

However, the state quantity x and input u are defined by equation (1), and the mathematical model f is expressed by equation (2).

Then, particles are generated in the input sampling range that satisfies the inequality expressed by the following equation (7).

In this embodiment, the inequality is specifically expressed by the following equation (8).

Here, α(h) is called an extended class κ function, which is monotonically increasing and α(0) = 0. For example, α(h) = α ^· h is used, where "α ^· " is a positive constant.

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 the speed range in which a moving object will not collide with an obstacle, and more specifically, this is disclosed in Sections II.B and III.B of Non-Patent Document 1.

In this embodiment, equations (6) and (8) are expressed in continuous time, but they can also be expressed in discrete time using the defined discrete time width Δt.

As described above, the state quantity x+ after a discrete time width Δt seconds is predicted by the above-mentioned mathematical model f of the system in the input sampling range in which the state constraint h(x)≧ ⁰ is considered in advance under the condition of mathematical formula (8). This makes it possible to predict the state of the particle taking the constraint into account.

The state quantity _Pn of particles generated in the input sampling range that satisfies the formula (8) 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. Although the approximation accuracy of the probability density distribution decreases by the number of valid particles that is reduced from the initially prepared _Np , since all generated particles are valid, the approximation accuracy can be guaranteed. Since the approximation accuracy of the probability density distribution is guaranteed, post-processing is not required, and efficiency is high.

Next, the target trajectory generation 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 formula (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.

The state quantity P of the particle is updated using the predicted state quantity x ⁺ and the input u which is the input sampling value, and is expressed as the following formula (9).

Here, the particle state quantity x, the predicted state quantity x ⁺ , and the input u which is the input sampling value are all column vectors, and transposition is used for simplification.

After calculating the state quantity P of the particle _Np 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 body 1 or the user's 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, φ is expressed as the following equation (10) as an observation variable for the lateral deviation y _d from the ideal movement path, the movement speed v, and the distance d from an obstacle.

Here, e represents the natural logarithm. Since each value can be expressed using the state quantity x of the particle, the observation variable can also be considered as a function φ(x) of the state quantity x. In the following discussion, it is written as φ for simplicity of notation. The observation variables include at least one of the lateral deviation y _d from the ideal movement path, the movement speed v, and the distance d from the obstacle.

Next, the weight w of each particle is updated based on the difference between the observed value φ of each particle and the ideal observed value φ _i . Here, the ideal observed value φ _i is an observed value for the moving body 1 in a virtually designed ideal state, and is determined based on the target of the travel 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 observed value φ _i is expressed by the following formula (11).

Here, each value corresponds to the observation variable φ. Therefore, in this embodiment, the lateral deviation is 0, the ideal moving speed is v _i , and the distance d from the obstacle is kept large, that is, d approaches infinity (∞) so that the value expressed by the natural logarithm e in formula (10) approaches 0, which is the ideal state.

The weight w of each particle is updated based on the theory of particle filters. The weight w is proportional to the weight wn of the _n -th particle and the likelihood γ, as shown in the following formula (12), so that the integrated value of the weights of all particles becomes 1.

Here, the likelihood γ of each particle is calculated using the covariance matrix Q for the particle's state quantity x and the covariance matrix R for the observation value φ, which have been set in advance, using the following formula (13).

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

where matrix H is the differential coefficient obtained by differentiating the observation variable φ with respect to the state quantity x when the state quantity x has a certain value (bar x), and is defined by the following equation (15).

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, resampling is performed only when the virtual effective particle number _Neff is equal to or less than the threshold value _Nth , and otherwise, nothing is performed in this step. Here, the virtual effective particle number _Neff is calculated using the following formula (16). Note that resampling can also be performed every time.

Here, the term "virtual effective particle number" is used because the number of particles is calculated virtually using the weight of each particle, and when the weights of each particle are equal, the calculation result of formula (16) will match the number of particles.

The resampling method is the same as in normal particle filters, where we sample at equal intervals from the empirical distribution function. When resampling is performed, the weights of each particle are initialized as equal based on formula (5).

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 process, 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.

Next, the target trajectory generating unit 321 judges whether the updated time t has reached the planning horizon value _τh , which is the planning target period of the motion plan (step S108). If t< _τh (No), the process repeats the process from step S103 onward. On the other hand, if t≧ _τh (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.

7 is a diagram showing a schematic diagram of the result of generating an operation plan by the above-mentioned sampling method. For simplicity, the number of particles is set to N _p =10 here, but in practice, sampling is performed with a rough guideline of 10 times the number of states to be estimated.

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

For each updated particle 331, the weights are calculated according to the relationship between the target trajectory and the target input and the surrounding environment through the process described using equations (10) to (16), 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 equation (10).

＜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, so that an efficient motion plan that can guarantee the accuracy of the probability density distribution can be implemented.

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 ensure 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 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.

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

As shown in formula (3), the state constraints are limited to those that take into account collision avoidance with obstacles, but are not limited to this. State constraints include constraints that take into account lane departure, constraints that take into account entry into dangerous areas, constraints that take into account tipping over due to sudden steering, constraints that take into account the range of motion in the case of a robot arm, and constraints that take into account singular postures.

<Embodiment 2>
Next, a description will be given of a 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.

In the relationship between the mathematical model (formula (1) and formula (2)) and the state constraint (formula (3)) of embodiment 1 described above, the input term appears as the first derivative with respect to time of the function h(x) representing the state constraint, but second or higher order time derivatives can also be performed.

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

The mathematical model f of moving body 1 is also redefined as follows: (18).

The state constraint h(x) ≧ 0 is intended to prevent collision with an obstacle, as in embodiment 1, and is expressed as equation (3).

Now, based on the redefined mathematical model f of the moving body, if we calculate the time derivatives of the first and second derivatives of equation (3), we get the following equations (19) and (20), respectively.

また、以下の数式（２１）のようにベクトル値関数η（ｘ）を定義する。

Also, a vector-valued function η(x) is defined as shown in the following equation (21).

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 in equation (7) is extended to the condition in equation (22) below.

Here, K _b =[k _b1 , k _b2 ] represents a vector in which all elements are positive.

By generating particles based on random numbers within the input sampling range expressed by formula (22), the state quantity x of the generated particles continues to satisfy the state constraint h(x). In other words, as with the results of embodiment 1, invalid particles are not 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 the acceleration range in which a moving body does not collide with an obstacle, and more specifically, this is disclosed in Section III of Non-Patent Document 2.

<Third embodiment>
Next, a description will be given of a 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.

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 formula (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 modify the input value so as to satisfy the constraint condition of formula (7). In addition, a modification method based on an optimization problem in which the input modification amount is an evaluation function can be used as a modification method.

Figure 8 is a diagram showing a schematic diagram of a method for correcting input values in embodiment 3. Here, embodiment 1 is shown as an example.

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

For example, a correction amount 805 is introduced for each of the violating input values 802, and the corrected input value 803 is obtained so as to 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 value 801 and the corrected input value 803.

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.

In addition, by using the amount of input correction for restricting the input value obtained based on the random number to satisfy equation (7) as an evaluation function, the designer can design the input sampling range to the designer's desired range and directly evaluate the amount of correction of the input value.

When the input correction amount is used as the evaluation function, the evaluation function can be set, for example, 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 to the quadratic programming problem can be applied, improving calculation efficiency.

<Fourth embodiment>
Next, a description will be given of a 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.

The input sampling range restriction described in embodiment 1 or embodiment 3 required that all input values obtained based on random numbers satisfy formula (7), but 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 formula (7).

In other words, the input sampling range is approximately restricted by the probability density distribution function and the range is adjusted so that the number of input values that satisfy formula (7) is equal to or greater than a certain number. By being able to apply information about the probability density distribution to the state estimation calculation of the particle filter, the computational load can be reduced.

For example, if you want to limit the input sampling range symmetrically, use a Gaussian distribution, and use the mean and standard deviation that characterize the Gaussian distribution as parameters to adjust the input sampling range. Alternatively, based on an optimization problem, use both the mean and/or standard deviation that characterize the Gaussian distribution as parameters to adjust the input sampling range. Note that if you want to limit the input sampling range in a specific direction, use a gamma distribution.

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 and standard deviation, thereby reducing the computational load.

In addition, the range adjustment using the Gaussian distribution can be adjusted mechanically by adjusting it based on an optimization problem.

In the following explanation, the input sampling range is limited to a Gaussian distribution, and an example of the limiting method is shown in FIG. 9. Here, the case of embodiment 1 is shown as an example.

In Figure 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, when 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 an input value obtained based on a random number satisfies formula (7) is less than 0.5.

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

Compared to input values obtained from the original input sampling range, input values obtained based on random numbers from a Gaussian distribution characterized by these modified mean value 815 and modified standard deviation 816 have more extracted values that satisfy formula (7). Using these input sampling values, the processing from step S104 onward in FIG. 6 is performed. For example, if the mean value 811 is modified by the modification amount 813 to obtain the modified mean value 815, the probability that the input value obtained based on random numbers satisfies formula (7) can be adjusted to 0.5 or more.

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. This simplifies the calculations and reduces the computational load.

<Hardware Configuration>
Each component 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.

Dedicated hardware may be applied to the processing circuit 1000. When the processing circuit 1000 is dedicated hardware, the processing circuit 1000 may be, 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.

The motion planning device 300 may have each of its component functions realized by separate processing circuits, or the functions may be realized together by a single processing circuit.

Figure 11 also shows the hardware configuration when 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 written 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 the computer to execute the procedure and method of operation of the components of the motion planning device 300.

Here, 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), etc., a HDD (Hard Disk Drive), a magnetic disk, a flexible disk, an optical disk, a compact disk, a mini disk, a DVD (Digital Versatile Disc) and its drive device, etc., or any storage medium to be used in the future.

A configuration has been described above in which the functions of each component of the motion planning device 300 are realized by either hardware or software, etc. However, this 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.

As described above, the motion planning device 300 can realize each of the above-mentioned functions through hardware, software, etc., or a combination of these.

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 envisioned without departing from the scope of the present disclosure.

In addition, within the scope of this disclosure, it is possible to freely combine the various embodiments, and to modify or omit each embodiment as appropriate.

Claims (12)

A motion planning device for a dynamic system using a sampling method, comprising:
a behavior planning unit that outputs a mathematical model that expresses a motion of the dynamic system based on a goal to be achieved by the dynamic system and a state constraint that should be considered in advance of the dynamic system;
a motion plan generation unit that limits an input sampling range in advance based on the mathematical model and the state constraint, and generates a motion plan for the dynamic system by a state estimation calculation in the limited input sampling range. The operation plan generating unit
The motion planning device according to claim 1 , wherein the input sampling range is restricted by obtaining an input value based on a random number in the unrestricted input sampling range and then correcting the input value so as to satisfy the state constraint. The operation plan generating unit
The motion planning device according to claim 1 , wherein the input sampling range is adjusted so that a certain number of input values satisfy the input sampling range. The operation plan generating unit
4. The motion planning device according to claim 2, wherein the input value is corrected based on an optimization problem. The operation plan generating unit
5. The motion planning apparatus according to claim 4, wherein a correction amount when correcting said input value is set as an evaluation function of said optimization problem. The motion planning device according to claim 1, wherein the mathematical model is linear with respect to the input. The motion planning device according to claim 1, wherein the state constraint is a scalar-valued constraint for input sampling by first-order differentiation. The operation plan generating unit
4. The motion planning apparatus according to claim 3, wherein the input sampling range is defined by a probability density distribution such that the input value that satisfies the input sampling range is equal to or greater than the certain number. The operation plan generating unit
9. The motion planning apparatus according to claim 8, wherein the probability density distribution is a Gaussian distribution, and a mean value and a standard deviation characterizing the Gaussian distribution are used as parameters for adjusting the input sampling range. The operation plan generating unit
9. The motion planning apparatus according to claim 8, wherein the probability density distribution is a Gaussian distribution, and both or one of a mean value and a standard deviation characterizing the Gaussian distribution based on an optimization problem is used as a parameter for adjusting the input sampling range. The sampling method includes:
2. The motion planning apparatus according to claim 1, which is a particle filter that approximates a probability density distribution of a state by a plurality of particles. The motion planning device according to any one of claims 1 to 3, wherein the dynamic system is a moving body.

Images