Mobile object control device

Abstract

The purpose of the present disclosure is to provide a mobile object control device capable of enhancing adherence to a reference path even when the position and the attitude of a mobile object are stochastic. A mobile object control device according to the present disclosure comprises a control calculation unit that calculates a mobile object control amount for controlling a mobile object. The control calculation unit includes: a stochastic system optimization calculation unit that calculates an optimal sequence by sequentially solving a stochastic plan problem based on an evaluation function, using, as a constraint condition, a state equation which is obtained by modeling dynamic characteristics of the mobile object, which depend on a stochastic parameter and which have a probability distribution; and a control determination unit that converts the optimal sequence into the mobile object control amount. The stochastic system optimization calculation unit includes: a stochastic system evaluation function conversion unit that converts a stochastic evaluation function into a deterministic evaluation function on the basis of a stochastic process model of the stochastic parameter; and a deterministic system optimization calculation unit that solves a deterministic optimization problem.

Description

Mobile control device

[0001] The present disclosure relates to a mobile body control device that controls a mobile body.

[0002] When controlling a moving object, information about the position and orientation of the moving object may be used to make the moving object follow a reference route while traveling. However, the information about the position and orientation of the moving object may be affected by noise and other factors, making it probabilistic.

[0003] A technology such as that shown in Patent Document 1 has been disclosed in the past. In Patent Document 1, a potential risk is calculated based on a movement distribution indicating the probability of the presence of a target moving object and a peripheral object distribution indicating the probability of the presence of peripheral objects, and a potential risk map is generated. Furthermore, a route with a relatively low potential risk is selected as a travel route based on the potential risk map.

[0004] Patent No. 6956932

[0005] Yohei Hosoe and Tomomichi Hagiwara, “Equivalent Stability Notions, Lyapunov Inequality, and Its Application in Discrete-Time Linear Systems With Stochastic Dynamics Determined by an iid Process”, IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 64, NO. 11, NOVEMBER 2019.

[0006] In Patent Document 1, a route with a low potential risk in a potential risk map is searched for and the searched route is selected as the driving route. However, the method disclosed in Patent Document 1 only takes into account the potential risk, and therefore cannot take into account the ability to follow a reference route, such as the center of a lane.

[0007] The present disclosure has been made to solve such problems, and aims to provide a mobile body control device that can improve tracking of a reference path even when the position and attitude of the mobile body are probabilistic.

[0008] In order to solve the above problems, a mobile body control device according to the present disclosure includes a control calculation unit that calculates mobile body control variables for controlling the mobile body, and the control calculation unit has a stochastic system optimization calculation unit that uses state equations that model the dynamic characteristics of the mobile body that depend on stochastic parameters having a probability distribution as constraints and calculates an optimal sequence by successively solving a stochastic programming problem based on an evaluation function, and a control decision unit that converts the optimal sequence into mobile body control variables, and the stochastic system optimization calculation unit has a stochastic system evaluation function conversion unit that converts the stochastic evaluation function into a deterministic evaluation function based on a stochastic process model of the stochastic parameters, and a deterministic system optimization calculation unit that solves the deterministic optimization problem.

[0009] According to the present disclosure, it is possible to improve the tracking ability to the reference path even when the position and orientation of the moving object are probabilistic.

[0010] The objects, features, aspects, and advantages of the present disclosure will become more apparent from the following detailed description and the accompanying drawings.

[0011] 1 is a block diagram showing an example of the configuration of a mobile body control device according to embodiment 1. FIG. 2 is a block diagram showing an example of the configuration of a mobile body according to embodiment 1. FIG. 3 is a block diagram showing an example of the configuration of a control calculation unit according to embodiment 1. FIG. 4 is a diagram showing an example of the configuration of a mobile body according to embodiment 1. FIG. 5 is a diagram for explaining a state equation of a mobile body according to embodiment 1. FIG. 6 is a diagram showing an example of a stochastic process model of a position measured by GNSS according to embodiment 1. FIG. 7 is a block diagram showing an example of the configuration of a mobile body control device according to embodiment 2. FIG. 8 is a block diagram showing an example of the configuration of a stochastic system optimization calculation unit according to embodiment 2. FIG. 9 is a diagram for explaining constraints according to embodiment 2. FIG. 10 is a block diagram showing an example of the configuration of a control calculation unit according to embodiment 3. FIG. 11 is a diagram for explaining the output of a speed planning unit according to embodiment 3. FIG. 12 is a block diagram showing an example of the configuration of a control calculation unit according to embodiment 3. FIG. 13 is a block diagram showing an example of the hardware configuration of a mobile body control device according to embodiments 1 to 3. FIG. 14 is a block diagram showing an example of the hardware configuration of a mobile body control device according to embodiments 1 to 3.

[0012] <Embodiment 1> <Overall Configuration of Mobile Body Control Device 1> Fig. 1 is a block diagram showing an example of the configuration of a mobile body control device 1 according to embodiment 1. As shown in Fig. 1 , the mobile body control device 1 is connected to an internal sensor 5, a position and attitude estimation device 6, a map database 7, and a mobile body 8.

[0013] The position and orientation estimation device 6 has a function of estimating the position and orientation of the mobile object 8 relative to the surrounding environment of the mobile object 8 and outputting the position and orientation information. An external sensor (not shown) such as a camera, LiDAR (Light Detection and Ranging), or radar is connected to the position and orientation estimation device 6.

[0014] The camera (e.g., camera 80 in Figure 4) is installed in a position where it can capture images of the front, sides, and rear of the moving body 8, and is used to extract, for example, features of the surrounding environment of the moving body 8 from the captured images.

[0015] LiDAR extracts features of the surrounding environment of a moving object 8 by irradiating the surrounding area with a laser and detecting the time difference between when the laser reflects off a surrounding object and when it returns.

[0016] The radar irradiates the surroundings and detects the reflected waves to measure the relative distance and relative speed of objects present around the mobile unit 8, and extracts these as feature quantities.

[0017] 1 is installed, the position and orientation estimation device 6 estimates the position and orientation of the moving body 8 by comparing the values ​​of the external sensors with the map database 7. If the map database 7 shown in Fig. 1 is not installed, the position and orientation estimation device 6 performs processing such as SLAM (Simultaneous Localization and Mapping), which estimates the position of the moving body 8 while generating a map as the moving body 8 itself moves through the environment, and estimates the position and orientation of the moving body 8.

[0018] When the mobile object 8 moves outdoors, a sensor such as a GNSS (Global Navigation Satellite System) sensor is used as the position and orientation estimation device 6 to estimate the position and orientation of the mobile object 8 .

[0019] The internal sensor 5 is installed in the moving body 8 and outputs internal information of the moving body 8. The internal information is information necessary for grasping the internal state of the moving body 8, in addition to information about the inertia of the moving body 8, such as acceleration, speed, angular acceleration, and angular velocity. For example, if the moving body 8 is an automobile, the internal sensor 5 may be, for example, a vehicle speed sensor, an IMU (Inertial Measurement Unit) sensor, a steering angle sensor, and a steering torque sensor.

[0020] The map database 7 stores map data of the surroundings of the mobile object 8 and outputs it as map information. In FIG. 1 , the reference route generating unit 3 is connected to the map database 7, but not only the reference route generating unit 3 but also each component in the mobile object control device 1 can access the map database 7. The map database 7 contains features of the mobile object environment for grasping the position and attitude of the mobile object 8. When the mobile object 8 is an automobile, the map database 7 often contains data related to driving, such as road center coordinate information, stop line information, white line information, and drivable area information.

[0021] The map database 7 need not be provided if its information is not required by the position and orientation estimation device 6 and the reference path generation unit 3. For example, if the mobile object 8 is traveling in the center of the lane markings on both sides, the lane markings on both sides may be detected by a camera, and the center may be used as the reference path. In this case, the detection of the lane markings is equivalent to estimating the position and orientation, and therefore the map database 7 is not required.

[0022] The mobile object control device 1 includes a state quantity estimation unit 2, a reference path generation unit 3, and a control calculation unit 4. The mobile object control device 1 has a function of outputting mobile object control amounts for controlling a mobile object 8 based on internal world information acquired from an internal world sensor 5, position and orientation information acquired from a position and orientation estimation device 6, and map information acquired from a map database 7.

[0023] The moving body control amount is a signal for controlling the moving body 8, and includes a target route, a target trajectory, and command values ​​to the actuators of the moving body 8, etc.

[0024] In the present disclosure, a route including position coordinates to be followed by the moving body 8 is referred to as a "target route," and a route including instructions other than position coordinates, such as an instruction for arrival time at each point in the route, is referred to as a "target trajectory." The target trajectory may also include a target speed (equivalent to an instruction for arrival time). The target trajectory is not limited to a target speed or a target position, and may be any state quantity of the moving body 8.

[0025] The state quantity estimating unit 2 has a function of estimating state quantities that are used in the control calculation unit 4 and are not directly acquired by the internal sensor 5 or the like. The state quantity estimating unit 2 estimates the state quantities of the moving body 8 using widely used methods such as an observer, a Kalman filter, or a particle filter based on internal information, position and orientation information, and a state equation that represents the dynamic characteristics of the moving body 8. Note that if all the state quantities used in the control calculation unit 4 are detected by sensors or the like, the state quantity estimating unit 2 is not necessary.

[0026] The reference path generating unit 3 generates a reference path as a path to be used as a reference based on map information and position and orientation information. If the mobile object 8 is an automobile, the reference path may be a path representing the center of the lane. A general method such as the Dijkstra algorithm or the A* algorithm may be used to calculate a path from the initial position and orientation of the mobile object 8 to the target position and orientation, and the calculated path may be used as the reference path.

[0027] The control calculation unit 4 will be described later with reference to FIG.

[0028] <Mobile object 8> A mobile object is an object that can change the position and posture of its body coordinate system relative to a reference coordinate system by using its own movement mechanism, such as wheels, propellers, legs, or crawlers. Examples of mobile objects include automobiles, aircraft, drones, legged robots, probes, and agricultural machinery. In addition to these, any object that can move can be treated as a mobile object.

[0029] 2 shows the basic configuration of the moving body 8. The moving body 8 includes a command value calculation unit 9 and an actuator 10.

[0030] The actuator 10 is a drive source for driving a moving mechanism of the moving body 8. When the moving body 8 is an automobile, the actuator 10 is an electric motor, an automobile drive device, a brake control device, or the like.

[0031] The command value calculation unit 9 has a function of outputting a command value for actually driving the actuator 10 to the actuator 10 based on the mobile body control amount from the mobile body control device 1. Note that, in order for the command value calculation unit 9 to calculate the command value, the position and orientation estimation information from the position and orientation estimation device 6 and the internal field information from the internal field sensor 5 may also be used.

[0032] <Control Calculation Unit 4> The control calculation unit 4 will be described with reference to Fig. 3. As shown in Fig. 3, the control calculation unit 4 includes a stochastic system optimization calculation unit 11 and a control decision unit 12.

[0033] The stochastic system optimization calculation unit 11 has a function of performing an optimization calculation to minimize or maximize a predetermined evaluation function based on the state quantity and the reference path, and outputting an optimal sequence to the control decision unit 12 .

[0034] The control decision unit 12 has a function of deciding the amount of control of the moving object using the optimum sequence from the stochastic system optimization calculation unit 11 .

[0035] <Probabilistic Model Predictive Control> The present disclosure relates to a technique called probabilistic model predictive control. Here, probabilistic model predictive control will be described.

[0036] In the following, we will use the term "probabilistic" when dealing with quantities represented by random variables, and "deterministic" when dealing with deterministic quantities not represented by random variables. Also, we will assume that the probability distribution is a probability density function used when the random variable is a real number, but the same applies to discrete probability distributions where the random variable is a discrete value. Also, in the following, we will use the term ξ ｋ is a Z-dimensional probabilistic vector whose elements vary in value according to respective probability distributions.

[0037] Conventional model predictive control is a control method that sequentially determines control inputs by expressing the desired behavior over a horizon, which is a finite evaluation interval, as an evaluation function and solving a constrained optimal control problem from moment to moment, with the dynamic characteristics of the controlled object (corresponding to a moving object in this disclosure) expressed by a state equation and state and input constraints as constraint conditions.

[0038] Based on this idea, stochastic model predictive control is a control method that treats the state variables and disturbances of the controlled object as random variables. In the following, the following discrete-time state equation (Equation (1)) is considered:

[0039]

[0040] In formula (1), x ｋ is an n-dimensional vector representing the state of the controlled object at time k (k is an integer), u ｋ is an m-dimensional vector representing the control input, z ｋ is an r-dimensional vector representing the evaluation output. In the following, the subscript k represents the time k. A(ξ ｋ ), B ｕ (ξ ｋ ), B ｗ (ξ ｋ ), C(ξ ｋ ), D ｕ (ξ ｋ ), D ｗ (ξ ｋ ) and ξ ｋ The elements of these matrices are n×n, n×m, n×1, r×n, r×m, and r×1 matrices determined by ｋ In equation (1), A ｋ :=A(ξ ｋ ), B ｕ,ｋ :=B ｕ (ξ ｋ ), B ｗ,ｋ :=B ｗ (ξ ｋ ), C ｋ :=C(ξ ｋ ), D ｕ,ｋ :=D ｕ (ξ ｋ ), D ｗ,ｋ :=D ｗ (ξ ｋ) Because these matrices are probabilistic, x ｋ , z ｋ is a random variable.

[0041] As mentioned above, probabilistic model predictive control solves a constrained optimal control problem from moment to moment. To do this, first consider a horizon of length N (N is a natural number equal to or greater than 1), and express the evaluation function J for the response of equation (1) from the initial time k = k0 to the final time k = k0 + N of the horizon as shown in equation (2) below.

[0042]

[0043] Here, P Ｎ is an nxn, Q is an rxr, and R is an mxm definite positive definite symmetric matrix. A positive definite matrix is ​​one whose eigenvalues ​​are real numbers greater than or equal to 0. P Ｎ is set so that the controlled object is stable, and Q and R are z ｋ and u ｋ The superscript T represents the transpose. The initial state x of the horizon ｋ０ is estimated by the state quantity estimator. E[f] represents the expected value calculation for the mapping f. In other words, it means the multiple integral of the quantity obtained by multiplying f by the joint probability density function p of the random variables on which f depends. More specifically, x ｋ０＋Ｎ is ξ ｋ０ From ξ ｋ０＋Ｎ Since it depends on, for example, it is expressed by the following formula (3).

[0044]

[0045] Since the expectation calculation has linearity, the expectation calculation for the linear sum of the mappings f and g is expressed by the following equation (4).

[0046]

[0047] Hereinafter, an evaluation function in the form of equation (2) will be referred to as a "stochastic evaluation function."

[0048] Below, the input sequence U between horizons Ｎ is expressed as in equation (5) and is treated as a variable vector to be optimized.

[0049]

[0050] At this time, U Ｎ is an N·m-dimensional vector. Similarly, the state sequence X between horizon N ｓ，Ｎ is expressed as the following equation (6). ｓ，Ｎ is an n·(N−1) dimensional vector.

[0051]

[0052] In the case of probabilistic model predictive control, the state sequence is a random variable, so n ｐ The constraints are probabilistic as shown in equation (7) below.

[0053]

[0054] Here, H ｐ is n ｐ ×n·(N-1) deterministic matrix. ｐ is n ｐ It is a β-dimensional vector. ｐ is n ｐ dimensional vector, each element of which takes a real number ranging from 0 to 1. Pr represents the probability that the event occurs, and equation (7) is H ｐ X ｓ，Ｎ Each element of h ｐ The probability that ｐ This shows that the above is the case. In this disclosure, a constraint such as that in equation (7) is called a "chance constraint."

[0055] On the other hand, n for the expected value of the state sequence as shown in the following equation (8) ｅ Individual deterministic constraints can also be imposed.

[0056]

[0057] Here, H ｅ is n ｅ ×n·(N-1) deterministic matrix. ｅ is n ｅ is a -dimensional vector. In this disclosure, a constraint such as equation (8) is referred to as an “expectation constraint.”

[0058] In addition, as shown in the following equation (9), the input sequence itself also has n cIndividual deterministic constraints can be imposed.

[0059]

[0060] Here, H ｃ is n ｃ ×N m deterministic matrix. ｃ is n ｃ is a vector of dimension. In this disclosure, a constraint such as equation (9) is referred to as an “input constraint.”

[0061] To summarize, the probabilistic model predictive control problem is a state equation of Equation (1) and constraints of Equations (7), (8), and (9), and U that minimizes the evaluation function J of Equation (2) is Ｎ That is, it is expressed by the following equation (10).

[0062]

[0063] Here, although equation (10) is written as minimizing J, if J is changed to -J, it becomes a maximization problem, so from now on we will not distinguish between minimizing and maximizing J, but will use the term minimization. Note that the stochastic optimization problem of equation (2) is Ｎ Since the problem is written in quadratic form, it is a stochastic quadratic programming problem. Also, although the constraints are given as equations (7), (8), and (9), it is not necessary to use all of them; set appropriate constraints for the problem.

[0064] Various methods have been proposed for solving equation (10), but there are problems such as the solution being an approximation depending on the solution method or algorithm, or the optimization calculation taking a long time. ｋ Assuming that the probability distribution of is known, a technique for converting the stochastic optimization problem of equation (10) into a deterministic optimization problem using mapping is disclosed.

[0065] For deterministic constrained optimization problems, there are many algorithms and solvers that can efficiently and quickly obtain optimal solutions, such as the active set method and the interior point method. Hereinafter, these will be referred to as "deterministic solvers." By solving Equation (10), which is a stochastic optimization problem, the problem can be converted into a deterministic optimization problem and then solved using a deterministic solver, thereby enabling the stochastic optimization problem to be solved efficiently and quickly.

[0066] 3, a description will be given of the stochastic optimization calculation unit 11. The stochastic optimization calculation unit 11 includes a stochastic evaluation function conversion unit 13 and a deterministic optimization calculation unit .

[0067] The probability system evaluation function conversion unit 13 converts the probability system evaluation function into U as shown in the following equation (11) based on the state quantity of the moving object 8 and the reference route. Ｎ It has the function of converting it into a deterministic evaluation function of quadratic form with respect to

[0068]

[0069] Here, H Ｎ is an N m × N m matrix, c Ｎ is an N·m-dimensional vector. const. is U Ｎ It is a constant term that does not contribute to the optimization of the above, and will be ignored below. Note that, hereinafter, the evaluation function that can be expressed by equation (11) will be referred to as a "deterministic evaluation function."

[0070] The deterministic optimization calculation unit 14 converts the deterministic evaluation function from the stochastic evaluation function conversion unit 13 into a U that minimizes the deterministic evaluation function using the deterministic solver described above. Ｎ is calculated and output as the optimal sequence.

[0071] The following describes a specific calculation method for converting the stochastic evaluation function of formula (2) into the deterministic evaluation function of formula (11), which is a function of the stochastic evaluation function conversion unit 13. As an example, the calculation of one item of formula (2) (the term of formula (12) below) will be described.

[0072]

[0073] From the discrete time state equation of equation (1), x ｋ０＋Ｎis expressed as the following equation (13).

[0074]

[0075] Here, when the following equation (14) is used, the equation (13) can be expressed as the following equation (15).

[0076]

[0077]

[0078] Therefore, equation (12) can be expressed as U Ｎ can be expressed in a quadratic form with respect to

[0079]

[0080] Here, we have described the term in equation (12) of equation (2), but the remaining terms are also similarly Ｎ Therefore, equation (2) can be finally transformed into the quadratic form of equation (12).

[0081] By calculating the expected value calculation E included in equation (16) through offline or online processing, it can be solved by a deterministic solver. Note that the expected value calculation E includes calculations such as the following equation (17), but A Ｔ ｋ０ and A ｋ０ It should be noted that the following equation (18) is obtained because the are not independent.

[0082]

[0083]

[0084] This makes it difficult to convert a stochastic evaluation function into a deterministic evaluation function. According to the method of the present disclosure, it is possible to perform calculations such as those in equation (17) precisely and quickly by using mapping.

[0085] There are two main methods for calculating E included in equation (16). ｋ Using the probability distribution that follows, ξ ｋ０ From ξ ｋ０＋ＮThis method generates multiple sample paths up to and including , and calculates the sample average of Equation (17). This calculation is based on the law of large numbers, but if the number of sample paths is insufficient, the accuracy is poor and the calculation efficiency is also very poor, making it unsuitable for online calculations. Furthermore, some sample paths may include events that are unlikely to occur probabilistically, and if there are few sample paths, this may have a negative impact on the calculation of E.

[0086] However, it is possible to calculate the expected value calculation E using the sample average in advance in offline processing and record it in memory, and then read out and use the corresponding part in online processing.

[0087] The second method is ξ ｋ This method is a method of analytically calculating the relationship between time k and ξ, i.e., a stochastic process model. This method makes it possible to calculate the expected value quickly by defining a mapping for each stochastic process model. This mapping differs depending on the stochastic process model, but in this disclosure, we will use ξ as an example. ｋ We will explain the cases where the values ​​follow an independent and identical distribution (hereinafter abbreviated as "i.i.d.") with respect to time k, and where the values ​​follow a hidden Markov model (hereinafter abbreviated as "HMM"). For other stochastic process models, it is possible to define a mapping for each model in a similar manner.

[0088] In the following, for the purpose of explaining the mapping, the calculation of the term (the following formula (19)) that arises when converting the stochastic evaluation function of formula (2) into the deterministic evaluation function of formula (11) will be explained.

[0089]

[0090] <Calculation of evaluation function in case of i.i.d.> ξ ｋ If follows i.i.d., then ξ ｋThe shape of the probability distribution of is time-invariant and has no time dependency on the way the values ​​are generated. In the case of i.i.d., Lemma 2 described in Non-Patent Document 1 can be used. That is, it utilizes the fact that the following equation (20) holds for any nxn symmetric matrix M. In equation (20), the circled cross represents the Kronecker product.

[0091]

[0092] Here, G Ａ is n'×n which satisfies the following formula (21): ２ A full-rank matrix H Ａ is expressed as an n'×n matrix H Ａｉ (i=1, . . . , n), which is then decomposed into the following equation (23), which is an n'·n×n matrix.

[0093]

[0094]

[0095]

[0096] Here, row(A ０ ) is the matrix A ０ It is a row vector in which each element of A is arranged in order from the first row. ０ (i.e., when k = 0) is only considered because ｋ is i.i.d., and it is sufficient to consider the case where k=0. Using equation (20), the mapping V ＡＡ Define

[0097]

[0098] Calculate equation (19) using the mapping of equation (24). In the i.i.d. case, ξ ｋ are independent, the following equation (25) holds.

[0099]

[0100] Furthermore, when equation (3) is taken into consideration, it is expressed as in equation (26) below.

[0101]

[0102] Here, if the mapping of equation (24) is used, the following equation (27) is obtained.

[0103]

[0104] Repeat this to get X Ｎ Formula (19), X ０ =P, a recurrence formula (the following formula (28)) is obtained, which makes it possible to calculate formula (19) efficiently and precisely.

[0105]

[0106] Here, a method using a mapping has been described for calculating formula (19), but by defining a mapping in the same way, it is possible to efficiently calculate other terms that arise when converting the stochastic evaluation function of formula (2) into the deterministic evaluation function of formula (11). For example, a term such as the following formula (29) arises, but as with formula (20), it is possible to calculate G that satisfies the following formula (30). Ｂｕ Using the above, the mapping of the following equation (31) is defined.

[0107]

[0108]

[0109]

[0110] Using this mapping, equation (29) can be calculated as equation (32) below.

[0111]

[0112] In addition, G Ｂｕ Is B ｕ，０ can be obtained using the same procedure as in equations (21), (22), and (23).

[0113] In the case of i.i.d., by defining the mapping in this way, it is possible to convert a stochastic evaluation function into a deterministic evaluation function.

[0114] <Calculation of evaluation function in case of HMM> Next, ξ ｋThis section explains the case where the HMM follows the HMM. The HMM is a probabilistic model constructed assuming that modes (states) that output a sequence that follows a discrete or continuous probability distribution transition between modes according to a transition probability determined for each mode. Hereinafter, the probability distribution corresponding to each mode in the HMM will be referred to as the "output distribution."

[0115] The output and mode transitions of an HMM will now be described. For example, if an HMM is in "mode A" at a certain time, it outputs a sequence according to the probability distribution of "mode A." On the other hand, the mode may transition to another mode according to a certain transition probability, and the probability distribution of the output may change. For example, if there is a transition from "mode A" to "mode B," a sequence according to the probability distribution of "mode B" is output during the time period in "mode B." Within an HMM, it is not possible to directly observe which mode it is currently in, and only the output sequence is observed, so it is considered "hidden."

[0116] In the following, the HMM is assumed to be composed of L modes, which are referred to as mode 1, mode 2, ..., mode L. The output distribution of each mode is D １ , D ２ , . . . , D Ｌ and the mode at each time k is σ ｋ (i.e., σ ｋ (where η is 1, 2, ..., L). ｋ is the output sequence output by the HMM at time k, and η ｋ is D at each time １ , D ２ , . . . , D Ｌ The output distribution is assumed to be η ｋ is the random variable η （ｊ） In the following, we will write η at the time when the same mode is reached. ｋ are assumed to be independent at each time. ｋ is expressed by the following formula (33), and σ ｋ This indicates that the output distribution differs depending on the

[0117]

[0118] The transition probability from "mode i" to "mode j" is p ｉｊ If it is assumed that the transition between the modes of the HMM follows an irreducible and aperiodic Markov chain, it is expressed by the following equation (34).

[0119]

[0120] The transition matrix Π expressed as the following equations (35) and (36) and the probability vector a at time k are ｋ Using this, the time transition of the mode is expressed as in the following equation (37).

[0121]

[0122]

[0123]

[0124] Next, as in the case of i.i.d., we will explain the mapping defined by HMM using equation (19) as an example. In the case of HMM, ξ ｋ is ξ one time ago ｋ－１ Since it depends only on , it can be expressed as the following equation (38) using conditional probability.

[0125]

[0126] Considering equation (3), equation (19) can be expressed as equation (39) below using conditional expectation.

[0127]

[0128] The conditional expectation of equation (39) is ξ ｋ Of which η ｋ It does not depend on σ ｋ Since it depends only on σ ｋ Only σ ｋ０－１ is the mode one time before k0.

[0129] The calculation of equation (39) needs to take into consideration the path that the HMM modes take between horizons. To do this, we use an n×n matrix M corresponding to L modes at time k. ｋ （ｉ）(i=1, 2, . . . , L), and M ｋ （ｉ） The ordered sequence M ｔ，ｋ (Equation (40) below) is defined.

[0130]

[0131] Furthermore, the mappings expressed by the following equations (41) and (42) are defined.

[0132]

[0133]

[0134] In addition, G Ａｉ is the probability distribution D in "mode i" ｉ and its output η （ｉ） is a matrix expressed by the following equation (43) using G Ａｉ can be obtained by the same procedure as in equations (21), (22), and (23).

[0135]

[0136] Equation (39) is calculated using equations (40), (41), and (42). First, assuming equation (44) below, the innermost part of equation (39) can be expressed by equation (45) below.

[0137]

[0138]

[0139] Here, when the recurrence formula expressed by the following formula (46) is used, formula (39) further becomes the following formula (47).

[0140]

[0141]

[0142] By repeating this process, the following calculation can be performed as shown in equation (48).

[0143]

[0144] The above equation is σ ｋ０－１ For this calculation, the probability vector a at time k0-1 is ｋ０－１If it is possible to observe which mode the HMM is in at time k0-1, for example, if it is "mode 1" at time k0-1, then a ｋ０－１ = [1 0 . . . 0] Ｔ In addition, if the HMM at time k0-1 cannot be observed, the initial probability of each mode is given by a ｋ０－１ is given, and for example, the probability of each mode is assumed to be uniform, and the following equation (49) is used.

[0145]

[0146] The probability vector a at time k0-1 is given in this way. ｋ０－１ Using this, it can be calculated as in the following equation (50).

[0147]

[0148] In this way, it is possible to efficiently and exactly calculate equation (19), where a ｋ０－１，ｉ is a probability vector a ｋ０－１ is the i-th element of

[0149] Similarly, for other terms such as equation (39), a mapping and a recurrence formula can be defined. In the case of HMM, by defining a mapping in this way, a probabilistic evaluation function can be converted into a deterministic evaluation function.

[0150] In addition, the horizon length N and the element p of the transition matrix Π of the HMM ｉｊ is not a fixed value, but may be changed online in accordance with the environment around the mobile object.

[0151] The processing in the stochastic system evaluation function conversion unit 13 explained here is ｋ０ , the initial probability of HMM a ｋ０－１ and transition probability p ｉｊ Except for the parts related to terms that take different values ​​online, such as , most parts are deterministic matrices and numerical values. Therefore, matrices and numerical values ​​that do not change online can be calculated offline, stored in memory, and then read and used online, thereby speeding up online processing. This enables high-speed processing even when the horizon length N increases or decreases.

[0152] For example, in the case of i.i.d., ξ ｋ As long as the shape of the probability distribution of Ａ does not change with online information. Ａ Calculate the value of G for online processing. Ａ After reading out, it is possible to convert the stochastic evaluation function into a deterministic evaluation function by using the recurrence formula of formula (28). Since the calculation of the recurrence formula of formula (28) itself has a small processing load, it is possible to convert the stochastic evaluation function into a deterministic evaluation function at high speed.

[0153] Similarly, in the case of HMM, η （ｉ） ｋ As long as the shape of the probability distribution of Ａｉ does not change with online information. Therefore, as in the case of i.i.d., the G calculated and recorded offline Ａｉ is read out online and the recurrence formula of equation (47) is calculated, it is possible to convert a stochastic evaluation function into a deterministic evaluation function at high speed.

[0154] Similarly, the control decision unit 12 described below can speed up processing by calculating matrices and numerical values ​​that do not change with information obtained online, recording them in memory, and reading them out during online processing.

[0155] Depending on the surrounding environment of the moving object 8, when the moving object 8 is controlled, ξ ｋ and η(i) ｋ It is also possible that the shape of the probability distribution of changes. Ａ and G Ａｉ Recalculate it online, or if you know in advance that there are multiple patterns of probability distribution, calculate G with the probability distribution of each pattern. Ａ and G Ａｉ In online processing, G is calculated according to the probability distribution pattern. Ａ and G Ａｉ Just read it out and use it.

[0156] In this way, the probability system evaluation function conversion unit 13 calculates ξ ｋ By using the stochastic process model, the stochastic evaluation function is converted into a deterministic evaluation function.

[0157] <Control decision unit 12> Returning to Fig. 3, a specific operation of the control decision unit 12 will be described. The control decision unit 12 determines the U Ｎ The optimum series is used to convert the data into a mobile object control amount for controlling the mobile object 8.

[0158] As described above, the moving body control amount includes the target path, the target trajectory, and the command value to the actuator of the moving body 8. When the moving body control amount is the command value to the actuator, U Ｎ U included in ｋ０ is output from the control decision unit 12, but taking into consideration the delay in calculation, U Ｎ The terms included in, for example, u ｋ０＋１ etc. may be output.

[0159] When the moving object control amount is a target route or a target trajectory, U Ｎ and is calculated from the equation of state of the moving body 8 of equation (1). That is, the following equation (51) is used.

[0160]

[0161] where Ψ Ｎ , Θ ｕ，Ｎ , and Θ ｗ，Ｎ are expressed by the following equations (52), (53), and (54), respectively.

[0162]

[0163]

[0164]

[0165] The control decision unit 12 determines X ｓ，Ｎ The amount required for controlling the moving object 8 is extracted from the target path or target trajectory, and the moving object control amount is output. ｓ，Ｎ is ξ ｋSince the probability distribution of X depends on the target route and target trajectory, the target route and target trajectory also have a probability distribution. If the moving object 8 can be controlled in accordance with the target route and target trajectory having such a probability distribution, it is sufficient to leave it as it is, but usually it is necessary to convert from the probability distribution to a definite target route and target trajectory. There are various conversion methods, such as taking the expected value or the maximum value of the distribution, but here we will use ｓ，Ｎ Take the expected value of E[X ｓ，Ｎ ], we will describe a method for generating a target path or a target trajectory.

[0166]

[0167] Here too, ξ ｋ We will explain the case where E[Ψ Ｎ ] is described below.

[0168] <In the case of i.i.d.> ξ ｋ When i.i.d., as shown in equation (25), ξ ｋ is independent of time k, so E[Ψ Ｎ ] can be calculated as shown in the following equation (56).

[0169]

[0170] <In the case of H.M.M> ξ ｋ When is an HMM, as shown in equation (38), ξ ｋ is ξ ｋ－１ Therefore, for example, the following equation (57) is obtained.

[0171]

[0172] As in the case of the stochastic evaluation function conversion unit 13, an n×n matrix N corresponding to L modes at time k is ｋ （ｉ） (i=1, 2, . . . , L) and N ｋ （ｉ） The ordered sequence N ｔ，ｋ (see equation (58) below).

[0173]

[0174] Here, the mapping of the following equation (59) is defined.

[0175]

[0176] By mapping the equation (59), the recurrence formula of the following equation (61) is obtained as the following equation (60).

[0177]

[0178]

[0179] By using the mapping in equation (59) and the recurrence formula in equation (61), equation (57) can be expressed as the initial probability a ｋ０－１ Using the above, it is possible to perform calculation as shown in the following equation (62).

[0180]

[0181] In the above, i.i.d. and HMM's E[Ψ Ｎ ] has been explained. ｕ，Ｎ ] and E[Θ ｗ，Ｎ ] can be calculated in the same way, so E[X ｓ，Ｎ ] can be calculated.

[0182] The control decision unit 12 determines the optimal sequence U Ｎ Extraction of a part of E[X ｓ，Ｎ ] to generate a target path or a target trajectory.

[0183] <When the moving body 8 is an automobile> Up to now, the processing of the moving body control device 1 has been described using Figures 1 and 3. Here, in order to provide a more specific example, a case where the moving body 8 is an automobile will be described.

[0184] FIG. 4 is a diagram showing an example of the configuration when the moving body 8 is an automobile.

[0185] A steering wheel 50, which is installed so that a driver (i.e., a person operating the vehicle) can operate the vehicle, is coupled to a steering shaft 51. A pinion shaft 62 of a rack-and-pinion mechanism 53 is connected to the steering shaft 51. The rack shaft 63 of the rack-and-pinion mechanism 53 is movable back and forth in response to the rotation of the pinion shaft 62, and front knuckles 55 are connected to both left and right ends of the rack shaft 63 via tie rods 54. The front knuckles 55 rotatably support front wheels 64 as steered wheels, and are supported by the vehicle body frame so that they can be steered.

[0186] The torque generated when the driver operates the steering wheel 50 rotates the steering shaft 51, and the rack and pinion mechanism 53 moves the rack shaft 63 left and right in response to the rotation of the steering shaft 51. The movement of the rack shaft 63 causes the front knuckle 55 to rotate about a kingpin shaft (not shown), which causes the front wheels 64 to turn left and right. Therefore, the driver can change the amount of lateral movement of the vehicle by operating the steering wheel 50 when the vehicle is moving forward or backward.

[0187] In addition, in the case of a non-rider type mobile body 8 such as a fully automatic driving type, components for driver operation such as the steering wheel 50 are not required.

[0188] The automobile is equipped with internal sensors for recognizing the internal environment of the automobile, such as a vehicle speed sensor 70, an IMU sensor 71, a steering angle sensor 72, and a steering torque sensor 73.

[0189] The automobile is also equipped with actuators such as an electric motor 52 for realizing lateral movement of the automobile, a vehicle drive unit 56 for controlling longitudinal movement of the automobile, and a brake control unit 59.

[0190] The electric motor 52 is generally composed of a motor and gears, and can freely rotate the steering shaft 51 by applying torque to the steering shaft 51 in accordance with instructions from the steering control device 61. In other words, the electric motor 52 can freely steer the front wheels 64 independently of the operation of the steering wheel 50 by the driver.

[0191] The vehicle drive device 56 is an actuator for driving the vehicle in the forward and backward directions. In accordance with instructions from the acceleration / deceleration control device 58, the vehicle drive device 56 rotates the front wheels 64 and the rear wheels 65 using driving force obtained from a driving source such as an engine or a motor via a transmission (not shown) and a shaft 57. This allows the vehicle drive device 56 to freely control the driving force of the vehicle.

[0192] On the other hand, the brake control device 59 is an actuator for braking the vehicle, and controls the amount of braking of the brakes 60 installed on the front wheels 64 and rear wheels 65 of the vehicle in accordance with instructions from the acceleration / deceleration control device 58. A typical brake 60 generates a braking force by using hydraulic pressure to press pads against disc rotors that rotate together with the front wheels 64 and rear wheels 65.

[0193] The internal sensors and the above-described devices are assumed to form a network using a controller area network (CAN) or local area network (LAN) inside the vehicle. Each device can acquire its own information via the network. The internal sensors can also transmit and receive data to and from each other via the network.

[0194] A continuous-time state equation that models the dynamic characteristics of each moving body 8 is obtained. In this disclosure, a moving body control device 1 that can be applied to various moving bodies 8 is provided, but here, an automobile will be used as an example for detailed explanation. Note that, although many methods for controlling the moving body 8 have been proposed, in the first embodiment, a method for controlling lateral movement relative to a reference path will be explained.

[0195] First, the continuous-time state equations that represent the lateral dynamic characteristics of the moving body 8 will be explained using FIG. 5. The coordinate system with the X-axis and Y-axis in FIG. 5 is the reference coordinate system, and the coordinate system with the xb-axis and yb-axis is the body coordinate system. The lateral deviation and the deflection angle of the moving body with respect to the reference path expressed in the reference coordinate system are respectively expressed as e ｙ , e θ Then, the continuous time state equation of the moving body 8 in the lateral direction is expressed as the following equation (63).

[0196]

[0197] Here, x ｃ , A ｃ , B ｃｕ , B ｃｗ are expressed as in the following equation (64).

[0198]

[0199] The superscript "'" indicates time differentiation. The symbols represent the following:

[0200] v ｘ : Vehicle speed [m / s] δ: Rudder angle [rad] κ: Curvature of reference route [1 / m] m: Mass [kg] L ｆ : Distance between center of gravity and front wheel axle [m] L ｒ : Distance between center of gravity and rear wheel axle [m] I ｚ : Moment of inertia around the yaw axis [kg m^2] C ｆ : Front wheel cornering stiffness [N / rad] C ｒ : Rear wheel cornering stiffness [N / rad] e ｙ : Lateral deviation from the reference path to the center of gravity of the vehicle [m] e ‘ ｙ : e ｙ Time derivative of [m / s] e θ :Angle from the reference path to the center of gravity of the vehicle [rad] e ' θ : e θ Time derivative [rad / s] Cornering stiffness is a proportional coefficient that represents the relationship between the lateral force generated on a moving body and the sideslip angle, and is a value that changes depending on the condition of the contact surface between the moving body and the dry surface, wet surface, frozen surface, etc.

[0201] It should be noted that the curvature κ of the reference trajectory requires a predicted value between horizons N. There are several possible methods for this prediction, but the simplest method is to assume that κ is a constant value between horizons N and use the same value of κ between horizons.

[0202] As another method, the predicted position of the moving object 8 on the reference path between horizons N can be obtained by assuming that the moving object 8 moves on the reference path at a constant speed. If the information on the reference path includes the curvature of each point on the reference path, the curvature of the reference path at the predicted position can be read and used. Furthermore, if the reference path is expressed by a spline, a polynomial, or the like, the curvature at the predicted position can be calculated. That is, for example, if the reference path is expressed by Y = fr(X), the curvature at the predicted position can be obtained by the following equation (65).

[0203]

[0204] If the reference path is expressed as an X point group, a Y point group, or the like, the curvature can be found by first approximating the point group with a smooth function such as a spline or a polynomial, and then calculating equation (65). In addition, the curvature can be treated as a parameter with variation, and ξ can be used as a random variable with a probability distribution. ｋ and can be incorporated into equation (1).

[0205] The continuous-time state equation of Equation (63) is converted to the discrete-time state equation of Equation (1). At this time, the sampling interval between horizons is h ｋ Then, the following equations (66) and (67) are obtained.

[0206]

[0207]

[0208] In addition, h ｋ is usually a constant value, but the deterministic optimization calculation unit 14 varies in its processing time, so h ｋ A random variable that represents the variance (i.e., ξ ｋ ) may be added.

[0209] Next, z in Equation (1) ｋFirst, to make the car follow the reference path, we design x ｋ It is necessary to converge all elements of e to 0. At the same time, in order to consider the probability distribution of the position and orientation of the car, ｙ and θ to the random variable ξ １ｋ , ξ ２ｋ is added as additive noise, i.e., z ｋ is taken as shown in the following equation (68).

[0210]

[0211] This makes it possible to take into account the probability distribution of the position and attitude of the vehicle.

[0212] Here, e ｙ and e θ Although only additive noise of is considered, multiplicative noise can also be considered. In that case, a new random variable is considered, C ｋ Noe ｙ and e θ Just set the random variable to the element corresponding to e ｙ 'Ya θ The probability distribution of ' can also be considered.

[0213] ξ １ｋ and ξ ２ｋ should be designed based on the probability distribution by which the position and orientation information from the position and orientation estimation device 6 can be expressed. In many cases, i.i.d. is assumed, and the probability distribution is often expressed by a normal distribution or the like, but the method disclosed herein does not limit the shape of the probability distribution, and therefore can also be applied to probability distributions of other shapes.

[0214] As an example other than i.i.d. and normal distribution, we will explain how a probability model of a position detected by GNSS can be expressed using an HMM. Figure 6 shows a stochastic process model of a position measured by GNSS. RTK (Real Time Kinematic) is often used as a method for measuring position with high accuracy using GNSS. RTK is a method for measuring the position of an antenna installed on a mobile object 8 with high accuracy and speed using information from reference points installed within a range of several kilometers from the mobile object 8 and positioning signals from positioning satellites. RTK basically has four modes: Fix, Float, DGNSS (Differential-GNSS), and False Fix. There is also an Unpositioned mode, but this mode is ignored because it does not provide position information. Furthermore, when using an inertial sensor, there is a Dead Reckoning (DR) mode, but this is not considered here.

[0215] Although a detailed explanation will be omitted, Fix is ​​a mode in which the wave number (integer) of radio waves from a positioning satellite is determined with high accuracy using carrier phase information, and has the highest position accuracy. Float is a provisional position calculated in the stage before Fix, and is output when an integer solution for the wave number cannot be determined. DGPS is an estimated position when carrier phase information cannot be obtained. False Fix is ​​output when the wave number is determined incorrectly.

[0216] Figure 6 further shows an image of the probability distribution for each mode. If the dashed line in the figure represents the true position, Fix is ​​close to the true position and has small variations. Float has larger variations than Fix, but smaller than DGNSS. False Fix has small variations, but outputs a position that is offset from the true position. The transition between these modes is expressed by the transition matrix Π as shown in equation (35).

[0217] In the case of GNSS, the transition matrix Π changes depending on the surrounding environment of the moving object 8, and the element value p ｉｊFor example, in an environment where there are few structures that block radio waves from satellites or reflect radio waves, such as buildings, the transition to the Fix mode is likely. Also, in an environment where there are many buildings, such as in urban areas, the transition to the Float or False Fix mode is likely. In the mobile body control device 1 of the present disclosure, the transition matrix Π is adjusted to match the surrounding environment of the mobile body 8. ｉｊ There is no problem in changing

[0218] In this way, even when the existence probability of the moving body 8 can be expressed by an HMM, the moving body control device 1 of the present disclosure can cause the moving body 8 to follow the reference path while maintaining accuracy.

[0219] In the present disclosure, the evaluation function is expressed as the optimization variable U Ｎ is a quadratic programming problem, but it is a linear programming problem, i.e., the evaluation function is U Ｎ In this case, it is called a linear programming problem, but it is possible to convert the stochastic programming problem into a deterministic optimization problem using a method similar to that described in this disclosure. In this case, an efficient solver using linear programming or the like can be used as the deterministic solver.

[0220] Furthermore, in the present disclosure, a state equation such as equation (64) is used, but it needs to be modified to suit the moving body 8.

[0221] The parameters used in equation (64), such as mass m, moment of inertia Iz, cornering stiffness Cf, and Cr, may change depending on the number of occupants and the state of the contact surface between the vehicle and the road surface. Although these cannot be directly observed, if the probability distribution is known, the random variable with that probability distribution can be expressed as ξ ｋ and can be incorporated into equation (1). This enables more accurate control of the moving body. Note that even for moving bodies 8 other than automobiles, adding the uncertain part of the dynamic characteristics of the moving body 8 to the random variables enables more accurate control of the moving body.

[0222] The method disclosed in this disclosure makes it possible to provide a mobile body control device 1 that can improve path tracking performance while taking into account the probability distribution of the vehicle's position and attitude, which was not taken into account in Patent Document 1.

[0223] <Embodiment 2> In embodiment 2, a mobile body control device is provided that can ensure safety by avoiding obstacles even when the obstacles are present around the mobile body 8. Note that a description of the same content as in embodiment 1 will be omitted.

[0224] 7 is a functional block diagram of a mobile body control device 15 according to embodiment 2. An obstacle sensor 18 is connected to the mobile body control device 1 according to embodiment 1, and an obstacle movement prediction unit 16 is added.

[0225] The obstacle sensor 18 is composed of one or more sensors such as a camera, LiDAR, or Ladar. The obstacle sensor 18 outputs the relative position between the mobile body 8 and an obstacle as obstacle information. The obstacle sensor 18 may also be capable of outputting the relative speed between the mobile body 8 and the obstacle. In this case, the obstacle information outputs the relative position and relative speed between the mobile body 8 and the obstacle. If the mobile body 8 is an automobile, the obstacle information includes the positions and speeds of obstacles such as other automobiles, bicycles, and pedestrians. The obstacle sensor 18 is often installed on the mobile body 8 itself, but multiple obstacle sensors 18 may also be installed as environmental sensors in the environment in which the mobile body 8 travels. In this case, the mobile body 8 acquires obstacle information by communicating with the environmental sensors.

[0226] The obstacle movement prediction unit 16 has a function of predicting the position of an obstacle between the horizons N using obstacle information from the obstacle sensor 18 and outputting the predicted obstacle information. As a method for predicting the movement of an obstacle, for example, when the obstacle sensor 18 outputs speed information of the obstacle, various existing methods can be applied, such as a method of making a prediction assuming that the obstacle moves at a constant speed in a straight line, or a method of making a prediction using an obstacle movement prediction model learned by machine learning. The obstacle movement prediction unit 16 may use any method as long as it can predict the position of an obstacle between the horizons N.

[0227] <Constraint Calculation Unit 20> Fig. 8 is a functional block diagram of the stochastic optimization calculation unit 19 according to embodiment 2. Compared to embodiment 1, a constraint calculation unit 20 is added. The stochastic optimization calculation unit 19 and the control decision unit 12 are included in the control calculation unit 17.

[0228] The constraint calculation unit 20 has a function of generating definite constraint conditions for avoiding obstacles using the reference route and obstacle information. Here, the definite constraint conditions are n ｕ These are individual constraints, and are referred to as "definite constraints" in this disclosure.

[0229]

[0230] Here, H ｕ is n ｕ ×m N deterministic matrix, and h ｕ is n ｕ It is a dimensional vector.

[0231] As already explained, constraints in probabilistic model predictive control include chance constraints, expected value constraints, and input constraints, as shown in equations (7), (8), and (9). For the input constraints, equation (7) can be used as is. For the expected value constraints, E[X ｓ，Ｎ ] can be calculated using equation (55). In this case, as already explained, if a stochastic process model such as i.i.d. or HMM is used, it becomes possible to perform the calculation quickly and precisely.

[0232] Various methods have been proposed to convert chance constraints into deterministic constraints, such as using probability inequalities such as Chebyshev's inequality and Cantelli's inequality, generalized polynomial chaos, and conversion to expectation constraints (conservative approximation). Using these methods, the chance constraints in equation (7) can be converted into deterministic constraints.

[0233] The constraint calculation unit 20 calculates a deterministic constraint condition such as equation (69) by combining the chance constraint, the expected value constraint, and the input constraint, and outputs it to the deterministic optimization calculation unit 21 .

[0234] The deterministic optimization calculation unit 21 optimizes the deterministic evaluation function from the stochastic evaluation function conversion unit 13 under the deterministic constraint conditions from the constraint calculation unit 20, and outputs an optimal sequence.

[0235] <Example of Obstacle Avoidance> The operation of the constraint calculation unit 20 will be described with reference to Fig. 9. Fig. 9 shows a case where a reference route of a moving body 8 (car) has a width of 2L. ｌ The figure uses the center of the dividing line of the reference route and shows a scene where a stationary obstacle exists near the reference route. In this case, the moving object 8 must observe the constraint of not approaching the stationary obstacle. The black dots in the figure are the predicted positions of moving objects near the reference route within horizon N, and are determined by assuming that the moving object 8 moves at a constant speed on the reference route.

[0236] In such a case, the constraint calculation unit 20 sets an avoidance area large enough to include the stationary obstacle so that the moving object 8 does not get closer than necessary to the stationary obstacle, and sets constraints on this avoidance area. In the example of FIG. 9, the width L of the lane marking ｌ In contrast, L ｏ The avoidance area is set inside by k. It is assumed that the vehicle is predicted to approach the avoidance area of ​​the stationary obstacle at times k = k0 + i1, k = k0 + i2, and k = k0 + i3 between horizon N. The lateral position deviation e ｙ,ｋ０＋ｉ１ , e ｙ,ｋ０＋ｉ２ , e ｙ,ｋ０＋ｉ３ For , a chance constraint is set as in the following equation (70).

[0237]

[0238] β ｃ For example, if it is set to 0.95, it is possible to prevent the moving body 8 from entering the avoidance area with a probability of 95% or more. Also, if there is no problem with the expected value constraint, the following formula (71) may be used.

[0239]

[0240] In addition, e at other times ｙ For e, we set a constraint so that it does not go beyond the boundary line. ｙ,ｋ０＋ｉ１ , e ｙ,ｋ０＋ｉ２ , e ｙ,ｋ０＋ｉ３ The constraints on ｃ And, X ｓ，Ｎ Therefore, it can be written as in equation (7).

[0241] In the second embodiment, an example of avoiding a stationary obstacle has been described, but moving obstacles can also be avoided in the same way. Specifically, it is sufficient to set constraints according to the predicted positions of obstacles between horizons N.

[0242] The configuration according to the second embodiment makes it possible to provide a mobile object control device 15 that can avoid obstacles.

[0243] Third Embodiment In a third embodiment, a mobile body control device is provided that can simultaneously control the movement of the mobile body 8 in the forward / backward direction and the backward / forward direction by separating the control of the mobile body 8 in the forward / backward direction from the control of the forward / backward direction. Note that a description of the same content as in the first and second embodiments will be omitted. Here, the forward / backward direction is the x ｂ The axial direction is indicated by y ｂ The reason why such a mobile body control device is necessary is that, in general, in the case of a mobile body, the movement in the forward / backward direction and the movement in the lateral direction are coupled due to the motion constraint of the mobile body. Therefore, it is desirable to control the movement in the forward / backward direction and the lateral direction simultaneously, but in this case, the state equation of the mobile body becomes nonlinear with respect to the state, making it difficult to handle. In fact, if the velocity v is added to the state equation in the lateral direction as in equation (64), ｘ Therefore, it is preferable to first perform control in the longitudinal direction and then use that information to perform control in the lateral direction.

[0244] 10 shows a first configuration example of the control calculation unit 22 according to the third embodiment. Unlike the control calculation unit 4 in the first embodiment, the control calculation unit 22 is configured with a longitudinal control unit 23, a lateral control unit 25, and a control determination unit 27.

[0245] The forward / backward direction control unit 23 has a function of outputting a forward / backward direction sequence between horizons N using the reference route, obstacle information, and state quantities.

[0246] The lateral direction control unit 25 has a function of outputting an optimal lateral direction sequence between N horizons using the longitudinal direction sequence, the reference route, the obstacle information, and the state quantity.

[0247] The control determination unit 27 determines the moving object control amount for controlling the moving object 8 from the longitudinal direction sequence and the lateral direction optimum sequence.

[0248] The longitudinal direction control unit 23 is composed of a speed planning unit 24. The output of the speed planning unit 24 will be described using FIG. 11 . In the figure, the horizontal axis represents time, and the vertical axis represents the speed at that time. The speed planning unit 24 calculates and outputs the speed at each time from the horizon start time k0 to k0+N as a longitudinal direction series. In particular, FIG. 11 shows speed series for normal driving sections, curve sections, approach sections, and stopping sections, and shows an example of a speed series in which the specified speed is achieved in normal driving sections, deceleration is performed in curve sections to match the curvature of the reference route, deceleration is performed in approach sections to stop before an obstacle, and the vehicle remains stopped in stopping sections. The longitudinal direction control unit 23 outputs the longitudinal direction series from the speed planning unit 24 that operates in this way.

[0249] The lateral direction control unit 25 reflects the longitudinal direction sequence from the longitudinal direction control unit 23 and calculates the optimum lateral direction sequence. Here, in order to take the longitudinal direction sequence into consideration, the equation (1) is changed to the external parameter q ｋ That is, it is rewritten as the following equation (72).

[0250]

[0251] In equation (72), A and B ｕ , B ｗ , C, D ｕ , D ｗ The matrix of random variables ξ ｋ and an external deterministic parameter q ｋ In the present disclosure, q ｋ corresponds to the longitudinal direction series from the longitudinal direction control unit 23. More specifically, as shown in equation (64), the state equation is ｘ Since it depends on q ｋ v between horizons ｘ This is equivalent to regarding it as:

[0252] In this case, it is necessary to change the mapping of the equations (24) and (41) used in the stochastic system evaluation function conversion unit 13. For example, in the case of the equation (24), GＡ Gaq ｋ That is, the following equation (73) is obtained.

[0253]

[0254] G Ａ (q ｋ Regarding the calculation method of A, in the process of calculating the formulas (21), (22), and (23), ０ (ξ ｋ ) but A ０ (ξ ｋ , q ｋ ) can be calculated as follows. Ａ (q ｋ ) for q ｋ Although it is possible to calculate it sequentially when q is obtained, it is not suitable for online processing because it takes time to calculate. ｋ If the speed is controlled only within the range of, for example, 0 [km / h] to 100 [km / h], the speed range is divided into Nq points, and the divided points are designated as q １ , q ２ ,... ,q Ｎｑ Then, G at each division point Ａ The map of (q) is obtained by offline processing. In online processing, the map obtained by offline processing is used to efficiently calculate G by interpolation or the like. Ａ (q ｋ ) can be used. ｋ Here, the mapping is described for the case of i.i.d., but the mapping for the case of HMM can also be defined in a similar manner. ｋ A mapping can be defined according to

[0255] The control decision unit 27 outputs the moving object control amount from the longitudinal direction series and the lateral direction optimum series. Specifically, as in the first embodiment, it outputs the target route, the target trajectory, and the command values ​​to the actuators of the moving object.

[0256] The configuration according to the third embodiment makes it possible to provide a mobile body control device that can simultaneously control not only the lateral direction of a mobile body but also the longitudinal direction.

[0257] <Configuration Example 2> Fig. 12 shows Configuration Example 2 of the control calculation unit 28 according to the third embodiment. In this configuration, a mobile object control device capable of further improving performance is provided by applying probabilistic model predictive control to longitudinal control. Configuration Example 2 can improve ride comfort by suppressing changes in acceleration, for example. The difference from Configuration Example 1 is that the longitudinal direction control unit 29 is composed of a stochastic system optimization calculation unit 30 and a longitudinal direction control determination unit 31. A feature of this configuration is that the longitudinal direction control unit 29 also uses probabilistic model predictive control.

[0258] The stochastic system optimization calculation unit 30 uses the reference route, obstacle information, and state quantities to output a longitudinal direction optimal sequence.

[0259] The state equation of the moving body 8 in the forward and backward direction is the target acceleration u ａ From vehicle speed v ｘ The continuous time state equation up to time constant T ａ When modeled as a first-order lag system, the longitudinal acceleration α ｘ and the gravitational acceleration g, the gradient θ of the reference path ａ Using the above, it is expressed as the following equation (74).

[0260]

[0261] Here, x ａ , A ａ , and B ａｕ are expressed by the following equation (75).

[0262]

[0263] In addition, θ ａ is included in a map database, etc.

[0264] As in the case of the horizontal continuous-time state equation described in the first embodiment, the sampling interval is set to h ｋ After discretization using equation (67) in this case, the evaluation function of equation (2) can be constructed taking into account the output of equation (68).

[0265] In this case, for the evaluation function Q, α ｘBy adjusting the vehicle speed to reduce this, it is possible to improve the ride comfort. Also, by setting appropriate constraints, it is possible to realize behaviors such as stopping the vehicle in front of an obstacle, or following the speed of a moving obstacle.

[0266] Other processing by the stochastic system optimization calculation unit 30 is the same as that described in the first embodiment, and the optimal sequence U ａＮ will be output.

[0267] The longitudinal direction control determination unit 31 has a function of outputting a longitudinal direction sequence from the longitudinal direction optimum sequence. ａＮ , Equation (74), and Equation (51) are used to output a series of velocities between horizon N.

[0268] Furthermore, in the present disclosure, a state equation such as equation (75) is used, but it must be modified to suit the mobile object 8.

[0269] The configuration of the third embodiment makes it possible to provide a mobile body control device that can take into consideration the riding comfort in the longitudinal direction.

[0270] <Hardware Configuration> The functions of the state quantity estimator 2, reference path generator 3, and control calculator 4 in the mobile object control device 1 described in embodiment 1 are realized by processing circuits. That is, the mobile object control device 1 includes processing circuits for estimating state quantities, generating reference paths, and calculating mobile object control quantities. The processing circuit may be dedicated hardware, or may be a processor (also referred to as a CPU, central processing unit, processing device, arithmetic unit, microprocessor, microcomputer, or DSP (Digital Signal Processor)) that executes a program stored in memory.

[0271] 13, the processing circuit 100 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 functions of the state quantity estimator 2, the reference path generator 3, and the control and calculation unit 4 may be realized by the processing circuit 100 respectively, or these functions may be realized together by a single processing circuit 100.

[0272] When the processing circuit 100 is the processor 200 shown in FIG. 14 , the functions of the state quantity estimation unit 2, the reference path generation unit 3, and the control calculation unit 4 are realized by software, firmware, or a combination of software and firmware. The software or firmware is written as a program and stored in the memory 201. The processor 200 realizes each function by reading and executing the program recorded in the memory 201. That is, the mobile object control device 1 includes the memory 201 for storing a program that results in the execution of a step of estimating a state quantity, a step of generating a reference path, and a step of calculating a mobile object control quantity. It can also be said that these programs cause a computer to execute the procedures or methods of the state quantity estimation unit 2, the reference path generation unit 3, and the control calculation unit 4. Here, the memory may be, for example, a non-volatile or volatile semiconductor memory such as RAM (Random Access Memory), ROM (Read Only Memory), flash memory, EPROM (Erasable Programmable Read Only Memory), EEPROM (Electrically Erasable Programmable Read Only Memory), a magnetic disk, a flexible disk, an optical disk, a compact disk, a DVD (Digital Versatile Disc), or any storage medium that will be used in the future.

[0273] It should be noted that some of the functions of the state quantity estimator 2, the reference path generator 3, and the control calculator 4 may be realized by dedicated hardware, and other functions may be realized by software or firmware.

[0274] Thus, the processing circuitry can implement each of the above-described functions through hardware, software, firmware, or a combination thereof.

[0275] The above describes the mobile body control device 1 according to embodiment 1, but the same applies to the mobile body control device 15 according to embodiment 2 (Figures 7 and 8) and the mobile body control device according to embodiment 3 (Figures 10 and 12).

[0276] Within the scope of the present disclosure, the embodiments can be freely combined, modified, or omitted as appropriate.

[0277] Although the present disclosure has been described in detail, the above description is illustrative in all respects and is not restrictive. It is understood that countless variations not illustrated can be envisioned.

[0278] 1 Mobile object control device, 2 State quantity estimation unit, 3 Reference path generation unit, 4 Control calculation unit, 5 Internal sensor, 6 Position and attitude estimation device, 7 Map database, 8 Mobile object, 9 Command value calculation unit, 10 Actuator, 11 Stochastic system optimization calculation unit, 12 Control decision unit, 13 Stochastic system evaluation function conversion unit, 14 Deterministic system optimization calculation unit, 15 Mobile object control device, 16 Obstacle movement prediction unit, 17 Control calculation unit, 18 Obstacle sensor, 19 Stochastic system optimization calculation unit, 20 Constraint calculation unit, 21 Deterministic system optimization calculation unit, 22 Control calculation unit, 23 Longitudinal direction control unit, 24 Speed ​​planning unit, 25 Lateral direction control unit, 26 Stochastic system optimization calculation unit, 27 Control decision unit, 28 Control calculation unit, 29 Longitudinal direction control unit, 30 Stochastic system optimization calculation unit, 31 Longitudinal direction control decision unit, 50 Steering wheel, 51 Steering axis, 52 Electric motor, 53 Rack and pinion mechanism, 54 Tie rod, 55 Front knuckle, 56 Vehicle drive device, 57 Shaft, 58 Acceleration / deceleration control device, 59 Brake control device, 60 Brake, 61 Steering control device, 62 Pinion shaft, 63 Rack shaft, 64 Front wheels, 65 Rear wheels, 70 Vehicle speed sensor, 71 IMU sensor, 72 Steering angle sensor, 73 Steering torque sensor, 80 Camera, 100 Processing circuit, 200 Processor, 201 Memory.

Claims

1. A mobile object control device comprising: a control calculation unit that calculates a mobile object control amount for controlling a mobile object, the control calculation unit having: a stochastic system optimization calculation unit that uses a state equation that models the dynamic characteristics of the mobile object that depend on stochastic parameters having a probability distribution as a constraint condition, and calculates an optimal sequence by successively solving a stochastic programming problem based on an evaluation function; and a control decision unit that converts the optimal sequence into the mobile object control amount, the stochastic system optimization calculation unit having: a stochastic system evaluation function conversion unit that converts the stochastic evaluation function into a deterministic evaluation function based on a stochastic process model of the stochastic parameters; and a deterministic system optimization calculation unit that solves the deterministic optimization problem.

2. A mobile control device according to claim 1, wherein the stochastic system evaluation function conversion unit converts the stochastic evaluation function into the deterministic evaluation function using a mapping and an asymptotic formula defined based on the stochastic process model.

3. A mobile body control device as described in claim 1 or 2, further comprising an obstacle movement prediction unit that predicts the movement of the obstacle for a certain period of time in the future from obstacle information regarding the obstacle around the mobile body and outputs it as obstacle prediction information, wherein the stochastic system optimization calculation unit generates deterministic constraint conditions that are treated as constraint conditions in the deterministic system optimization calculation unit based on a reference route and the obstacle prediction information, and the deterministic system optimization calculation unit optimizes the deterministic system evaluation function under the deterministic system constraint conditions.

4. A mobile body control device as described in any one of claims 1 to 3, wherein the control calculation unit has a longitudinal direction control unit that outputs a longitudinal direction series for controlling the mobile body in the longitudinal direction, and a lateral direction control unit that outputs a lateral direction optimal series for controlling the mobile body in the lateral direction, the lateral direction control unit including the stochastic system optimization calculation unit, and the stochastic system optimization calculation unit calculates the lateral direction optimal series by referring to the longitudinal direction series output by the longitudinal direction control unit as a deterministic parameter.

5. A mobile body control device according to claim 4, wherein the longitudinal direction control unit calculates a longitudinal optimization sequence based on longitudinal dynamic characteristics of the mobile body.

6. A mobile control device according to any one of claims 1 to 5, wherein the variation in calculation time for the optimization problem is expressed as a probability distribution and incorporated into the state equation as a random sampling time.

7. A mobile control device according to any one of claims 1 to 6, wherein the stochastic system evaluation function conversion unit uses an independent identical distribution or a hidden Markov model as the stochastic process model.

Images