Mobile control device

Abstract

The present disclosure aims to provide a mobile body control device capable of improving tracking to a reference path even when the position and attitude of the mobile body are stochastic. The mobile body control device according to the present disclosure includes a control calculation unit that calculates a mobile body control amount for controlling the mobile body, the control calculation unit has a stochastic system optimization calculation unit that calculates an optimal sequence by successively solving a stochastic programming problem based on an evaluation function using a state equation that models the dynamic characteristics of the mobile body that depend on stochastic parameters having a probability distribution as a constraint condition, and a control decision unit that converts the optimal sequence into a mobile body control amount, 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.

Description

[Technical field]

[0001] The present disclosure relates to a mobile object control device that controls a mobile object. [Background technology]

[0002] When controlling a moving object, information on the position and orientation of the moving object may be used to make the moving object follow a route that serves as a reference when the moving object travels. However, the information on the position and orientation of the moving object may be affected by noise, etc., and may become probabilistic.

[0003] Conventionally, a technology such as that shown in Patent Document 1 has been disclosed. In Patent Document 1, a potential danger level is calculated based on a movement distribution indicating the presence probability of a target moving object and a surrounding object distribution indicating the presence probability of surrounding objects, and a potential danger level map is generated. In addition, a route with a relatively low potential danger level is selected as a travel route based on the potential danger level map. [Prior art documents] [Patent documents]

[0004] [Patent Document 1] Patent No. 6956932 [Non-patent literature]

[0005] [Non-Patent Document 1] 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. Summary of the Invention [Problem to be solved by the invention]

[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 a travel route. However, the method disclosed in Patent Document 1 considers only the potential risk, and therefore cannot consider 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. [Means for solving the problem]

[0008] In order to solve the above problems, a mobile object control device according to the present disclosure includes a control calculation unit that calculates a mobile object control amount for controlling the mobile object, and the control calculation unit has a stochastic system optimization calculation unit that uses a state equation that models 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 sequentially solving a stochastic programming problem based on an evaluation function, and a control decision unit that converts the optimal sequence into a mobile object control amount, and the stochastic system optimization calculation unit calculates a state equation that models dynamic characteristics of the mobile object that depend on stochastic parameters having a probability distribution as a constraint condition, and a stochastic system optimization calculation unit that converts the optimal sequence into a mobile object control amount based on a stochastic process model of the stochastic parameters, Includes probabilistic parameters The system includes a stochastic evaluation function conversion unit that converts a stochastic evaluation function into a deterministic evaluation function, and a deterministic optimization calculation unit that solves a deterministic optimization problem. Effect of the Invention

[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 a 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. [Brief description of the drawings]

[0011] [Figure 1] 1 is a block diagram showing an example of a configuration of a moving object control device according to a first embodiment. [Diagram 2] 1 is a block diagram showing an example of a configuration of a moving body according to a first embodiment. [Diagram 3] 2 is a block diagram showing an example of the configuration of a control calculation unit according to the first embodiment. FIG. [Figure 4] 1 is a diagram showing an example of a configuration of a moving body according to a first embodiment; [Diagram 5] FIG. 2 is a diagram for explaining a state equation of a moving body according to the first embodiment. [Figure 6] FIG. 2 is a diagram showing an example of a stochastic process model of a position measured by a GNSS in the first embodiment. [Figure 7] FIG. 11 is a block diagram showing an example of the configuration of a mobile object control device according to a second embodiment. [Figure 8] FIG. 11 is a block diagram showing an example of the configuration of a stochastic system optimization calculation unit according to the second embodiment. [Figure 9] FIG. 11 is a diagram for explaining constraints according to the second embodiment. [Figure 10] FIG. 11 is a block diagram showing an example of the configuration of a control calculation unit according to a third embodiment. [Figure 11] FIG. 11 is a diagram for explaining an output of a speed planning unit according to the third embodiment. [Figure 12] FIG. 11 is a block diagram showing an example of the configuration of a control calculation unit according to a third embodiment. [Figure 13] 1 is a block diagram showing an example of a hardware configuration of a mobile object control device according to first to third embodiments. [Figure 14] 1 is a block diagram showing an example of a hardware configuration of a mobile object control device according to first to third embodiments. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

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

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

[0014] A camera (e.g., camera 80 in FIG. 4) is installed in a position capable of capturing 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] The LiDAR extracts features of the environment surrounding the moving object 8 by shining a laser on the surroundings 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 in the vicinity of the moving body 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 object 8 by comparing the values ​​of the external sensor with the map database 7. If the map database 7 shown in Fig. 1 is not set, the position and orientation estimation device 6 performs processing such as SLAM (Simultaneous Localization and Mapping), which estimates the position of the moving object 8 while generating a map as the moving object 8 itself moves through the environment, and estimates the position and orientation of the moving object 8.

[0018] When the moving object 8 moves outdoors, a sensor such as a Global Navigation Satellite System (GNSS) sensor is used as the position and orientation estimation device 6 to estimate the position and orientation of the moving 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 related to 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 is, 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 moving body 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 moving body control device 1 can access the map database 7. The map database 7 contains features of the moving body environment for grasping the position and attitude of the moving body 8. When the moving body 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.

[0021] The map database 7 may not be provided if the information is not necessary for the position and orientation estimation device 6 and the reference route generation unit 3. For example, when the moving object 8 runs along 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 set as the reference route. In this case, the detection of the lane markings is equivalent to the estimation of 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 a mobile object control quantity 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 object control amount is a signal for controlling the moving object 8, and includes a target route, a target trajectory, and command values ​​for the actuators of the moving object 8, and the like.

[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 arrival time for each point, is referred to as a "target trajectory". The target trajectory may also include a target speed (equivalent to an arrival time instruction). 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 the internal world information, the 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 route generating unit 3 generates a reference route as a route to be referenced based on map information and position and orientation information. If the moving body 8 is an automobile, the reference route is a route that represents the center of the lane. A route from the initial position and orientation of the moving body 8 to the target position and orientation can be calculated by a general method such as the Dijkstra algorithm or the A* algorithm, and the calculated route can be used as the reference route.

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

[0028] <Mobile Unit 8> A mobile object is an object that can change the position and orientation of its body coordinate system relative to a reference coordinate system by using its own moving mechanisms such as wheels, propellers, legs, or crawlers. Examples of mobile objects include automobiles, aircraft, drones, legged robots, exploration vehicles, and agricultural machinery. 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 moving body control amount from the moving 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 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 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 a mobile object control amount by using the optimal 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. In addition, 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. In the following, we will use the term "ξ" when dealing with quantities that are deterministic and are not represented by random variables. k Let be a Z-dimensional probabilistic vector whose elements vary according to their respective probability distributions.

[0037] Conventional model predictive control is a control method in which the desired behavior over a horizon, which is a finite evaluation interval, is expressed as an evaluation function, and control inputs are determined sequentially by 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, we consider the following discrete-time state equation (Equation (1)).

[0039]

number

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

[0041] As mentioned above, stochastic model predictive control solves the 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 terminal time k=k0+N of the horizon as the following equation (2).

[0042]

number

[0043] Here, P N is an n×n, Q is an r×r, and R is an m×m definite positive definite symmetric matrix. A positive definite matrix is ​​one whose eigenvalues ​​are real numbers greater than or equal to 0. P N is set so that the controlled plant is stable, and Q and R are set to z k and u k The superscript T stands for transposition. The initial state of the horizon x k0is estimated by the state quantity estimator. E[f] represents the expectation operation for the mapping f. In other words, it means the multiple integral of the quantity obtained by multiplying the joint probability density function p of the random variables on which f depends and f. More specifically, x k0+N is k0 From ξ k0+N Since it depends on, for example, it is expressed by the following equation (3).

[0044]

number

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

[0046]

number

[0047] In the following, an evaluation function of the form shown in equation (2) is referred to as a "stochastic evaluation function."

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

[0049]

number

[0050] At this time, U N is an N m-dimensional vector. Similarly, the state sequence X s,N Let X be the following equation (6). s,N is a n·(N-1) dimensional vector.

[0051]

number

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

[0053]

number

[0054] Here, H p is p ×n·(N-1) deterministic matrix. p is p It is a β-dimensional vector. p is p dimensional vector, each element of which is a real number ranging from 0 to 1. Pr represents the probability of the event occurring, and equation (7) is H p X s,N Each element of h p The probability that p 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, the expected value of the state sequence as shown in the following equation (8) e Individual deterministic constraints can also be imposed.

[0056]

number

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

[0058] In addition, as shown in the following equation (9), the input sequence itself also has n c It is possible to impose definite constraints on each

[0059]

number

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

[0061] To summarize, the stochastic model predictive control problem is expressed as a state equation in equation (1) and constraints in equations (7), (8), and (9), and a problem to minimize the evaluation function J in equation (2) is solved by N This is an optimization problem to search for the following equation (10).

[0062]

number

[0063] Here, equation (10) is written as the minimization of J, but if J is changed to -J, it becomes a maximization problem, so from here on, we will not distinguish between minimization and maximization of J, but will use the term minimization. Note that the stochastic optimization problem of equation (2) is N Since the problem is written in quadratic form, it is a stochastic quadratic programming problem. The constraints are given in equations (7), (8), and (9), but 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 depending on the solution method or algorithm, there are problems such as the solution being an approximation or the optimization calculation taking a long time. k We disclose a technique for converting the stochastic optimization problem of equation (10) into a deterministic optimization problem using a mapping, assuming that the probability distribution of is known.

[0065] There are many algorithms and solvers that can efficiently and quickly obtain optimal solutions to deterministic constrained optimization problems, such as the active set method and the interior point method. Hereinafter, these will be referred to as "deterministic solvers." By solving the stochastic optimization problem of equation (10), which is a deterministic optimization problem, it is possible to convert it into a deterministic optimization problem and then solve it using a deterministic solver, thereby solving the stochastic optimization problem efficiently and quickly.

[0066] <Stochastic system optimization calculation section> 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. N It has the ability to convert the equation into a deterministic evaluation function of quadratic form with respect to

[0068]

number

[0069] Here, H N is an N m × N m matrix, c N is a vector of N m dimensions. const. is U N This is a constant term that does not contribute to the optimization of , and will be ignored below. Note that, below, the evaluation function expressed by equation (11) is referred to as the "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 by using the above-mentioned deterministic solver. N is calculated and output as the optimal sequence.

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

[0072]

number

[0073] From the discrete-time state equation in equation (1), x k0+N is expressed as the following equation (13).

[0074]

number

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

[0076]

number

[0077]

number

[0078] Therefore, equation (12) can be expressed as U N It can be expressed as a quadratic form in

[0079]

number

[0080] Here, we have described the term in equation (12) of equation (2), but the remaining terms are similarly calculated using U N Since equation (2) can be expressed as a quadratic form with respect to equation (12), equation (2) can finally be converted to the quadratic form of equation (12).

[0081] By calculating the expected value calculation E in equation (16) by offline or online processing, it can be solved by a deterministic solver. Note that the expected value calculation E includes the calculation of the following equation (17), for example. T k0 and A k0 It should be noted that the following equation (18) is used because the are not independent.

[0082]

number

[0083]

number

[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 a mapping.

[0085] There are two main ways to calculate E in equation (16). The first is to use the random variable ξ k Using the probability distribution of k0 From ξ k0+N This method generates multiple sample paths up to and calculates the sample average of equation (17). This is a calculation 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, so it is not suitable for online calculations. Also, some sample paths may include events that are unlikely to occur probabilistically, and if there are few sample paths, this may have a negative effect on the calculation of E.

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

[0087] The second method is to analytically calculate using the relationship regarding the time k of ξ k , that is, using a stochastic process model. For this method, by defining a mapping for each stochastic process model, it becomes possible to calculate the expected value at high speed. Although this mapping differs depending on the stochastic process model, in the present disclosure, as an example, when ξ k follows an independent and identical distribution (hereinafter abbreviated as "i.i.d.") with respect to time k and when it follows a Hidden Markov Model (hereinafter abbreviated as "HMM") will be described. For other stochastic process models, it is also possible to define a mapping in the same manner for each model.

[0088] Hereinafter, for the explanation of the mapping, the calculation of the term (the following formula (19)) that occurs when converting the probability system evaluation function of formula (2) into the deterministic system evaluation function of formula (11) will be described.

[0089]

Equation

[0090] <Calculation of the evaluation function in the case of i.i.d.> ξ k When follows i.i.d., the shape of the probability distribution of ξ k is time-invariant and has no time-dependence regarding the way of value occurrence. In the case of i.i.d., Lemma 2 described in Non-Patent Document 1 can be used. That is, for any n×n symmetric matrix M, the following formula (20) holds is utilized. In formula (20), the circled cross represents the Kronecker product.

[0091]

Equation

[0092] Here, G A is an n’×n that satisfies the following formula (21) 2A full-rank matrix H A As shown in the following equation (22), Ai (i = 1,…,n) and then expressed as the following equation (23):

[0093]

number

[0094]

number

[0095]

number

[0096] Here, row(A0) is a row vector in which each element of matrix A0 is arranged in order from the first row. Note that the reason why only A0 (i.e., k=0) is considered in equation (20) is because ξ k This is because k is iid and it is sufficient to consider the case of k = 0. Using equation (20), the mapping V AA Define.

[0097]

number

[0098] Calculate (19) using the mapping of (24). In the iid case, ξ k Since are independent, the following equation (25) holds.

[0099]

number

[0100] Moreover, taking into consideration equation (3), it can be expressed as equation (26) below.

[0101]

number

[0102] Here, if we use the mapping of equation (24), we get the following equation (27).

[0103]

number

[0104] Repeat this to get X N is expressed as equation (19), and the recurrence formula (equation (28) below) is obtained with X0 = P, making it possible to calculate equation (19) efficiently and precisely.

[0105]

number

[0106] Here, we have explained a method of using a mapping to calculate equation (19), but by defining a mapping in a similar manner, it is possible to efficiently calculate other terms that arise when converting the stochastic evaluation function of equation (2) into the deterministic evaluation function of equation (11). For example, terms such as those in the following equation (29) arise, but as with equation (20), if G satisfies the following equation (30), Bu Using this, we define the mapping of the following equation (31).

[0107]

number

[0108]

number

[0109]

number

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

[0111] [Number]

[0112] Note that G Bu can be obtained using B u,0 by 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, the probabilistic evaluation function can be converted into a deterministic evaluation function.

[0114] <Calculation of the Evaluation Function in the Case of HMM> Next, the case where ξ k follows an HMM will be described. An HMM is a probability model constructed such that the modes (states) that output a sequence following a discrete or continuous probability distribution transition according to the defined transition probabilities between each mode. Hereinafter, the probability distribution corresponding to each mode in the HMM is referred to as the "output distribution".

[0115] The output and mode transition of the HMM will be described. For example, when the HMM is in "Mode A" at a certain time, a sequence following the probability distribution of "Mode A" is output. On the other hand, the mode transitions to another mode according to a certain transition probability, and the probability distribution of the output may change. For example, when transitioning from "Mode A" to "Mode B", in the time interval when it is in "Mode B", a sequence following the probability distribution of "Mode B" is output. In the HMM, it is not possible to directly observe which mode it is currently in, and only the output sequence is observed, so it is "hidden".

[0116] Hereinafter, it is assumed that the HMM is composed of L modes, and each mode is referred to as Mode 1, Mode 2, ···, Mode L. The output distributions of each mode are D1, D2,..., D L respectively, and the mode at each time k is σk (i.e., σ k (Let η take the values ​​1, 2, ..., L). k is the output sequence of the HMM at time k, and η k At each time, D1, D2, ..., D L Let us assume that the output distribution follows one of the following distributions: k is the random variable η (j) In the following, we will write η k are independent at each time. Also, the random variable ξ k is expressed by the following formula (33), and σ k This indicates that the output distribution differs depending on the

[0117]

number

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

[0119]

number

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

[0121]

number

[0122]

number

[0123]

number

[0124] Next, as in the case of iid, we will explain the mapping defined by HMM using equation (19) as an example. k is ξ one hour ago k-1 Since it depends only on , it can be expressed as the following equation (38) using conditional probability.

[0125]

number

[0126] Taking into account equation (3), equation (19) can be expressed as the following equation (39) using conditional expectation.

[0127]

number

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

[0129] The calculation of equation (39) needs to take into account the path that the HMM modes take between horizons. To do so, we use an n × n matrix M k (i) (i=1,2,...,L), and M k (i) The ordered sequence M t,k (Equation (40) below) is defined.

[0130]

number

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

[0132]

number

[0133]

number

[0134] In addition, G Ai is the probability distribution D for "mode i" i and its output η (i) Using the above, the matrix is ​​expressed by the following equation (43). G Ai can be obtained in the same manner as equations (21), (22), and (23).

[0135]

number

[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]

number

[0138]

number

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

[0140]

number

[0141]

number

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

[0143]

number

[0144] The above equation is σ k0-1 For this calculation, the probability vector a at time k0-1 is k0-1 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 k0-1 =[1 0 ... 0] T In addition, if the HMM at time k0-1 cannot be observed, the initial probability of each mode is given by a k0-1 is given, and for example, the probability of each mode is assumed to be uniform, and the following equation (49) is used.

[0145]

number

[0146] The probability vector a at time k0-1 is given in this way. k0-1 Using this, the calculation can be done as shown in the following equation (50).

[0147]

number

[0148] In this way, it is possible to efficiently and precisely calculate equation (19), where a k0-1,i is a probability vector a k0-1 is the i-th element of .

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

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

[0151] The process in the stochastic evaluation function conversion unit 13 explained here is the state x k0 , the initial probability of the HMM a k0-1 and transition probability p ij Except for the parts related to terms that take different values ​​online, such as those mentioned above, 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, speeding up online processing. This enables high-speed processing even when the horizon length N increases or decreases.

[0152] For example, in the iid case, ξ k As long as the shape of the probability distribution of does not change, the G used in the mapping of Eq. (24) A does not change with online information. Therefore, G A Calculate the value of G for online processing. A After reading out, it is possible to convert the stochastic evaluation function into a deterministic evaluation function by using the recurrence formula in formula (28). Since the calculation of the recurrence formula in 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, η (i) k As long as the shape of the probability distribution of does not change, the G used in the mapping of Eq. (41) Aidoes not change with online information. Therefore, as in the iid case, the G Ai By reading out the above on-line and calculating the recurrence formula of equation (47), 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 performing offline calculations and recording in memory, and reading out during online processing, for matrices and numerical values ​​that do not change using information obtained online.

[0155] Depending on the surrounding environment of the moving object 8, the ξ k or η(i) k It is also possible that the shape of the probability distribution of changes. In that case, the G used in the mapping A Or G Ai Recalculate the probability distribution by online processing, or if it is known in advance that there are multiple patterns of the probability distribution, calculate G A Or G Ai In online processing, G is calculated according to the probability distribution pattern. A Or G Ai Just read it out and use it.

[0156] In this way, the probability system evaluation function conversion unit 13 converts ξ k 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 uses U N Using this optimal sequence, the system is converted into a mobile object control amount for controlling the mobile object 8.

[0158] As described above, the moving object control amount includes a target path, a target trajectory, and a command value to an actuator of the moving object 8. When the moving object control amount is a command value to an actuator, U NIncluded in k0 is output from the control decision unit 12, but considering delays in calculation, U N A term in, say, u k0+1 etc. may be output.

[0159] If the moving object control amount is a target path or a target trajectory, U N This is obtained from the state equation of the moving body 8 of formula (1). That is, the following formula (51) is used.

[0160]

number

[0161] Here, Ψ N , Θ u,N , and Θ w,N are expressed by the following equations (52), (53), and (54), respectively.

[0162]

number

[0163]

number

[0164]

number

[0165] In the control decision unit 12, X s,N 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. However, X s,N is kSince it depends on the probability distribution, the target path and target trajectory also have probability distributions. If the moving body 8 can be controlled corresponding to such a target path or target trajectory having a probability distribution, it can remain as it is. Usually, however, it is necessary to convert from the probability distribution to a definite target path or target trajectory. There are various conversion methods such as taking the expected value or taking the maximum value of the distribution. Here, X s,N takes the expected value of, that is, the following formula (55), and a method of generating a target path or target trajectory from E[X s,N will be described.

[0166]

Equation

[0167] Note that here too, the cases of ξ k being i.i.d. and the HMM case will be explained, and as an example, the calculation of E[Ψ N will be described.

[0168] <Case of i.i.d.> When ξ k is i.i.d., as shown in formula (25), since ξ k is independent with respect to time k, E[Ψ N can be calculated as in the following formula (56).

[0169]

Equation

[0170] <Case of H.M.M.> When ξ k is an HMM, as shown in formula (38), ξ k depends on ξ k-1 . Therefore, for example, the following formula (57) is obtained.

[0171]

Equation

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

[0173]

number

[0174] Here, we define the mapping of the following equation (59).

[0175]

number

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

[0177]

number

[0178]

number

[0179] By using the mapping in equation (59) and the recurrence formula in equation (61), equation (57) gives the initial probability a k0-1 Using this, it is possible to calculate as shown in the following equation (62).

[0180]

number

[0181] In the above, we consider iid and HMM E[Ψ N ] was explained. u,N ] and E[Θw,N ] can be calculated in the same way, so E[X s,N ] can be calculated.

[0182] The control decision unit 12 thus determines the optimal sequence U N Extraction of a part of E[X s,N ] to generate a target path or trajectory.

[0183] <When the moving object 8 is a car> So far, the processing of the mobile object control device 1 has been explained using Figures 1 and 3. Here, in order to provide a more specific example, a case will be explained in which the mobile object 8 is an automobile.

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

[0185] A steering wheel 50, which is installed so that a driver (i.e., a person who drives a 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 capable of reciprocating motion in response to 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 knuckle 55 rotatably supports front wheels 64 as steered wheels, and is supported by the vehicle body frame so as to be freely steerable.

[0186] Torque generated by the driver operating 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. Thus, the driver can change the amount of lateral movement of the vehicle by operating the steering wheel 50 when the vehicle moves forward or backward.

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

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

[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 a gear, and can freely rotate the steering shaft 51 by applying torque to the steering shaft 51 according to 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. The vehicle drive device 56 rotates the front wheels 64 and the rear wheels 65 using the driving force obtained from a driving source such as an engine or a motor via a transmission (not shown) and a shaft 57 in accordance with instructions from an acceleration / deceleration control device 58. 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 automobile, and controls the amount of braking of the brakes 60 installed on the front wheels 64 and rear wheels 65 of the automobile according to instructions from the acceleration / deceleration control device 58. A typical brake 60 generates a braking force by using hydraulic pressure to press a pad against a disc rotor that rotates together with the front wheels 64 and rear wheels 65.

[0193] The internal sensor and the above-described devices are assumed to form a network using a controller area network (CAN) or local area network (LAN) in the vehicle. Each device can obtain 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, a detailed description is given using an automobile as an example. Note that, although many methods for controlling the moving body 8 have been proposed, in the first embodiment, a method for controlling lateral movement with respect to a reference path will be described.

[0195] First, the continuous-time state equations that express the lateral dynamic characteristics of the moving body 8 will be described with reference to 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 deviation angle of the moving body with respect to the reference path expressed in the reference coordinate system are respectively defined as e y , e θ Then, the continuous-time state equation of the moving object 8 in the lateral direction is expressed as the following equation (63).

[0196]

number

[0197] where x c , A c , B cu , B cw Each of these is expressed as in the following equation (64).

[0198]

number

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

[0200] v x :Vehicle speed [m / s] δ: steering angle [rad] κ: Curvature of the reference path [1 / m] m: Mass [kg] L f :Distance between center of gravity and front wheel axle [m] L r :Distance between center of gravity and rear wheel axle [m] I z : Moment of inertia around the yaw axis [kg m^2] C f : Front wheel cornering stiffness [N / rad] C r : Rear wheel cornering stiffness [N / rad] e y : Lateral deviation of the vehicle's center of gravity from the reference path [m] e ‘ y :e y Time derivative of [m / s] e θ :Angle from the reference path to the center of gravity of the vehicle [rad] e ' θ :e θ Time derivative of [rad / s] Cornering stiffness is a proportionality coefficient that represents the relationship between the lateral force generated in a moving body and the sideslip angle, and is a value that changes depending on the state of the contact surface between the moving body and, for example, a dry surface, a wet surface, a frozen surface, or the like.

[0201] Note that the curvature κ of the reference trajectory needs to be predicted between horizons N. There are several possible methods for 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 on the reference path of the moving object 8 between horizons N is obtained by assuming that the moving object 8 moves on the reference path at a constant speed. If the curvature of each point on the reference path is included as information on the reference path, the curvature of the reference path at the predicted position can be read and used. Also, if the reference path is expressed by a spline or a polynomial, 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]

number

[0204] In addition, when the reference path is expressed by a set of X points, a set of Y points, etc., the curvature can be obtained by first approximating the set of points with a smooth function such as a spline or polynomial, and then calculating equation (65). In addition, the curvature can be treated as a parameter with variation, and ξ can be treated as a random variable with a probability distribution. k and can be incorporated into equation (1).

[0205] The continuous-time state equation in equation (63) is transformed into the discrete-time state equation in equation (1). In this case, the sampling interval between horizons is h k This results in the following equations (66) and (67).

[0206]

number

[0207]

number

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

[0209] Next, z in Eq. (1) k First, we design x k 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 vehicle, y and e θ Let us consider the random variable ξ 1k , ξ 2k is added as additive noise. That is, z k is taken as shown in the following equation (68).

[0210]

number

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

[0212] In addition, here, e y and e θ We have considered only additive noise of , but multiplicative noise can also be considered. In that case, we consider a new random variable, C k e y and e θ We can set the random variable to the element corresponding to e y ' or θ The probability distribution of ' can also be considered.

[0213] ξ 1k and ξ 2k should be designed based on what probability distribution the position and orientation information from the position and orientation estimation device 6 can be expressed with. In many cases, iid is assumed, and the probability distribution is often expressed with a normal distribution or the like, but the method disclosed herein does not limit the shape of the probability distribution, and therefore can be applied to probability distributions of other shapes.

[0214] As an example other than iid and normal distribution, we will explain that the probability model of the position detected by GNSS can be expressed by HMM. Figure 6 shows a stochastic process model of the position measured by GNSS. RTK (Real Time Kinematic) is often used as a method to measure the position with high accuracy by GNSS. RTK is a method to measure the position of an antenna installed on a moving object 8 with high accuracy and high speed using information of a reference point installed within a range of several kilometers from the moving object 8 and a positioning signal from a positioning satellite. RTK basically has four modes: Fix, Float, DGNSS (Differential-GNSS), and False Fix. There is also an unfixed mode, but this is ignored because no position information is obtained in this case. In addition, when an inertial sensor is used, there is a DR (Dead Reckoning) mode, but it is not included here.

[0215] A detailed explanation will be omitted, but 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 is the most accurate position mode. Float is a provisional position calculated 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 calculated incorrectly.

[0216] Figure 6 further shows an image of the probability distribution for each mode. If the dashed line in the figure is 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 with an 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 its element value p ijFor example, in an environment with few obstructions to radio waves from satellites or structures that reflect radio waves such as buildings, the mode is likely to transition to Fix mode. Also, in an environment with many buildings such as an urban area, the mode is likely to transition to Float or false Fix mode. In the mobile object control device 1 of the present disclosure, the transition matrix Π is changed according to the surrounding environment of the mobile object 8. ij There is no problem in varying

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

[0219] In this disclosure, the evaluation function is expressed by the optimization variable U N This is a quadratic programming problem for N It is also possible to make it linear with respect to . 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 shown in this disclosure. In this case, an efficient solver using linear programming or the like can be used as the deterministic solver.

[0220] In addition, in this disclosure, a state equation such as equation (64) is used, but it needs to be modified to suit the moving object 8.

[0221] The parameters of mass m, moment of inertia Iz, cornering stiffness Cf and Cr used in equation (64) may change depending on the number of passengers and the state of the contact surface between the vehicle and the road surface. Although these cannot be observed directly, if the probability distribution is known, the random variable with that probability distribution can be expressed as ξ k and can be incorporated into equation (1). This enables more accurate control of the moving body. For moving bodies 8 other than automobiles, adding the uncertainties 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 is capable of improving path tracking performance while taking into account the probability distribution of the position and attitude of the vehicle, something that could not be taken into account in Patent Document 1.

[0223] <Embodiment 2> In the second embodiment, a moving body control device is provided that can ensure safety by avoiding an obstacle even if the obstacle is present around the moving body 8. Note that a description of the same contents as in the first embodiment will be omitted.

[0224] 7 is a functional block diagram of a mobile object control device 15 according to embodiment 2. An obstacle sensor 18 is connected to the mobile object 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 cameras, LiDAR, Ladar, and other sensors. The obstacle sensor 18 outputs the relative position between the moving body 8 and an obstacle as obstacle information. The obstacle sensor 18 may also be capable of outputting the relative speed between the moving body 8 and an obstacle. In this case, the obstacle sensor 18 outputs the relative position and relative speed between the moving body 8 and the obstacle as the obstacle information. If the moving body 8 is an automobile, the position and speed of obstacles such as other automobiles, bicycles, and pedestrians become the obstacle information. The obstacle sensor 18 is often installed on the moving body 8 itself, but multiple obstacle sensors may be installed as environmental sensors in the environment in which the moving body 8 travels. In this case, the moving body 8 acquires the obstacle information by communicating with the environmental sensor.

[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 of 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 predicting the movement of an obstacle by assuming that the obstacle moves at a constant speed in a straight line, or a method of predicting the movement of an obstacle 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 operation section 20> 8 is a functional block diagram of a stochastic system optimization calculation unit 19 according to embodiment 2. Compared to embodiment 1, a constraint calculation unit 20 is added. The stochastic system optimization calculation unit 19 and the control decision unit 12 are included in a 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 u These constraints are referred to as "definite constraints" in this disclosure.

[0229]

number

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

[0231] As already explained, the constraints in the probabilistic model predictive control include chance constraints, expectation 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 expectation constraints, E[X s,N] can be calculated using equation (55). As already explained, if a stochastic process model such as iid or HMM is used, the calculation can be performed quickly and precisely.

[0232] There are various methods proposed for converting chance constraints into deterministic constraints, such as using probability inequalities such as Chebyshev's inequality and Cantelli's inequality, generalized polynomial chaos, and converting 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 performance function from the stochastic performance 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 reference route of a moving body 8 (car) having a width of 2L l The center of the lane markings is used to show a scene where a stationary obstacle exists near the reference route. In this case, the moving object 8 must adhere to 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 during horizon N, and are obtained 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 body 8 does not get closer than necessary to the stationary obstacle, and sets a constraint on this avoidance area. In the example of FIG. 9, the width L l In contrast, L oThe avoidance area is set inside by k = k0 + i1, k = k0 + i2, and k = k0 + i3. Suppose 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 horizons N. The lateral position deviation e y,k0+i1 , e y,k0+i2 , e y,k0+i3 For , we set the opportunity constraint as shown in the following equation (70).

[0237]

number

[0238] β c For example, if it is set to 0.95, the moving object 8 can be prevented 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]

number

[0240] In addition, e at other times y For e, we set a constraint so that it does not go beyond the boundary line. y,k0+i1 , e y,k0+i2 , e y,k0+i3 The constraints on c And, X s,N Therefore, it can be written as equation (7).

[0241] In the second embodiment, an example of avoiding a stationary obstacle has been described, but even if the obstacle moves, it is possible to avoid the obstacle in the same way. Specifically, it is sufficient to set a constraint according to the predicted position of the obstacle between the horizons N.

[0242] The configuration according to the second embodiment makes it possible to provide a moving object control device 15 capable of avoiding obstacles.

[0243] <Embodiment 3> In the third embodiment, a moving body control device is provided that is capable of simultaneously controlling the movement in the forward / backward direction and the lateral direction by separating the control of the lateral direction and the control of the forward / backward direction of the moving body 8. Note that a description of the same contents as in the first and second embodiments will be omitted. Here, the forward / backward direction is the x b The axial direction is indicated, and the horizontal direction is indicated by y b The reason why such a mobile object control device is necessary is that, in general, in the case of a mobile object, the movement in the forward / backward direction and the movement in the lateral direction are coupled due to the motion constraint of the mobile object. 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 object becomes nonlinear with respect to the state, and handling becomes complicated. In fact, if the velocity v is added to the state equation in the lateral direction as shown in equation (64), x Therefore, it is preferable to first control the front-rear direction and then use that information to control the lateral direction.

[0244] <Configuration example 1> 10 shows a first configuration example of the control calculation unit 22 according to the embodiment 3. Unlike the control calculation unit 4 in the embodiment 1, the control calculation unit 22 is composed of a longitudinal control unit 23, a lateral control unit 25, and a control decision unit 27.

[0245] The forward / backward direction control unit 23 has a function of outputting a forward / backward direction sequence between the N horizons 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 a moving object control amount for controlling the moving object 8 from the longitudinal sequence and the lateral optimum sequence.

[0248] The longitudinal control unit 23 is composed of a speed planning unit 24. The output of the speed planning unit 24 will be described with reference to 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 start time k0 of the horizon to k0+N as a longitudinal series. In particular, FIG. 11 shows speed series in a normal driving section, a curve section, an approach section, and a stopping section, and shows an example of a speed series in which the specified speed is used in normal driving, deceleration is performed in accordance with the curvature of the reference route in a curve section, deceleration is performed in order to stop in front of an obstacle in an approach section, and the vehicle is kept stopped in a stopping section. The longitudinal control unit 23 outputs the longitudinal series from the speed planning unit 24 that performs such operations.

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

[0250]

number

[0251] In equation (72), A, B u , B w , C, D u , D w The matrix of random variables ξ k and an externally deterministic parameter q k In the present disclosure, q k corresponds to the longitudinal series from the longitudinal control unit 23. More specifically, the state equation is expressed as the velocity v x Since it depends on q k v between horizons x This is equivalent to regarding it as such.

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

[0253]

number

[0254] G A (q k Regarding the calculation method of A0(ξ k ) instead of A0(ξ k ,q k ) can be calculated as G A (q k ) for q k It is possible to calculate it sequentially when q is obtained, but this is not suitable for online processing because it takes a long time to calculate. k If you want to control only within the range of 0 [km / h] to 100 [km / h], divide that range into Nq points and call the divided points q 1 ,q 2 ,...,q Nq Then, G at each division point A The map of (q) is obtained by offline processing. In online processing, the map obtained by offline processing is used to efficiently calculate G A (q k ) can be used. This allows q k Here, we have described the mapping for the iid case, but the mapping for the HMM case can be defined in a similar way. k We can define a mapping according to

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

[0256] The configuration according to the third embodiment makes it possible to provide a moving body control device that can control not only the lateral direction of a moving body but also the forward and backward directions at the same time.

[0257] <Configuration example 2> FIG. 12 shows a second configuration example 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. The second configuration example can improve ride comfort, for example by suppressing changes in acceleration. The difference from the first configuration example is that the longitudinal control unit 29 is composed of a stochastic system optimization calculation unit 30 and a longitudinal control decision unit 31. This configuration is characterized in that the longitudinal 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 forward / rearward optimal sequence.

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

[0260]

number

[0261] where x a , A a , and B. au Each of these is expressed by the following equation (75).

[0262]

number

[0263] In addition, θ a 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 k After discretization using equation (67) in this case, the evaluation function of equation (2) can be constructed taking into account the output such as equation (68).

[0265] In this case, for the evaluation function Q, α x By adjusting the vehicle speed to suppress 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 the 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 aN will be output.

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

[0268] In addition, in this disclosure, a state equation such as equation (75) is used, but it needs to be modified to suit the moving object 8.

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

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

[0271] When the processing circuit is a dedicated hardware, the processing circuit 100 corresponds to, for example, a single circuit, a composite circuit, a programmed processor, a parallel programmed processor, an ASIC (Application Specific Integrated Circuit), an FPGA (Field Programmable Gate Array), or a combination of these, as shown in Fig. 13. Each function of the state quantity estimation unit 2, the reference path generation unit 3, and the control calculation unit 4 may be realized by the processing circuit 100, or each function may be realized collectively by one 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 the other functions may be realized by software or firmware.

[0274] Thus, the processing circuitry may implement the functions described above through hardware, software, firmware, or a combination of these.

[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 (FIGS. 7 and 8) and the mobile body control device according to embodiment 3 (FIGS. 10 and 12).

[0276] Within the scope of the present disclosure, the embodiments can be freely combined, and each embodiment can be 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. [Explanation of symbols]

[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 optimization calculation unit, 12 Control decision unit, 13 Stochastic evaluation function conversion unit, 14 Deterministic optimization calculation unit, 15 Mobile object control device, 16 Obstacle movement prediction unit, 17 Control calculation unit, 18 Obstacle sensor, 19 Stochastic optimization calculation unit, 20 Constraint calculation unit, 21 Deterministic optimization calculation unit, 22 Control calculation unit, 23 Forward / rearward direction control unit, 24 Speed ​​planning unit, 25 Lateral direction control unit, 26 Stochastic optimization calculation unit, 27 Control decision unit, 28 Control calculation unit, 29 Forward / rearward direction control unit, 30 Stochastic optimization calculation unit, 31 Forward / rearward 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 wheel, 65 rear wheel, 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 control calculation unit that calculates a moving body control amount for controlling the moving body; The control calculation unit a stochastic system optimization calculation unit that calculates an optimal sequence by sequentially solving a stochastic programming problem based on an evaluation function, using a state equation that models dynamic characteristics of the moving object that depend on stochastic parameters having a probability distribution as a constraint; a control decision unit that converts the optimal sequence into the mobile object control amount; and The stochastic system optimization calculation unit a stochastic system evaluation function conversion unit that converts a stochastic evaluation function into a deterministic evaluation function based on a stochastic process model of stochastic parameters; a deterministic optimization calculation unit that solves a deterministic optimization problem; A mobile object control device having the above.

2. The mobile body 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. an obstacle movement prediction unit that predicts the movement of the obstacle in the future for a certain period of time based on obstacle information relating to the obstacle around the moving object, and outputs the prediction information as obstacle prediction information; the stochastic optimization calculation unit generates deterministic constraint conditions to be treated as constraint conditions in the deterministic optimization calculation unit based on the reference route and the obstacle prediction information; The mobile body control device according to claim 1 , wherein the deterministic optimization calculation unit optimizes the deterministic evaluation function under the deterministic constraint condition.

4. The control calculation unit a longitudinal direction control unit that outputs a longitudinal direction sequence for controlling the longitudinal direction of the moving body; a lateral direction control unit that outputs a lateral direction optimum sequence for controlling the moving object in the lateral direction; and the lateral direction control unit includes the stochastic system optimization calculation unit, 3. The mobile body control device according to claim 1, wherein the stochastic optimization calculation unit calculates the lateral optimum sequence by referring to the longitudinal sequence output by the longitudinal control unit as a deterministic parameter.

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

6. 3. The mobile body control device according to claim 1, wherein the variation in the calculation time of the optimization problem is expressed as a probability distribution and incorporated into the state equation as a random sampling time.

7. The mobile control device according to claim 1 or 2, wherein the stochastic system evaluation function conversion unit uses an independent identical distribution or a hidden Markov model as the stochastic process model.

Images