Motion planning device

Abstract

The present disclosure relates to an action planning device for a dynamic system using a sampling method, comprising: an action planning unit that outputs a mathematical expression model that expresses the movement of the dynamic system in accordance with a target achieved by the dynamic system, and a state restriction of the dynamic system that should be considered in advance; and an operation plan generation unit that restricts the input sampling range on the basis of the mathematical expression model and the state restriction, and generates an operation plan for the dynamic system on the basis of a state estimation calculation in the input sampling range on the basis of the mathematical expression model.

Description

Technical Field

[0001] This disclosure relates to motion planning devices, and more particularly to motion planning devices for dynamic systems utilizing sampling methods. Background Technology

[0002] Several methods have been proposed for the action planning problem of dynamic systems. As one of these methods, a sampling method has been developed. Action planning problems involve constraints that objects must satisfy. The sampling method formulates action plans based on identifying valid particles that satisfy the constraints and invalid particles that do not. Therefore, it has the advantage of easily handling complex constraints.

[0003] For example, in the vehicle control system of Patent Document 1, after sampling the control input, the time evolution of the physical model is calculated, and the effective particles of the state constraints are determined, thereby controlling the vehicle in a way that optimizes the probability of satisfying the desired motion by using only the effective particles.

[0004] Furthermore, in the autonomous driving system of Patent Document 2, by sampling the control input within the constraints of road surface slippage, the vehicle is controlled in a way that optimizes the probability of satisfying the desired motion by using only effective particles related to road surface slippage.

[0005] Existing technical documents

[0006] Patent documents

[0007] Patent Document 1: Japanese Patent No. 6494872

[0008] Patent Document 2: Japanese Patent No. 6594589

[0009] Non-patent literature

[0010] Non-patent literature 1: AD Ames

[0011] Non-patent literature 2: Q. Nguyen and K. Sreenath. "Exponential control barrierfunctions for enforcing high relative-degree safety-critical constraints." 2016 American Control Conference. 2016. Summary of the Invention

[0012] In sampling methods, a large amount of sampling calculation is required to ensure the approximate accuracy of the probability density distribution approximated by the sampling. When calculating the time evolution of the physical model and determining the effective particles that satisfy the constraints after input sampling, as shown in Patent Document 1, the approximate accuracy of the probability density distribution cannot be guaranteed when the number of effective particles is small. That is, the possibility of a decrease in the method's functionality due to the reduction in effective particles increases.

[0013] Furthermore, in the method of Patent Document 2, the input sampling is restricted in advance, so it is easy to ensure the effective number of particles. However, since it is limited to the slippage of the road surface, it is difficult to apply to other uses. Considering other state constraints, it will result in the same outcome as Patent Document 1.

[0014] This disclosure was made to solve the problems described above, and its purpose is to provide an action planning device using a sampling method that has a low probability of a decrease in the function of the method due to a reduction in effective particles, and is not limited to the state constraints that should be considered in advance in the system to which it is applied.

[0015] The action planning apparatus disclosed herein is an action planning apparatus for a dynamic system utilizing a sampling method, comprising: an action planning unit that outputs a mathematical model representing the motion of the dynamic system based on a goal to be achieved by the dynamic system and state constraints of the dynamic system to be considered in advance; and an action plan generation unit that, based on the mathematical model and the state constraints, limits the input sampling range and generates an action plan for the dynamic system by performing state inference calculations within the input sampling range based on the mathematical model.

[0016] According to the action planning device disclosed herein, input sampling is performed within the range of state constraints that should be considered in advance, thus improving the approximate accuracy of the probability density distribution approximated based on the sampling, reducing the possibility of a decrease in the function of the method due to a reduction in effective particles, and is not limited to state constraints that should be considered in advance in the system to which it is applied. Attached Figure Description

[0017] Figure 1This is a schematic diagram showing the structure of a mobile body equipped with the motion planning device according to Embodiment 1 of this disclosure.

[0018] Figure 2 This is a schematic diagram showing the structure of a mobile body equipped with the motion planning device according to Embodiment 1 of this disclosure.

[0019] Figure 3 This is a diagram schematically illustrating an example of a moving body equipped with the motion planning device of Embodiment 1 of the present disclosure moving.

[0020] Figure 4 This is a block diagram illustrating the structure of the action planning device according to Embodiment 1 of this disclosure.

[0021] Figure 5 This is a diagram schematically illustrating the coordinate system used in Embodiment 1 of this disclosure.

[0022] Figure 6 This is a flowchart illustrating the computational processing of the particle filter used in the action planning apparatus of Embodiment 1 of this disclosure.

[0023] Figure 7 This is a diagram schematically showing the result of motion plan generation in the motion planning device according to Embodiment 1 of this disclosure.

[0024] Figure 8 This is a diagram schematically illustrating the method for correcting input sample values ​​in the action planning apparatus according to Embodiment 3 of this disclosure.

[0025] Figure 9 This is a diagram schematically illustrating the method for limiting the input sampling range in the action planning apparatus according to Embodiment 4 of this disclosure.

[0026] Figure 10 This is a diagram showing the hardware structure of the action planning device for implementing embodiments 1 to 4.

[0027] Figure 11 This is a diagram showing the hardware structure of the action planning device for implementing embodiments 1 to 4. Detailed Implementation

[0028] <Implementation Method 1>

[0029] <System Structure of Moving Bodies>

[0030] Figure 1 as well as Figure 2 This is a schematic diagram showing the system structure of a mobile body 1, which is a dynamic system equipped with an action planning device according to Embodiment 1 of this disclosure. Figure 1This is a perspective view of the moving object 1 viewed from above. Figure 2 This is a side view of the moving object 1. Figure 1 As shown, the mobile body 1 includes wheels 101, actuators 102, and a battery 103 as a drive device. The actuators 102 convert the electricity received from the battery 103 into driving force and input it to the wheels 101. The actuators 102 are controlled by a control device 10. The control device 10 includes a storage unit and a processing unit, and controls the actuators 102 according to a set control program, thereby controlling the movement of the mobile body 1.

[0031] In this embodiment, an example is shown where a motion planning device is mounted on a mobile body 1 that uses three wheels and three actuators and can move in all directions. However, this is not a limitation; for example, it can also be mounted on a differential two-wheeled mobile body or a legged mobile robot. Here, a differential two-wheeled mobile body refers to a mobile body that has two independently rotating wheels and uses the difference in rotational speed of each wheel to move forward in a straight line and turn.

[0032] Furthermore, the dynamic systems to which this disclosure is applied are not limited to moving bodies; any dynamic system in which the motion is represented by a mathematical model (differential equation) can be applied, such as a robotic arm or a roof crane.

[0033] When the dynamic system is set as a moving body, it is possible to set complex target trajectories toward the target location.

[0034] Within the moving body 1, it serves as an observation device. Figure 1 The wheel angle sensor 111 shown is shown. Figure 2 The optical rangefinder 112 and depth camera 113 are shown. The observation device is connected to the control device 10.

[0035] Wheel angle sensors 111 are installed on each wheel 101 to detect the amount of rotation of the wheel 101. Based on the amount of rotation of the wheel 101, the control device 10 calculates the amount of movement of the moving body 1. The wheel angle sensors 111 are, for example, composed of a rotary encoder.

[0036] An optical rangefinder 112, such as a LiDAR (Light Detection and Ranging) sensor, is mounted on the upper surface of the moving body 1. The optical rangefinder 112 measures the physical shape data of the space surrounding the moving body 1 along the scanning plane. The control device 10 creates a map of the surrounding environment based on the measured shape data. By referring to the created map, the position of the moving body 1 on the map plane is inferred. Alternatively, a map can be created in advance, and the position of the moving body 1 can be inferred by referring to this map information.

[0037] Figure 3This is a diagram schematically illustrating an example of a moving body 1 in motion. Figure 3 In this process, the physical shape data of the space obtained by the optical field sensor 112 includes the shapes of people 500 and objects 600 that obstruct the movement of the moving body 1. The control device 10 identifies these shape data as objects that the moving body 1 should avoid. Furthermore, in the following description, all objects that hinder the movement of the moving body 1, such as people, walls, and other moving bodies, are referred to as obstacles.

[0038] The forward depth camera 113, together with the image, acquires the physical shape data of the space in front of the vehicle. The forward depth camera 113 acquires the measurement information within the camera's field of view, and the optical measurement sensor 112 supplements the shape data on the scanning plane (data obtained by cutting a plane in three-dimensional space).

[0039] However, the above-mentioned observation device is just one example, and there is no particular limitation on the method of obtaining the shape data of obstacles in the environment in which the moving body 1 is moving.

[0040] <Device Structure>

[0041] Figure 4 This is a block diagram showing the structure of the action planning device 300 according to Embodiment 1 of this disclosure, and a block diagram showing the structure of the drive device 100 and the observation device 200 connected to the action planning device 300.

[0042] like Figure 4 As shown, the drive unit 100 includes the previously described wheel 101, actuator 102, and battery 103. The observation unit 200 includes the previously described wheel angle sensor 111, optical range sensor 112, and depth camera 113.

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

[0044] The action planning unit 310 includes a moving body state prediction unit 311, a moving target calculation unit 312, and a state constraint calculation unit 313.

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

[0046] In the moving target calculation unit 312, based on the position of the moving body 1 output from the moving body state estimation unit 311 and obstacle information output from the observation device 200, the target that the moving body 1 should achieve is calculated and output to the state constraint calculation unit 313. This target may be, for example, information such as the reference track, target location, and obstacles to be avoided.

[0047] In the state constraint calculation unit 313 within the action planning unit 310, based on the goal for the moving body 1 to achieve, output from the moving target calculation unit 312, a mathematical model of the moving body 1 and state constraint information to be considered in advance are calculated and output. In this embodiment 1, the state constraint information to be considered in advance is the relative position information between the moving body 1 and the obstacle, and is output based on information including the position of the moving body 1, the position of the obstacle, and the shape of the obstacle.

[0048] By incorporating the state constraint calculation unit 313 into the action planning unit 310, making it independent of the action planning generation unit 320, constraints can be handled uniformly without relying on the mathematical model representing the action and state constraints of the dynamic system or the sampling method.

[0049] The relative position information output from the state constraint calculation unit 313 is input to the action 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, and can be set as a value calculated based on values ​​from one or more sensors. For example, the system controlling the object and the position of the object to be avoided can be obtained from GPS, and the relative distance can be calculated based on each position. In addition, two camera image sensors can be used to acquire each image data, and the relative distance can be calculated using the parallax in each image data.

[0050] Furthermore, it is also possible to set values ​​that are obtained by mathematically formulating phenomena without using sensors, and values ​​that are calculated solely by software. For example, a process can be performed that mathematically predicts the sudden appearance of a person or robot even when no person or robot is detected. This is a process that uses simulation to configure the sudden appearance of an undetected person, or virtual walls representing a prohibited area for a moving object, or predicts obstacles probabilistically appearing outside the sensor range.

[0051] The motion planning generation unit 320 includes a target trajectory generation unit 321 and a target trajectory storage unit 322. The target trajectory generation unit 321 generates a target trajectory for moving while achieving a target, based on the relative position information (e.g., relative distance) between the moving body 1 and obstacles output from the motion planning unit 310, taking into account obstacle avoidance. A sampling method is applied in the generation of this target trajectory. The generated target trajectory is output to the target trajectory storage unit 322.

[0052] The target track storage unit 322 stores the target track obtained from the target track generation unit 321, selects the necessary amount of target track information, and outputs it as an action plan to the drive device 100. The drive device 100 causes the moving body 1 to move according to the received action plan.

[0053] <Coordinate system of the moving object>

[0054] Figure 5 This is a diagram schematically showing the coordinate system used in Embodiment 1. Figure 5 The X and Y axes are inertial coordinate systems, x r and y r This represents the position of the center of gravity of the moving body 1 in the inertial coordinate system. However, x r and y r The location of the moving body 1 is not limited to the center of gravity, as long as it can represent the position of the moving body 1. For example, it can be set as the shape center point, the depth camera setting point, or the domain sensor setting point, etc. x and v y These are the X and Y velocities of the moving body 1 in the inertial coordinate system. Additionally, x... o and y o The representative point position of obstacle 600 is set and represented by the XY inertial coordinate system. The representative point position of obstacle 600 can be set to more than one point, such as the point where the distance between the moving body 1 and obstacle 600 becomes the shortest, the shape center of obstacle 600, and the center of gravity of obstacle 600.

[0055] <Sampling Method>

[0056] In this embodiment, the target trajectory generation unit 321 generates a target trajectory as an indicator for the control device 10 to control the moving body 1 based on information obtained from the observation device 200 using a sampling method, thus serving as an action plan. The action planning device 300 of Embodiment 1 uses a mathematical model f that mathematically represents the motion of the moving body 1 to perform time-varying state calculations, and solves the designed optimization problem based on prior consideration of state constraints, thereby generating the target trajectory. In this embodiment, a particle filter is used as one sampling method. The advantage of this disclosure is that it addresses all cases where calculations related to the time-varying state of a dynamic system are included within the formulation of the sampling method; therefore, the sampling method is not limited to a particle filter. Details related to this effect will be explained in the specific formulation described later.

[0057] <Formula for Action Plan Generation Using Sampling Methods>

[0058] In this embodiment, as shown in the following mathematical formula (1), the state quantity x and input u of the moving body used in the target trajectory generation unit 321 are set.

[0059] [Formula 1]

[0060]

[0061] Here, the coordinate system is set as Figure 4 The coordinate system shown, v x Let v be the velocity in the X direction. y Let it be the velocity in the Y direction.

[0062] The mathematical model f representing the motion of the moving body is represented by the following mathematical formula (2), which is linear relative to the input. Therefore, the extraction of the input sampling range can be simplified, and the computational load can be reduced.

[0063] [Equation 2]

[0064]

[0065] Furthermore, mathematical expressions (1) and (2) are examples of mathematical modeling, so the state variables x, input u, and mathematical model f can be selected to match the characteristics of the system. The coordinate system is not limited to an orthogonal coordinate system; for example, it can be defined using a path coordinate system.

[0066] In this embodiment, as a state constraint that should be considered in advance, the following mathematical expression (3) represents the state constraint h(x) ≥ 0 that the moving body 1 will not collide with the obstacle.

[0067] [Formula 3]

[0068]

[0069] Here, r m This represents a certain interval between the object and the obstacle. It means that during the period when the state constraint h(x) ≥ 0 is satisfied, the distance between the object and the obstacle ensures that r... m The above refers to the movement of distances. Furthermore, the state constraint h(x) ≥ 0 can also be considered as h(x) ≤ 0. However, in this embodiment, the state constraint h(x) ≥ 0 is taken as the object, so pay attention to the positive and negative signs.

[0070] Furthermore, the state constraint h(x) is a scalar value. Because the state constraint h(x) is a scalar value, the extraction of the input sampling range can be simplified, which can reduce the computational load.

[0071] In addition, the position of the obstacle can also be x o y o It can be included in the mathematical model and can also be defined as the position x of the obstacle. o yo It is a dynamic system.

[0072] In summary, the optimization problem is defined by the following mathematical formula (4).

[0073] [Formula 4]

[0074]

[0075] Here, J(x, u) is the evaluation function. The evaluation function can be designed based on the desired evaluation value. For example, it can use the time integral value of the distance to the target location or the time integral value of the input size.

[0076] In mathematical formula (4), the optimization method is expressed as minimizing the time integral value of the distance to the target location and the time integral value of the input size. Furthermore, the evaluation function is used as a term in optimization theory within the field of mathematics.

[0077] In this embodiment, a mobile body capable of omnidirectional movement is described as the object in the formula, but it is not limited to any object that can represent the mathematical model f and state constraints h(x) of a dynamic system. For example, a differential two-wheeled model or a four-wheeled vehicle model can also be used as the object.

[0078] <Action plan generation using sampling methods>

[0079] In this embodiment, a particle filter is used as the sampling method. A particle filter is a prediction method that uses time series data with probability density distribution. By using this particle filter to perform state prediction operations, the optimization problem of mathematical formula (4) is solved successively.

[0080] The particle filter used in state prediction operations is a filter that approximates the probability density distribution of a state using multiple particles. For example, if there are many particles representing a certain state quantity, the probability density of that state is high. In this case, a large number of particles are needed to ensure the approximate accuracy of the probability density distribution. That is, in the method that is the subject of this disclosure, the functionality of the method is reduced due to the decrease in effective particles. This reduction in functionality occurs when particles computed within the algorithm violate constraints and are deleted. The technique involved in this disclosure ensures the number of effective particles and suppresses the reduction in functionality by considering the constraints that are the cause of this reduction in functionality in advance.

[0081] <Operational flow of particle filter>

[0082] Figure 6 This is a flowchart illustrating the computational processing of the particle filter performed by the target trajectory generation unit 321 in the motion planning device 300 of this embodiment.

[0083] When the computational processing begins, the target trajectory generation unit 321 first obtains information on the state constraints that should be considered in advance (step S101). The probability density distribution is approximated in a way that satisfies the state constraints for each particle.

[0084] Target orbit generation unit 321 will N p Particle initialization (step S102). Here, N p It is an integer greater than or equal to 2. In this case, N... p Each particle can have its own distinct state variables. Furthermore, initialization can be performed based on the current state variables of the moving body 1. Here, particle initialization refers to the process of preparing N particles in the software. p The processing of individual particles is a necessary preparation for performing subsequent processing steps, such as step S102.

[0085] In this embodiment, the state variable P of a particle is defined by mathematical formula (1). Furthermore, the state variable of the nth particle is expressed as P. n .

[0086] In this embodiment, the initial value of the variable is set to the same value for all particles, denoted as x. r =0、y r =0、v x =0、v y =0. In addition, a weight w is defined for each particle, with an initial value equal to that of all particles, as set in the following mathematical formula (5). In addition, time t is defined and set to an initial value of 0.

[0087] [Formula 5]

[0088]

[0089] 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).

[0090] 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 the calculation of the following mathematical expression (6).

[0091] [Formula 6]

[0092]

[0093] In this context, the state variable x and the input u are defined by mathematical formula (1), and the mathematical model f is represented by mathematical formula (2).

[0094] Furthermore, particles are generated by satisfying the input sampling range that is expressed by the following mathematical expression (7).

[0095] [Formula 7]

[0096]

[0097] In this embodiment, the inequality is specifically expressed by the following mathematical expression (8).

[0098] [Formula 8]

[0099]

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

[0101] When the state constraint h(x) ≥ 0 is satisfied at a certain time t ≥ 0, the state quantity x that evolves over time using the input sampling that satisfies the inequality of mathematical formula (8) and the mathematical model f continues to satisfy h(x) ≥ 0 after time t. Theoretical proof is disclosed in Non-Patent Document 1. Non-Patent Document 1 discloses a method for calculating the speed range of a moving body that will not collide with an obstacle, more specifically, in Sections II.B and III.B of Non-Patent Document 1.

[0102] Furthermore, in this embodiment, mathematical expressions (6) and (8) are represented using continuous time, but it is also possible to use the defined discrete time width Δt to represent discrete time.

[0103] As described above, under the condition of mathematical formula (8), the input sampling range with state constraint h(x)≥0 is considered in advance. Using the mathematical model f of the above system, the state quantity x after the discrete time width Δt seconds is predicted. + Therefore, it is possible to predict the state of particles that takes into account constraints.

[0104] The state quantity P of the particle generated within the input sampling range that satisfies mathematical formula (8) n The state constraint h(x) ≥ 0 is satisfied. Therefore, all generated particles are effective, guaranteeing the approximate accuracy of the probability density distribution. Here, the approximate accuracy of the probability density distribution is defined by the number of effective particles. The approximate accuracy of the probability density distribution will decrease from the initially prepared N. p The amount of reduction is small, but since all generated particles are effective, the approximate accuracy can be guaranteed. Because the approximate accuracy of the probability density distribution is guaranteed, no post-processing is required, making it highly efficient.

[0105] Next, the target trajectory generation unit 321 predicts the state after a discrete time width Δt seconds based on state constraints (step S104). In this embodiment, the mathematical model of mathematical formula (2) is used in the particle state prediction. Here, random numbers are used to generate particles in the input sampling range extracted in step S103, thereby enabling particle generation based on the state constraint h(x)≥0.

[0106] The particle's state variable P is obtained by using the predicted state variable x. + The input u, which is the input sample value, is updated as shown in the following mathematical expression (9).

[0107] [Formula 9]

[0108]

[0109] Here, the particle's state variable x and the predicted state variable x + The input u, which serves as the input sample value, is entirely a column vector, and its transpose is used for simplification.

[0110] The state quantity P of the particle was calculated to be N. p After this step, the observation values ​​for each particle are calculated (step S105). The observation variables are designed based on the objectives of the action plan. The objectives of the action plan are determined by the surrounding environment of the moving body 1 or by the user's settings. In this embodiment, the objectives are maintaining the ideal movement path, maintaining the movement speed, and maintaining the distance from obstacles. Based on these objectives, the lateral deviation y from the ideal movement path is calculated. d The observed variables, namely, the moving speed v and the distance d from the obstacle, are represented by the following mathematical formula (10) φ.

[0111] [Formula 10]

[0112]

[0113] Here, e denotes the natural logarithm. Each value can be represented using the particle's state variable x, so the observed variable can also be considered as a function φ(x) of the state variable x. For simplicity, it will be denoted as φ in the following discussion. Furthermore, as observed variables, the transverse deviation y from the ideal path is also included. d At least one of the following: movement speed v, distance d from the obstacle.

[0114] Next, based on the observed value φ of each particle and the ideal observed value φ... i The difference is used to update the weights w of each particle. Here, the ideal observation value φ iThis refers to the observation value of the moving body 1 in a virtually designed ideal state, determined according to the objective of the driving plan. Therefore, when the moving body 1 meets the objective of the driving plan, the moving body 1 is in an ideal state. In this embodiment, the ideal observation value φ i It is represented by the following mathematical expression (11).

[0115] [Equation 11]

[0116]

[0117] Here, each value corresponds to the observed variable φ. Therefore, in this embodiment, the lateral deviation is 0, and the ideal moving speed is v. i The distance d from the obstacle is kept relatively large, that is, d is close to infinity (∞), so the value of the mathematical formula (10) expressed by the natural logarithm e is close to 0, which is the ideal state.

[0118] According to the theory of particle filters, the weights w of each particle are updated. As shown in the following mathematical formula (12), the update is related to the weight w of the nth particle. n The weights are proportional to the likelihood γ, making the cumulative weight of all particles equal to 1.

[0119] [Equation 12]

[0120]

[0121] Here, the likelihood γ of each particle is calculated using the following mathematical formula (13), with the covariance matrix Q related to the pre-defined state variable x of the particle and the covariance matrix R related to the observed value φ.

[0122] [Equation 13]

[0123]

[0124] Here, the det operator is used to calculate the determinant of a square matrix, and matrix S is represented by the following mathematical expression (14).

[0125] [Formula 14]

[0126]

[0127] Wherein, matrix H is the differential coefficient of the observed variable φ when the state variable x is a certain value (horizontal bar x), defined by the following mathematical formula (15).

[0128] [Formula 15]

[0129]

[0130] Next, the target trajectory generation unit 321 resamples the particles according to their weights w (step S106). However, in this embodiment, to prevent large deviations in the particles, only the virtual effective particle number N is resampled. eff Become the threshold N th Resampling is performed in the following cases; otherwise, nothing is done in this step. Here, the following mathematical formula (16) is used to calculate the virtual effective particle number N. eff Furthermore, resampling can be performed every time.

[0131] [Formula 16]

[0132]

[0133] Here, the term "virtual effective particle count" is used because the particle count is calculated virtually using the weights of each particle. When the weights of each particle are equal, the result of mathematical formula (16) is consistent with the particle count.

[0134] As a resampling method, similar to a typical particle filter, sampling is performed at equal intervals based on an empirical distribution function. In the case of resampling, the weights of each particle are initialized according to mathematical formula (5), assuming they are equal.

[0135] Next, regarding the state quantity P of the particle obtained in the above processing, the target orbit generation unit 321 calculates the weighted average value based on the weight w, and stores the state quantity x and the input u in the target orbit generation unit 321 as an action plan (step S107), and updates the time to t+Δt.

[0136] Next, the target trajectory generation unit 321 determines whether the updated time t has reached the planned final value τ during the planning period of the action plan. h (Step S108). When t < τ h In the case of "No", the processing from step S103 onwards is repeated. On the other hand, when t≥τ h In the case of "yes", the state variable x stored as the action plan and the data of the input u are output as the target trajectory and target input data, and the operation of generating the action plan ends.

[0137] Figure 7 This diagram schematically illustrates the result of the action plan generated using the sampling method described above. For simplicity, the number of particles is set to N. p =10, but in practice, sampling is implemented based on roughly 10 times the number of states used for speculation.

[0138] exist Figure 7In this embodiment, the initial value of all particles performing the prediction calculation is referenced to node 330. In this embodiment, the calculation is started with the same value as node 330. For these particles, after processing using mathematical formulas (1) to (8), their respective state transitions are predicted, and the updated values ​​of the particles are obtained by updating the state variables of the particles using mathematical formula (9). They satisfy the state constraint h(x) ≥ 0 expressed by mathematical formula (3) while following mathematical formula (2) of the mathematical model. Figure 7 When node 330 is set to its initial value, it has a deviation as represented by particle 331.

[0139] For each updated particle 331, after processing described using mathematical formulas (10) to (16), the target trajectory and target input are weighted according to their relationship with the surrounding environment, and resampling is performed according to the weights, for example, to become a new node 332. Here, in order to calculate the observation value of mathematical formula (10), information on the ideal path 400 and obstacles 600 (or people 500) is obtained.

[0140] <Effect>

[0141] Based on the structure of the action plan generation unit 320 described above, when an action plan using a sampling method represented by a particle filter is applied to a dynamic system, by generating particles in the input sampling range where the state constraint h(x)≥0 is considered in advance, an efficient action plan that can guarantee the accuracy of the probability density distribution can be implemented.

[0142] For particles judged to have collided with obstacles without prior consideration of the state constraint h(x) ≥ 0, post-processing is performed to ensure system safety or simulation compatibility. For example, when setting the deletion of particles judged to have collided, the accuracy of the probability density distribution cannot be guaranteed. For example, when adjusting weights as post-processing, it is necessary to ensure compatibility with the mathematical model f of the dynamic system, which may result in computational overhead for adjustment.

[0143] Furthermore, in this embodiment, a particle filter has been described as an example of a sampling method, but the sampling method can also be different methods, such as the Monte Carlo method, as part of the technical background. A feature of this disclosure is that the input sampling range is pre-limited based on state constraints expressed by a mathematical model, which can be introduced regardless of the method itself.

[0144] Furthermore, as shown in mathematical formula (3), the state constraints are limited to those that consider avoiding collisions with obstacles, but are not limited to this. Examples of state constraints include those that consider leaving the driving lane, those that consider intrusion into dangerous areas, those that consider falling due to abrupt steering, those that consider the movable domain in the case of a robotic arm, and those that consider special postures.

[0145] <Implementation Method 2>

[0146] Next, the action planning device 300 of Embodiment 2 according to this disclosure will be described. Furthermore, the structure of the action planning device 300 of Embodiment 2 is similar to... Figure 4 The structure of the action planning device 300 in Embodiment 1 shown is the same.

[0147] In the relationship between the mathematical model (mathematical formula (1), mathematical formula (2)) and state constraints (mathematical formula (3)) of the above-described implementation method 1, the input term is represented by the time-dependent first-order differential of the function h(x) representing the state constraints, but second-order or higher time differentials can also be performed.

[0148] For example, the state variable x of the moving body 1 is set as the position and velocity in the XY coordinate system, and the input u is set as the acceleration in the XY coordinate system, as shown in the following mathematical formula (17).

[0149] [Equation 17]

[0150]

[0151] The mathematical model f of the moving body 1 is also redefined as shown in the following mathematical formula (18).

[0152] [Formula 18]

[0153]

[0154] Let the state constraint h(x) ≥ 0 be used, for example, in the same way as in implementation 1, to prevent collisions with obstacles, as shown in mathematical formula (3).

[0155] Here, when calculating the time derivatives of the first and second derivatives of mathematical formula (3) based on the mathematical formula model f of the redefined moving body, the calculations become the following mathematical formulas (19) and (20), respectively.

[0156] [Formula 19]

[0157]

[0158] [Formula 20]

[0159]

[0160] In addition, the vector-valued function η(x) is defined as shown in the following mathematical expression (21).

[0161] [Equation 21]

[0162]

[0163] In order to use the vector value function η(x) to limit the input sampling range in a way that takes into account the state constraint h(x)≥0, the condition of mathematical expression (7) is extended to the condition of the following mathematical expression (22).

[0164] [Equation 22]

[0165]

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

[0167] By generating particles based on random numbers within the input sampling range shown in mathematical formula (22), the state variable x of the generated particles continuously satisfies the state constraint h(x). That is, similar to the result of implementation method 1, invalid particles are not generated, and sampling can be performed efficiently. A mathematical proof related to satisfying the state constraint h(x) is disclosed in Non-Patent Document 2. Non-Patent Document 2 discloses a method for calculating the acceleration range in which a moving body will not collide with obstacles, and more specifically, it is disclosed in Section III of Non-Patent Document 2.

[0168] <Implementation Method 3>

[0169] Next, the action planning device 300 of Embodiment 3 according to this disclosure will be described. Furthermore, the structure of the action planning device 300 of Embodiment 2 is similar to... Figure 4 The structure of the action planning device 300 in Embodiment 1 shown is the same.

[0170] Regarding the limitation of the input sampling range in Implementation 1 described above, the input value is obtained from a random number based on the input sampling range limited by mathematical formula (7). This limitation method also allows for the correction of the input value after obtaining it from a random number within an initially unrestricted input sampling range, in a manner that satisfies the constraint condition of mathematical formula (7). Furthermore, as a correction method, a correction method based on an optimization problem using the correction amount of the input as an evaluation function can be used.

[0171] Figure 8 This diagram schematically illustrates the input value correction method in Embodiment 3. Here, Embodiment 1 is shown as an example.

[0172] exist Figure 8 In the original input sampling range 804, which is an unrestricted input sampling range, when an input value is obtained by a random number, the input value can be separated according to the boundary 806 of the positive and negative changes on the left side of the mathematical formula (7) for example, the input value 801 that satisfies the mathematical formula (7) and the input value 802 that does not satisfy or violates the mathematical formula (7).

[0173] Regarding the violated input value 802, for example, regarding each value, a correction amount 805 is introduced, which is set as the corrected input value 803 in a way that satisfies the original input sampling range 804 and the input sampling range restricted by mathematical expression (7). Then, the input value 801 and the corrected input value 803 are used for execution. Figure 6 The processing after step S104 shown.

[0174] In this embodiment, by limiting the input sampling range, for example, when it is difficult to obtain the input value from the range limited by mathematical formula (7) according to the random number, it is possible to correct it after obtaining the input value from the random number through other optimization problems.

[0175] In addition, by using the correction amount of the input obtained from the random number to limit the input value in a way that satisfies mathematical formula (7) as the evaluation function, the designer can design the input sampling range to the range desired by the designer and can directly evaluate the correction amount of the input value.

[0176] When the input correction is used as the evaluation function, for example, the evaluation function can be set as the square of the input correction. In this case, for example, if mathematical expression (7) is linear with respect to the input, the analytical solution of the quadratic planning problem can be applied, so the computational efficiency becomes good.

[0177] <Implementation Method 4>

[0178] Next, the action planning device 300 of Embodiment 4 according to this disclosure will be described. Furthermore, the structure of the action planning device 300 of Embodiment 4 is similar to... Figure 4 The structure of the action planning device 300 in Embodiment 1 shown is the same.

[0179] The limitation on the input sampling range described in Implementation 1 or Implementation 3 requires that all input values ​​obtained from random numbers satisfy mathematical expression (7). However, in Implementation 4, by adjusting the input sampling range in advance based on the information of the state constraint h(x), it is also allowed that some or all of the input values ​​obtained from random numbers do not satisfy mathematical expression (7).

[0180] That is, by making the input value that satisfies mathematical formula (7) a certain number, the input sampling range is approximately limited by the probability density distribution function, and the range is adjusted. The information of the probability density distribution can be applied in the state prediction operation of the particle filter, thereby reducing the computational load.

[0181] For example, when symmetrically restricting the input sampling range, a Gaussian distribution can be used, with the mean and standard deviation representing the Gaussian distribution set as parameters for adjusting the input sampling range. Alternatively, depending on the optimization problem, either the mean or standard deviation representing the Gaussian distribution can be set as parameters for adjusting the input sampling range. Furthermore, when it is desired to restrict the input sampling range in a specific direction, a gamma distribution can be used.

[0182] In the state prediction operation of particle filters, information from the Gaussian distribution can be applied, thereby reducing the computational load. Furthermore, by defining the input sampling range using a Gaussian distribution, the parameters for range adjustment can be reduced to the mean and standard deviation, further reducing the computational load.

[0183] Furthermore, the adjustment of the range based on the Gaussian distribution is based on the optimization problem, which allows for mechanical adjustment.

[0184] In the following description, we assume that the input sampling range is restricted to a Gaussian distribution. Figure 9 Examples of limiting methods are illustrated schematically. Here, the case of Implementation 1 is shown as an example.

[0185] exist Figure 9 In this case, the original input sampling range, which serves as the adjusted input sampling range, is determined to be a Gaussian distribution, characterized by a mean of 811 and a standard deviation of 812. At this time, when obtaining input values ​​based on random numbers, a large number of values ​​that violate mathematical formula (7) are extracted. For example, when the mean of 811 violates mathematical formula (7), the probability that the input value obtained based on random numbers satisfies mathematical formula (7) is less than 0.5.

[0186] Here, based on the boundary 806 of the positive and negative changes on the left side of mathematical formula (7), for example, the mean 811 is corrected with a correction amount 813 to become the corrected mean 815, and the standard deviation 812 is corrected with a correction amount 814 to become the corrected standard deviation 816. Furthermore, as a correction method, similar to embodiment 3, a correction method based on the optimization problem of using the input correction amount as an evaluation function can be used.

[0187] Compared to the input values ​​obtained from the original input sampling range, a large number of values ​​satisfying mathematical equation (7) are extracted from input values ​​obtained from random numbers using a Gaussian distribution characterized by these corrected mean 815 and corrected standard deviation 816. Using these input sample values, the following is performed: Figure 6The processing after step S104. For example, when the mean 811 is corrected by the correction amount 813 and set to the corrected mean 815, the probability that the input value obtained by the random number satisfies the mathematical formula (7) can be adjusted to more than 0.5.

[0188] By approximating the input sampling range with a pre-defined probability density distribution function, only the parameters characterizing the probability density distribution need to be corrected. Therefore, the calculation can be simplified and the computational load reduced.

[0189] <Hardware Structure>

[0190] Furthermore, each component of the action planning device 300 described in embodiments 1 to 4 above can be configured using a computer and implemented by executing a program via a computer. That is, for example, by... Figure 10 The processing circuit 1000 shown is implemented in this way. In the processing circuit 1000, processors such as CPUs (Central Processing Units) and DSPs (Digital Signal Processors) are used to execute programs stored in a storage device to achieve the functions of each part. Furthermore, the target track storage unit 322 is implemented using a storage device included in a computer.

[0191] In the processing circuit 1000, dedicated hardware may also be used. When the processing circuit 1000 is dedicated hardware, it may be a single circuit, a composite circuit, a programmable processor, a parallel programmable processor, an ASIC (Application Specific Integrated Circuit), an FPGA (Field-Programmable Gate Array), or a combination thereof, which are equivalent to the processing circuit 1000.

[0192] The action planning device 300 can implement the functions of each component using independent processing circuits, or it can integrate these functions and implement them using a single processing circuit.

[0193] In addition, Figure 11 The diagram illustrates the hardware structure of the processing circuit 1000, which is configured using a processor. In this case, the functions of each part of the action planning device 300 are implemented through software (software, firmware, or a combination of software and firmware). The software is described as a program and stored in the memory 1002. The processor 1001, which functions as the processing circuit 1000, implements the functions of each part by reading and executing the program stored in the memory 1002 (storage device). That is, the program can be described as a program that describes the process and method of causing the computer to perform the actions of the constituent elements of the action planning device 300.

[0194] Here, the memory 1002 may be, for example, 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, floppy disk, optical disk, compact disk, mini disk, DVD (Digital Versatile Disc) and its drive device, or any storage medium used in the future.

[0195] The above describes the structure by which the functions of each component of the motion planning device 300 are implemented using either hardware or software. However, it is not limited to this; it is also possible to implement some components of the motion planning device 300 using dedicated hardware and others using software. For example, some components can be implemented using a processing circuit 1000, which is dedicated hardware, while others can be implemented by the processing circuit 1000, which is a processor 1001, reading and executing a program stored in the memory 1002.

[0196] As described above, the action planning device 300 can realize the above functions through hardware, software, or a combination thereof.

[0197] While this disclosure has been described in detail, the foregoing description is merely illustrative of all embodiments, and this disclosure is not limited thereto. Numerous variations not illustrated can be conceived without departing from the scope of this disclosure.

[0198] Furthermore, this disclosure allows for free combination of various embodiments or appropriate modification or omission of various embodiments within the scope of its disclosure.

Claims

1. A motion planning device for a dynamic system utilizing a sampling method, comprising: The action planning department, based on the objectives achieved by the dynamic system, outputs a mathematical model representing the motion of the dynamic system and the state constraints of the dynamic system that should be considered in advance; and The action plan generation unit, based on the mathematical model and the state constraints, limits the input sampling range and, based on the mathematical model, generates the action plan for the dynamic system through state inference calculations within the input sampling range.

2. The motion planning device according to claim 1, wherein, After obtaining an input value from a random number within the unrestricted input sampling range, the action planning generation unit modifies the input value in a manner that satisfies the state constraints.

3. The motion planning device according to claim 1, wherein, The action planning generation unit adjusts the input sampling range in such a way that the number of input values ​​that satisfy the input sampling range becomes a certain number or more.

4. The motion planning device according to claim 2 or 3, wherein, The motion planning generation unit corrects the input values ​​based on optimization problems.

5. The motion planning device according to claim 4, wherein, The motion planning generation unit uses the correction amount when modifying the input value as the evaluation function for the optimization problem.

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

7. The motion planning device according to claim 1, wherein, The state constraint is a constraint that the input sample based on the first derivative becomes a scalar value.

8. The motion planning device according to claim 3, wherein, The action planning generation unit defines the input sampling range using a probability density distribution in such a way that the input values ​​that satisfy the input sampling range become more than a certain number.

9. The motion planning device according to claim 8, wherein, The action plan generation unit sets the probability density distribution to a Gaussian distribution and sets the mean and standard deviation of the Gaussian distribution as parameters for adjusting the input sampling range.

10. The motion planning device according to claim 8, wherein, The action plan generation unit sets the probability density distribution to a Gaussian distribution, and based on the optimization problem, sets the mean and standard deviation, which represent the Gaussian distribution, as parameters for adjusting the input sampling range.

11. The motion planning device according to claim 1, wherein, The sampling method is a particle filter that approximates the probability density distribution of multiple particle pairs' states.

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

Images