RU2531864C1 - Moving object control device - Google Patents

Abstract

FIELD: physics, control.

SUBSTANCE: invention relates to control systems and can be used at development of the systems of control of moving objects providing their travel along the preset trajectory at a preset speed in uncertain mediums. The moving object control unit contains the trajectory scheduler, three evaluators of matrix coefficients, control signal evaluator, two matrix transposition units, information sensor unit, sensor support unit, unit for creation of vector of nonlinear elements, unit for creation of matrix of control coefficients, unit for creation of the matrix - derivative of the column vector of external velocities by the row-vector of internal coordinates, unit for creation of the matrix - derivative of the column vector of external velocities by the row-vector of world coordinates, unit of creation of the vector of external velocities, threshold device, device for measuring of the range of variation of angle of sight of an obstacle and distance to it, unit for calculation of the allowance of the control signal, summator, actuating device and mechanical system.

EFFECT: decrease of deviation of actual trajectory of the controlled object from preset one, and decrease of time for execution of preset trajectory.

5 dwg

Description

The invention relates to control systems and can be used in the development of control systems for moving objects, ensuring their movement along a given path with a given speed path, or to a given point along a given path without presenting requirements for the speed path, or to a given point with zero final speed.

A device for trajectory control [P.D. Cool. Management of executive systems of robots. - M.: “Science”, the main edition of the phys.-mat. literature, 1977, 336 p. - p. 308-313], containing a path planner, a block for solving the inverse kinematics problem, an approximation block, an interpolation block, a regulator block, an information sensor block, computational blocks, multipliers and adders. This device allows to provide the asymptotic stability of the control object during the development of the planned trajectories.

The signs of this analogue, common with the claimed device, are a trajectory planner, a block of information sensors, calculators, multipliers and adders.

The reason that impedes the achievement in this analogue of the technical result provided by the invention is the low accuracy of trajectory refinement. It is due to the fact that the procedure of local approximation of complex trajectories introduces significant errors in planning and, therefore, in the development of these trajectories and does not allow to stabilize a given value by the velocity trajectory.

More accurate and closer to the proposed device is the control of a moving object, protected by RF patent No. 2393522, cl. G05D 1/00, 2009. This device includes a path planner, first, second, and third matrix coefficient calculators, a control signal calculator, first and second matrix transpose blocks, an information sensor block, a sensor support unit, a control coefficient matrix generation unit, a generating unit matrices - the derivative of the column vector of external velocities with respect to the row vector of external coordinates, the unit for forming the vector of external velocities and the actuator and mechanical part of the control object my system.

The signs of this device, which coincides with the essential features of the claimed device, are all of the above elements.

The reason that impedes the achievement in this analogue of the technical result provided by the invention is the limited functionality of the device. The fact is that in this device the movement of a controlled object is limited to routes that do not contain obstacles, or contain only fixed and previously known obstacles. This is due to the fact that the device requires preliminary mapping of the area of functioning of the moving object and the calculation of such a trajectory of the controlled object at a given point in space, which would ensure that the obstacle is bypassed and the managed object reaches a predetermined positioning point.

The closest in technical essence to the claimed (prototype) is a device for controlling a movable object, protected by RF patent No. 2450308, class. G05D 1/00, G05B 19/19, 2008. This device contains all the elements that make up the device, protected by RF patent No. 2393522. In addition, it additionally includes an electronic switch, an inverter of a sign for determining the matrix and a threshold device.

Moreover, all of the listed elements of the prototype, except for the electronic switch and inverter of the sign for determining the matrix, are also included in the inventive device.

где $$r_{доп}$$

- минимально допустимая дистанция сближения объекта управления с препятствием, изменяется знак одного из элементов матриц на противоположный.In the prototype device, the distance g between the controlled object and the nearest obstacle in the way of its movement is constantly measured and when the condition $$r\le r_{d\mathrm{about}P},$$

Where $$r_{d\mathrm{about}P}$$

- the minimum allowable distance of approach of the control object with an obstacle, the sign of one of the elements of the matrices changes to the opposite.

то есть до выхода в зону, свободную от препятствий. После выхода в эту зону планировщик траекторий системы управления рассчитывает новую траекторию. В результате устройство формирует новый сигнал управления, обеспечивающий разворот (доворот) объекта управления до направления на целевую точку и движение его по вновь спланированной траектории.In this case, the control object enters the mode of motion unstable in distance to the obstacle until the condition $$r>r_{d\mathrm{about}P},$$

that is, before entering an area free of obstacles. After entering this zone, the trajectory planner of the control system calculates a new trajectory. As a result, the device generates a new control signal, providing a turn (turnaround) of the control object to the direction to the target point and its movement along the newly planned path.

а затем совершается новая попытка обойти препятствие стороной с определенным радиусом разворота. Эти обстоятельства вызывают существенные отклонения траектории объекта управления от заданной траектории и значительные затраты времени на реализацию объектом управления заданной траектории.The reason that impedes the achievement in the prototype device of a technical result provided by the claimed device is the error in the implementation of a given trajectory of the control object in the event of an unexpected obstacle along its route. The fact is that in this case, in the prototype device, the obstacle, in principle, is overcome, but at the same time the control object, in fact, collides with the obstacle, then it again returns to a distance exceeding the value $$r_{d\mathrm{about}P},$$

and then a new attempt is made to bypass the obstacle with a certain turning radius. These circumstances cause significant deviations of the trajectory of the control object from the given trajectory and a significant investment of time on the implementation by the control object of the given trajectory.

The task to which the invention is directed is to reduce the deviation of the actual trajectory of the control object from the given one, and therefore, reduce the time spent on the implementation of the given trajectory.

The technical result is achieved by the fact that in the known device for controlling a movable object, protected by RF patent No. 2450308, an additional meter for changing the range of the angle of sight of the obstacle, a block for calculating the correction of the control signal and an adder are added, and the sixth output of the path planner is connected to the seventh input of the third matrix coefficient calculator and the fourth inputs of the first and second calculators of the matrix coefficient, the first and second outputs of the meter range of the angle of sight of obstacles The actions are connected respectively to the first and second information inputs of the control signal correction calculation block, the second output of the touch support block is connected to the third information input of the control signal correction calculation block, the output of the threshold device is connected to the control input of the control signal correction calculation block, the first and second adder inputs are connected accordingly, with the output of the control signal calculator and the output of the control signal correction calculation unit, and the output with the control input of the executor Nogo device managed object.

To achieve a technical result, a known moving object control device comprising a path planner, first, second and third matrix coefficient calculators, a control signal calculator, first and second matrix transpose blocks, a nonlinear element vector generating unit, a nonlinear element vector generating unit, a matrix generating unit control coefficients, information block sensors, sensor support unit, matrix forming unit - derivative of the column vector of external bones in a row vector of internal coordinates, a matrix formation unit is a derivative of a column vector of external velocities in a row vector, external coordinates, a block of external velocity vector formation, a threshold device, an actuator, and a mechanical system of a controlled object, in which the first, second, third and the fourth outputs of the trajectory scheduler are connected respectively to the first, second, third and fourth inputs of the third matrix coefficient calculator, the fifth output of the trajectory scheduler is connected to by the inputs of the first and second matrix coefficient calculators, the first input of the second matrix coefficient calculator is connected to the second output of the path planner, the second input of the first matrix coefficient calculator is connected to the sixth input of the second matrix coefficient calculator, the input of the first matrix transpose block is connected to the fifth input of the third matrix coefficient calculator , and the output - with its sixth input and fifth input of the second matrix coefficient calculator, the seventh input of the third subtract the matrix coefficient spreader is connected to the fourth inputs of the first and second matrix coefficient calculators, the first, second and third inputs of the control signal calculator are connected respectively to the output of the first, first output of the second and the output of the third matrix coefficient calculators, the fourth and fifth inputs to the outputs of the vector forming unit, respectively nonlinear elements and a block for generating a matrix of control coefficients, the first and fifth inputs of the first calculator of the matrix coefficient are connected to responsibly, with the second and third outputs of the second matrix coefficient calculator, the input of the information sensor block is connected to the output of the actuator of the managed object, the mechanical system of the controlled object is connected directly to the input of the information sensor block, and through the external environment, to the input of the sensor support block, the output of the information sensor block connected to the first input of the trajectory planner and the first inputs of the blocks for forming the vector of nonlinear elements and matrix of control coefficients of matrices of derivatives of the vector column of external velocities and the vector of external velocities, the first output of the sensor support unit is connected to the second inputs of the matrix forming units - derivatives of the vector column of external velocities and the vector of external velocities, the input of the first transpose matrix unit and the second input of the path planner, the second the output of the sensor support unit is connected to the input of the second matrix transpose unit, the output of which is connected to the second input of the first matrix coefficient calculator, the meter of the range of variation of the angle of sight of the obstacle and the distance to it, the block for calculating the correction of the control signal and the adder, the sixth output of the path planner is connected to the seventh input of the third matrix coefficient calculator and the fourth inputs of the first and second matrix coefficient calculators, the first and second outputs of the meter the range of changes in the angle of sight of the obstacle and the distance to it are connected respectively to the first and second information inputs of the pop calculation block control signal avki, the second output of the external velocity vector generation unit is connected to the third information input of the control signal correction calculation unit, the output of the threshold device is connected to the control input of the control signal correction calculation unit, the first and second adder inputs are connected respectively to the output of the control signal calculator and the output of the block calculating the correction of the control signal, and the output with the control input of the actuator of the managed object.

Studies of the claimed technical solution in the patent and scientific and technical literature showed that the totality of the newly introduced meter for changing the range of the angle of sight of the obstacle, the block for calculating the correction of the control signal, the adder and their connections in combination with the other elements and connections of the prototype device cannot be independently classified. At the same time, it does not follow explicitly from the prior art. Therefore, the proposed device should be considered as satisfying the criterion of "novelty" and having an inventive step.

The invention is illustrated in the drawing, which shows:

- figure 1 is a structural diagram of the proposed device in conjunction with those included in the control object of the actuator and the mechanical system;

- figure 2 is a structural diagram of a first calculator matrix coefficient;

- figure 3 is a structural diagram of a second calculator matrix coefficient;

- figure 4 is a structural diagram of a third calculator matrix coefficient;

- figure 5 is a structural diagram of a computer signal control.

The moving object control device comprises a trajectory scheduler 1, a first 2, a second 3 and a third 4 matrix coefficient calculators, a control signal calculator 5, a first 6 and a second 7 matrix transpose blocks, an information sensor unit 8, a sensor support unit 9, a nonlinear vector generating unit 10 elements, a block 11 forming a matrix of control coefficients, a block 12 forming a matrix - a derivative of a column vector of external speeds with respect to a vector row of internal coordinates, a block 13 forming a matrix - column vector of external velocities according to the vector line of external coordinates, block 14 for generating the vector of external velocities, threshold device 15, meter 16 for the range of variation of the angle of sight of the obstacle and the distance to it, block 17 for calculating the correction of the control signal, adder 18, actuator 19, and mechanical system 20.

The first, second, third and fourth outputs of the scheduler 1 are connected respectively to the first, second, third and fourth inputs of the calculator 4. The fifth output of the scheduler 1 is connected to the third inputs of the calculators 2 and 3. The first and second inputs of the calculator 3 are connected respectively to the second and third outputs scheduler 1, the first input of which is connected to the output of block 8 and the first inputs of blocks 10, 11, 12, 13 and 14, and the second to the first output of block 9, the input of block 6, the second inputs of blocks 12, 13 and 14 and the fifth input of the calculator 4. The output of block 6 is connected to the sixth stroke calculator 4 and the fifth calculating 3. Sign input unit 7 is connected to the second output unit 9, and an output - a sixth input of the calculator 3 and the second input of the calculator 2, the first and fifth inputs of which are connected respectively to the second and third outputs of the calculator 3.

The input of the threshold device 15 is connected to the third output of the meter 16.

The first and second outputs of the meter 16 are connected respectively to the first and second signal inputs of block 17, the third signal input of which is connected to the first output of block 9, and the control input is connected to the output of the threshold device 15.

The first input of the adder 18 is connected to the output of the calculator 5 of the control signal, the second is connected to the output of the block 17, and the output is connected to the input of the actuating device included in the controlled object 19. The output of which is connected to the input of block 8 and the input of the mechanical system 20 that is part of the controlled object , which through the external environment 21 is connected to the input of block 9.

The output of the calculator 2, the second output of the calculator 3 and the output of the calculator 4 are connected respectively to the first, second and third inputs of the calculator 5, the fourth, fifth, sixth, seventh and eighth inputs of which are connected to the outputs of blocks 10, 11, 12, 13 and 14, respectively.

The calculator 2 contains a register 22, a two-multiplier block 23, a multiplier 24, an adder 25, a register 26 and a multiplier 27. The output of the register 22 is connected to the first input of the multiplier 24, the second input of which is connected to the output of the block 23 connected to its input with the second input of the calculator 2. The first input of the adder 25 is connected to the output of the multiplier 24, the second input to the fifth input of the calculator 2, and the output to the second input of the register 26, the first input of which is connected to the first input of the calculator 2, and the output to the first input of the multiplier 27. The second and the third inputs lane the multiplier 27 is connected respectively with the third and fourth inputs of the calculator 2, and the output is with its output.

Calculator 3 contains blocks 28 and 29 of multiplication by two, multipliers 30, 31, 32 and 33, register 34 and adders 35, 36 and 37. The first input of calculator 3 is connected to the first inputs of multipliers 30 and 31, the second inputs of which are connected to the outputs of the blocks 28 and 29, respectively, connected by their inputs to the sixth and fifth inputs of the calculator 3, respectively. The first input of the register 34 is connected to the output of the multiplier 30, the second is grounded, and the output is connected to the third input of the multiplier 32, the first input of which is connected to the first input of the adder 36 and the fourth input of the calculator 3, and the output to the second input of the adder 37.

The first input of the adder 35 is connected to the second input and the third output of the calculator 3, the second to the output of the multiplier 31, and the output to the second output of the calculator 3 and the first input of the multiplier 33, the second input of which is connected to the output of the adder 36. The first input of the adder 37 is connected with the output of the multiplier 33, and the output with the first output of the transmitter 3.

The calculator 4 contains multipliers 38, 39, 40 and 41, an adder 42, an inverter 43, a register 44, and a matrix transpose unit 45. The first, second and third inputs of the multiplier 38 are connected respectively to the second, fifth and sixth inputs of the calculator 4, and the output to the first input of the adder 42. The first input of the multiplier 39 is connected to the fifth input of the calculator 4, the second to the third input of the calculator 4, and the output - with the second input of the adder 42. The inputs of the multiplier 40 are connected to the first input of the calculator 4, and the output is connected to the input of the inverter 43. The output of the inverter 43 is connected to the second input of the register 44, the first input of which is grounded, and the output is connected to the input of the matrix transpose unit 45. The first and second inputs of the multiplier 41 are connected respectively to the output of block 45 and the seventh input of the calculator 4, and the output is connected to the fourth input of the adder 42, the third input of which is connected to the fourth input of the calculator 4, and the output is the output of the calculator 4.

The calculator 5 contains multipliers 46, 47, 48, 49 and 50, adders 51 and 52, and a matrix inversion unit 53. The first input of the calculator is connected to the first inputs of the multipliers 46, 47 and 48. The second input of the multiplier 46 is connected to the second input of the multiplier 47 and the sixth input of the calculator 5, the third input is connected to the fifth input of the calculator 5, and the output is connected to the input of block 53. The third input of the multiplier 47 is connected to the fourth input of the calculator 5, and the output is connected to the first inverse input of the adder 52. The second input of the multiplier 48 is connected to the seventh input of the calculator 5, and the output is connected to the first input of the adder 51, the second input of which is connected to the second input of the calculator 5, and the output - from the first the input of the multiplier 50. The second input of the multiplier 50 is connected to the eighth input of the calculator 5, and the output is connected to the second inverse input of the adder 52, the third inverse input of which is connected to the third input of the calculator 5. The first input of the multiplier 49 is connected to the output of block 53, the second to the output of the adder 52, and the output is the output of the transmitter 5.

The operation of the proposed control device is as follows.

Formed by a computer. 5, the control signal C / through the adder 18 is fed to the control input of the device 19.

First, consider the case. When the second input of the adder 18 from the output of block 17 receives a zero correction signal.

The procedure for generating the control signal U will also be described below.

. На втором выходе блока 9 формируется вектор $${\dot{Y}}^{*}$$

производной вектора Y, причем только для тех значений $${\dot{Y}}^{*}$$

вектора Y, которые являются координатами центра тяжести управляемого объекта. Координаты вектора Y с первого выхода блока 9 поступают на второй вход планировщика 1 и на вход блока 6, а те из значений Y^*, которые являются координатами центра тяжести управляемого объекта, поступают кроме того на входы блоков 12, 13, 14 и на пятый вход вычислителя 4.The device 19 and the mechanical system 20 and the sensor unit 8 connected to its output process this signal. Block 8 measures the internal coordinates of the controlled object (steering angle, steering wheels, etc.). At its outputs, a vector Z of internal coordinates of dimension “n” is formed, which is fed to the first input of the trajectory 1 of the trajectories and the inputs of blocks 10, 11, 12, 13 and 14. From the output of the mechanical system 20 through the external environment 21, the results of mining are fed to the input of block 9 sensory support. This unit measures the external coordinates of the controlled object - the coordinates of its center of gravity and the orientation of the body. At the first output of block 9, a vector Y of external coordinates of the managed object of dimension "m" is formed. The dimension m satisfies the condition $$n\le m\le 6$$

. At the second output of block 9, a vector is formed $${\dot{Y}}^{*}$$

derivative of the vector Y, and only for those values $${\dot{Y}}^{*}$$

vector Y, which are the coordinates of the center of gravity of the controlled object. The coordinates of the vector Y from the first output of block 9 go to the second input of the scheduler 1 and to the input of block 6, and those of the values Y ^* , which are the coordinates of the center of gravity of the controlled object, also go to the inputs of blocks 12, 13, 14 and the fifth input calculator 4.

Scheduler 1, which is a control computer, under the influence of control signals Z and Y generates the following control signals at its outputs:

- trajectory (contour) speed V _K of the control object;

- matrices N _1j , N _2j and N _{3j of} quadratic forms from external coordinates, where $$j=\overline{\mathrm{one,}n};$$

 - diagonal matrices C and A of constant coefficients of dimension n × n.

The signal V _K is generated at the first output of the scheduler 1 and comes from there to the first input of the calculator 4. The signal N _1j is generated at the second output of the scheduler 1 and comes from there to the first input of the calculator 3 and to the second input of the calculator 4. The signal N _2j is generated at the third output of the scheduler 1 and fed from there to a second input of the calculator 3 and the third input of the calculator 4. The signal N _3j is formed on the fourth output scheduler 1 and fed from there to a fourth input of the calculator 4. C matrix is formed at the fifth output scheduler 1 and supplied therefrom n third inputs calculators 2 and 3. The matrix A is formed on the sixth output scheduler 1 and supplied therefrom to the fourth calculators inputs 2 and 3 and input 4 seventh calculator.

In block 10, a vector F of non-linear transformation of internal coordinates Z is formed. This vector, a non-linear function, is unique for each specific control object. From the output of block 10, the vector F enters the fourth input of the calculator 5.

Similarly, in block 11, a matrix B of non-linear transformation of internal coordinates Z is formed. It is a matrix of control coefficients and, like the vector F, is specific for each specific control object. From the output of block 11, the matrix B goes to the fifth input of the calculator 5.

In block 12, the matrix R is formed - the derivative of the column vector of external velocities with respect to the row vector of internal coordinates. It is formed as a nonlinear function of not only internal Z, but also external Y coordinates and is also specific for each specific control object. From the output of block 12, the matrix R goes to the sixth input of the calculator 5.

In block 13, the matrix L is similarly formed - the derivative of the column vector of external velocities with respect to the row vector of external coordinates. It, like the matrix R, is formed as a nonlinear function of the Z and Y coordinates and is specific for each specific control object. From the output of block 13, the matrix L goes to the seventh input of the calculator 5.

In block 14, a vector M of external speeds is formed, which is also a nonlinear function of the coordinates Z and Y, specific for each control object. From the output of block 14, the vector M goes to the eighth input of the calculator 5. In addition, at the output of block 15, a signal φ ₃ corresponding to the angle of inclination of the speed vector of the control object is generated.

Block 6 transposes the vector Y of external coordinates received at its input. The transposition result Y ^T from the output of block 6 goes to the fifth input of the calculator 3. A part of the elements of this vector Y ^* , which is the coordinates of the center of gravity of the controlled object, also goes to the sixth input of the calculator 4.

- производной вектора внешних координат, являющихся координатами Центра тяжести управляемого объекта. Результат $${\dot{Y}}^{*T}$$

транспонирования с выхода блока 7 поступает на второй вход вычислителя 2 и шестой вход вычислителя 3.Similarly, block 7 transposes the vector received at its input $${\dot{Y}}^{*}$$

- the derivative of the vector of external coordinates, which are the coordinates of the Center of gravity of the controlled object. Result $${\dot{Y}}^{*T}$$

transposition from the output of block 7 goes to the second input of the calculator 2 and the sixth input of the calculator 3.

The threshold device 15 determines the fulfillment of the condition:

$$r\le r_{d\mathrm{about}P},(\mathrm{one})$$

- минимально допустимая дистанция сближения управляемого объекта с препятствиями.Where $$r_{d\mathrm{about}P}$$

- the minimum allowable distance of approach of a controlled object with obstacles.

Let us first consider the operation of the control device if $$r>r_{d\mathrm{about}P}.$$

In the calculator 3, an auxiliary matrix D _{j is} calculated and a second matrix coefficient K _{2 is formed} .

The matrix D _{j is} calculated by the formula:

$$D_j=2Y^T\cdot N_{\mathrm{one}j}+N_{2j}.(2)$$

The calculation of the matrix D _{j is} carried out using the block 29 multiplication by two, the multiplier 31 and the adder 35.

с первого входа вычислителя поступает на первые входы перемножителей 30 и 31, а матрица $$N_{2j}$$

со второго входа вычислителя поступает на первый вход сумматора 35, кроме того, ее составляющая $$N_{2n}$$

поступает на третий выход вычислителя. Вектор Y^T с пятого входа вычислителя поступает на вход блока 29.Matrix $$N_{\mathrm{one}j}$$

from the first input of the calculator goes to the first inputs of the multipliers 30 and 31, and the matrix $$N_{2j}$$

from the second input of the calculator goes to the first input of the adder 35, in addition, its component $$N_{}$$$${}_{2n}$$

goes to the third output of the calculator. Vector Y ^T from the fifth input of the calculator is fed to the input of block 29.

удвоения поступает на второй вход перемножителя 31. На выходе последнего формируется произведение $$2Y^T\cdot N_{1i},$$

где $$i=j-\mathrm{1,}$$

которое поступает на второй вход сумматора 35, где оно суммируется с матрицей $$N_{2j},$$

поступившей на его первый вход. В результате на выходе сумматора 35 формируется матрица D_j в соответствии с уравнением (2). С выхода сумматора 35 матрица D_j поступает на первый вход перемножителя 33 и на второй выход вычислителя, откуда затем поступает на первый вход вычислителя 2.In block 29, the vector Y ^T arriving at its input is doubled, and the result $$2Y^T$$

doubling goes to the second input of the multiplier 31. At the output of the last product is formed $$2Y^T\cdot N_{\mathrm{one}i},$$

Where $$i=j-\mathrm{one,}$$

which goes to the second input of the adder 35, where it is summed with the matrix $$N_{2j},$$

received at his first entrance. As a result, at the output of the adder 35, a matrix D _{j is formed} in accordance with equation (2). From the output of the adder 35, the matrix D _j goes to the first input of the multiplier 33 and to the second output of the calculator, from where it then goes to the first input of the calculator 2.

The second matrix coefficient K ₂ is formed using the block 28 multiplication by two, multipliers 30, 32 and 33, register 34 and adders 36 and 37.

с шестого входа вычислителя поступает на вход блока 28. Матрица C с третьего входа вычислителя поступает на вторые входы перемножителя 32 и сумматора 36, а матрица A - с четвертого входа вычислителя поступает на первые входы перемножителя 32 и сумматора 36.Vector $${\dot{Y}}^{*T}$$

from the sixth input of the calculator enters the input of block 28. Matrix C from the third input of the calculator enters the second inputs of the multiplier 32 and adder 36, and the matrix A from the fourth input of the calculator enters the first inputs of the multiplier 32 and adder 36.

удваивается, и результат $$2{\dot{Y}}^{*T}$$

удвоения поступает на второй вход перемножителя 30. На выходе перемножителя 30 формируется произведение $$2{\dot{Y}}^{*T}\cdot N_{1i},$$

которое поступает на первый вход регистра 34. Поскольку второй вход регистра 34 заземлен, на его выходе формируется матрица $$\left[\begin{array}{l}2{\dot{Y}}^{*T}\cdot N_{1i}\\ 0_1\end{array}\right],$$

которая поступает на третий вход перемножителя 32.In block 28, the vector received at its input $${\dot{Y}}^{*T}$$

doubles and the result $$2{\dot{Y}}^{*T}$$

doubling enters the second input of the multiplier 30. At the output of the multiplier 30, the product is formed $$2{\dot{Y}}^{*T}\cdot N_{\mathrm{one}i},$$

which goes to the first input of the register 34. Since the second input of the register 34 is grounded, a matrix is formed at its output $$\left[\begin{array}{l}2{\dot{Y}}^{*T}\cdot N_{\mathrm{one}i}\\ 0_{\mathrm{one}}\end{array}\right],$$

which goes to the third input of the multiplier 32.

суммирования с выхода сумматора 36 поступает на второй вход перемножителя 33, где он умножается на матрицу D_j, поступившую на его первый вход. Результат $$[C+A]\cdot [D_j]$$

умножения, представляющий собой первое слагаемое матричного коэффициента K₂, поступает на первый вход сумматора 37.In adder 36, the matrices A and C received at its inputs are summed, and the result $$[C+A]$$

summation from the output of the adder 36 is fed to the second input of the multiplier 33, where it is multiplied by the matrix D _j received at its first input. Result $$[C+A]\cdot [D_j]$$

multiplication, which is the first term of the matrix coefficient K ₂ , is fed to the first input of the adder 37.

перемножения, представляющий собой второе слагаемое матричного коэффициента K₂, поступает на второй вход сумматора 37.In the multiplier 32, the matrices received at its input are multiplied, and the result $$C\cdot A\left[\begin{array}{l}2{\dot{Y}}^{*T}\cdot N_{\mathrm{one}i}\\ 0_{\mathrm{one}}\end{array}\right]$$

multiplication, which is the second term of the matrix coefficient K ₂ , is fed to the second input of the adder 37.

In the adder 37, the matrices received at its inputs are summed, and the summation result is formed at its output — the matrix coefficient K ₂ in accordance with the equation:

$${\mathrm{<figure-callout\; id="2"\; label="K"\; filenames="00000074.png"\; state="\{\{state\}\}">K</figure-callout>}}_2=[C+A][D_j]+C\cdot A\left[\begin{array}{l}2{\dot{Y}}^{*T}\cdot N_{\mathrm{one}i}\\ 0_{\mathrm{one}}\end{array}\right].(3)$$

This result goes to the first output of the calculator.

Thus, in the calculator 3 are formed:

- at the first output, the second matrix coefficient K ₂ in accordance with equation (3);

- at the second output, the auxiliary matrix D _i in accordance with equation (2);

- at the third output, a matrix N _{2n of a} quadratic form from external coordinates.

- с третьего выхода вычислителя 3 на пятый вход вычислителя 2.The matrix coefficient K ₂ comes from the first output of the calculator 3 to the second input of the calculator 5, the matrix D _i comes from the second output of the calculator 3 to the first input of the calculator 2, and the matrix $$N_{}$$$${}_{2n}$$

- from the third output of the calculator 3 to the fifth input of the calculator 2.

In the calculator 2, the first matrix coefficient K _{1 is formed} .

со второго входа вычислителя поступает на вход блока 23 умножения на два. В блоке 23 он удваивается, и результат $$2{\dot{Y}}^{*T}$$

удвоения поступает на второй вход перемножителя 24. Регистр 22 представляет собой единичную матрицу E размерностью m×m. Его содержимое поступает на первый вход перемножителя 25. На выходе перемножителя 24 формируется произведение $$2{\dot{Y}}^{*T}\cdot E$$

, которое поступает на первый вход сумматора 25, на второй вход которого поступает матрица $$N_{2n}$$

с пятого входа вычислителя. На выходе сумматора 25 формируется сумма матриц на его входах, равная $$[2{\dot{Y}}^{*T}\cdot E+N_{2n}].$$

С выхода сумматора 25 она поступает на второй вход регистра 26, на первый вход которого поступает матрица D_i с первого входа вычислителя. В результате в регистре 26 формируется матрица $$\left[\begin{array}{l}{D}_i\\ N_{2n}+2{\dot{Y}}^{*T}\cdot E\end{array}\right],$$

которая с его выхода поступает на первый вход перемножителя 27. На второй и третий входы перемножителя 27 поступают матрицы C и A соответственно с третьего и четвертого входов вычислителя. На выходе перемножителя 27 и выходе вычислителя формируется результат перемножения - матричный коэффициент K₁ в соответствии с уравнением:Vector $${\dot{Y}}^{*T}$$

 from the second input of the calculator enters the input of the block 23 multiplication by two. In block 23, it doubles, and the result $$2{\dot{Y}}^{*T}$$

 doubling arrives at the second input of the multiplier 24. Register 22 is a unit matrix E of dimension m × m. Its content goes to the first input of the multiplier 25. At the output of the multiplier 24, the product is formed $$2{\dot{Y}}^{*T}\cdot E$$

, which is fed to the first input of adder 25, to the second input of which the matrix $$N_{}$$$${}_{2n}$$

 from the fifth input of the calculator. The output of the adder 25 is formed by the sum of the matrices at its inputs, equal to $$[2{\dot{Y}}^{*T}\cdot E+{\mathrm{<figure-callout\; id="2n"\; label="N"\; filenames="00000074.png"\; state="\{\{state\}\}">N</figure-callout>}}_{2n}].$$

 From the output of the adder 25, it enters the second input of the register 26, the first input of which receives the matrix D_i from the first input of the calculator. As a result, a matrix is formed in register 26 $$\left[\begin{array}{l}{\mathrm{<figure-callout\; id="2n"\; label="D\; i\; N"\; filenames="00000074.png"\; state="\{\{state\}\}">D\; </figure-callout>}}_i& \\ N_{}{}_{2n}+2{\dot{Y}}^{*T}\cdot E\end{array}\right],$$

 which, from its output, goes to the first input of the multiplier 27. Matrices C and A, respectively, from the third and fourth inputs of the calculator, go to the second and third inputs of the multiplier 27. At the output of the multiplier 27 and the output of the calculator, the result of multiplication is formed - the matrix coefficient K_one according to the equation:

$$K_{\mathrm{one}}=C\cdot A\left[\begin{array}{l}{D}_i\\ {\mathrm{<figure-callout\; id="2n"\; label="N"\; filenames="00000074.png"\; state="\{\{state\}\}">N</figure-callout>}}_{2n}+2{\dot{Y}}^{*T}\cdot E\end{array}\right].\text{(four)}$$

From the output of the calculator 2, the matrix coefficient K _{1 is} supplied to the first input of the calculator 5.

In the calculator 4, a third matrix coefficient K _{3 is formed} .

со второго входа вычислителя, на второй - вектор Y с пятого входа вычислителя, а на третий - результат $$Y^{*T}$$

транспонирования этого вектора с шестого входа вычислителя. На выходе перемножителя 38 формируется матрица $${\dot{Y}}^{*T}\cdot N_{1j}\cdot Y^{*}$$

- результат перемножения матриц на его входах. Сформированная матрица с выхода перемножителя 38 поступает на первый вход сумматора 42. На первый вход перемножителя 39 поступает вектор $${\dot{Y}}^{*}$$

с пятого входа вычислителя, а на второй - матрица $$N_{2j}$$

его третьего входа. На выходе перемножителя 39 формируется матрица $$N_{2j}\cdot Y^{*}$$

- результат перемножения матриц на его входах. Сформированная матрица с выхода перемножителя 39 поступает на второй вход сумматора 42, на третий вход которого поступает матрица $$N_{3j}$$

квадратичных форм с четвертого входа вычислителя. На оба входа перемножителя 40 с первого входа вычислителя поступает заданное значение V_к траекторией скорости. Перемножитель 40 возводит его в квадрат, результат $$V_{к}^2$$

инвертируется инвертором 43, и результат $$(-V_{к}^2)$$

инвертирования поступает на второй вход регистра 44. Поскольку первый вход регистра 44 заземлен, на его выходе формируется матрица вида $$[0_1-V_{к}^2],$$

которая поступает на вход блока 45 транспонирования матриц. На выходе блока 45 формируется матрица $${[0_1-V_{к}^2]}^T,$$

которая поступает на первый вход перемножителя 41. На второй вход этого перемножителя поступает матрица A с седьмого входа вычислителя. На выходе перемножителя 41 и на четвертом входе сумматора 42 формируется матрица $$A\cdot {[0_1-V_{к}^2]}^T,$$

равная произведению матриц на его входах. На выходе сумматора 39 и выходе вычислителя формируется матрица, равная сумме матриц на его четырех входах. Она представляет собой третий матричный коэффициент K₃ и определяется уравнением:At the first input of the multiplier 38 receives the matrix $$N_{\mathrm{one}j}$$

from the second input of the calculator, on the second - the vector Y from the fifth input of the calculator, and on the third - the result $$Y^{*T}$$

transpose this vector from the sixth input of the calculator. At the output of the multiplier 38, a matrix is formed $${\dot{Y}}^{*T}\cdot N_{\mathrm{one}j}\cdot Y^{*}$$

- the result of the multiplication of matrices at its inputs. The formed matrix from the output of the multiplier 38 goes to the first input of the adder 42. At the first input of the multiplier 39 the vector $${\dot{Y}}^{*}$$

from the fifth input of the calculator, and at the second - the matrix $$N_{2j}$$

his third entrance. At the output of the multiplier 39, a matrix is formed $$N_{2j}\cdot Y^{*}$$

- the result of the multiplication of matrices at its inputs. The formed matrix from the output of the multiplier 39 enters the second input of the adder 42, the third input of which receives the matrix $$N_{3j}$$

quadratic forms with the fourth input of the calculator. Both inputs of the multiplier 40 from the first input of the calculator receives the specified value of V _to the velocity path. Multiplier 40 squares it, the result $$V_{\mathrm{to}}^2$$

inverted by inverter 43 and the result $$(-V_{\mathrm{to}}^2)$$

inversion is fed to the second input of the register 44. Since the first input of the register 44 is grounded, a matrix of the form is formed at its output $$[0_{\mathrm{one}}-V_{\mathrm{to}}^2],$$

which is fed to the input of the matrix transpose unit 45. At the output of block 45, a matrix is formed $${[0_{\mathrm{one}}-V_{\mathrm{to}}^2]}^T,$$

which goes to the first input of the multiplier 41. The second input of this multiplier receives the matrix A from the seventh input of the calculator. At the output of the multiplier 41 and at the fourth input of the adder 42, a matrix is formed $$A\cdot {[0_{\mathrm{one}}-V_{\mathrm{to}}^2]}^T,$$

equal to the product of the matrices at its inputs. At the output of the adder 39 and the output of the calculator, a matrix is formed equal to the sum of the matrices at its four inputs. It represents the third matrix coefficient K ₃ and is determined by the equation:

$${\mathrm{<figure-callout\; id="3"\; label="K"\; filenames="00000074.png"\; state="\{\{state\}\}">K</figure-callout>}}_3=[Y^{*T}\cdot N_{\mathrm{one}j}\cdot Y^{*}+N_{2j}\cdot Y^{*}+N_{3j}]+A\cdot {[0_{\mathrm{one}}-V_{\mathrm{to}}^2]}^T.(5)$$

From the output of the calculator 4, the matrix coefficient K _{3 is} supplied to the third input of the calculator 5.

In the calculator 5, a control vector U is formed.

- результат перемножения матриц на его входах. С выхода перемножителя 46 результат перемножения поступает на вход блока 53 обращения матриц. На выходе блока 53 формируется обращенная матрица $${[K_1\cdot R\cdot B]}^{-1},$$

которая поступает на первый вход перемножителя 49. На выходе перемножителя 47 формируется матрица $$K_1\cdot R\cdot F$$

- результат перемножения матриц на его входах, которая поступает на первый инверсный вход сумматора 52. На выходе перемножителя 48 и первом входе сумматора 51 формируется матрица $$K_1\cdot L$$

- результат перемножения матриц на его входах. Второй матричный коэффициент K₂ со второго входа вычислителя поступает на второй вход сумматора 51, где он суммируется с матрицей $$K_1\cdot L,$$

а результат $$[K_1\cdot L+K_2]$$

суммирования поступает на первый вход перемножителя 50. На второй вход этого перемножителя поступает вектор M внешних скоростей с восьмого входа вычислителя. На выходе перемножителя 50 формируется матрица $$[K_1\cdot L+K_2]\cdot M]$$

- результат перемножения матриц на его входах, которая поступает на второй инверсный вход сумматора 52. На третий вход этого сумматора с третьего входа вычислителя поступает третий матричный коэффициент K₃. На выходе сумматора 52 и на втором входе перемножителя 49 формируется матрица $$[-K_3-K_1\cdot R\cdot F-[K_1\cdot L+K_2]\cdot M]$$

- проинвертированная сумма матриц на входах сумматора 52. На выходе перемножителя 49 и выходе вычислителя формируется результат перемножения U - вектор управления в соответствии с уравнением:The first inputs of the multipliers 46, 47 and 48 from the first input of the calculator receive the first matrix coefficient K ₁ , the second inputs of the multipliers 46 and 47 receive the matrix R from the sixth input of the calculator, the second input of the multiplier 48 is the matrix L from the seventh input of the calculator, and the third inputs of the multipliers 46 and 47 are the matrix B and the vector F from the fifth and fourth inputs of the calculator, respectively. At the output of the multiplier 46, a matrix is formed $$K_{\mathrm{one}}\cdot R\cdot B$$

- the result of the multiplication of matrices at its inputs. From the output of the multiplier 46, the result of the multiplication is fed to the input of the matrix inversion block 53. At the output of block 53, an inverse matrix is formed $${[K_{\mathrm{one}}\cdot R\cdot B]}^{-\mathrm{one}},$$

which is fed to the first input of the multiplier 49. At the output of the multiplier 47, a matrix is formed $$K_{\mathrm{one}}\cdot R\cdot F$$

- the result of the multiplication of matrices at its inputs, which is fed to the first inverse input of the adder 52. At the output of the multiplier 48 and the first input of the adder 51, a matrix is formed $$K_{\mathrm{one}}\cdot L$$

- the result of the multiplication of matrices at its inputs. The second matrix coefficient K ₂ from the second input of the calculator is fed to the second input of the adder 51, where it is summed with the matrix $$K_{\mathrm{one}}\cdot L,$$

and the result $$[K_{\mathrm{one}}\cdot L+K_2]$$

summation is fed to the first input of the multiplier 50. At the second input of this multiplier, a vector M of external velocities is received from the eighth input of the calculator. At the output of the multiplier 50, a matrix is formed $$[K_{\mathrm{one}}\cdot L+K_2]\cdot M]$$

- the result of the multiplication of matrices at its inputs, which is fed to the second inverse input of the adder 52. The third matrix coefficient K ₃ is supplied to the third input of this adder from the third input of the calculator. At the output of the adder 52 and at the second input of the multiplier 49, a matrix is formed $$[-K_3-K_{\mathrm{one}}\cdot R\cdot F-[K_{\mathrm{one}}\cdot L+K_2]\cdot M]$$

- the inverted sum of the matrices at the inputs of the adder 52. At the output of the multiplier 49 and the output of the calculator, the result of the multiplication U is formed - the control vector in accordance with the equation:

$$U=-{[K_{\mathrm{one}}\cdot L+K_2]}^{-\mathrm{one}}\cdot [K_{\mathrm{one}}\cdot R\cdot F+[K_{\mathrm{one}}\cdot L+K_2]\cdot M+K_3].(6)$$

From the output of the computer control (control signal) U is supplied to the control input of the actuator 19.

заявляемое устройство реализует следующий алгоритм формирования управляющего сигнала U:Thus, under the condition $$r>r_{d\mathrm{about}P}$$

The claimed device implements the following algorithm for generating a control signal U:

- measurement of the internal coordinates Z of the managed object;

- measurement of its external coordinates Y and their derivatives $$\dot{Y};$$

квадратичных форм, первой A и второй C диагональных матриц постоянных коэффициентов и вспомогательной матрицы D в соответствии с уравнением:- matrix formation $$N_{\mathrm{one}j},N_{2j},N_{3j}$$

quadratic forms, the first A and second C diagonal matrices of constant coefficients and an auxiliary matrix D in accordance with the equation:

$$D_j=2Y^T\cdot N_{\mathrm{one}j}+N_{2j}.$$

- the formation of a vector M of external velocities, a vector F and a matrix B of non-linear transformation of internal coordinates, matrices - derivatives of R and L of a column vector of external velocities;

- the formation of matrix coefficients K ₁ , K ₂ and K ₃ in accordance with equations (3), (4) and (5), respectively;

- the formation of the control vector U in accordance with equation (6). The described algorithm is fully consistent with the control algorithm described in [V.Kh. Pshikhopov "Analytical synthesis of synergetic regulators for position-trajectory control systems for mobile robots." Materials of the XI scientific and technical conference "Extreme Robotics". Under the scientific editorship of prof. E.I. Yurevich. SPb., Publishing house SPbSTU, 2000]. It corresponds to the movement of a mobile (moving) object along a given trajectory.

поправки к сигналу управления. Этот сигнал, суммируясь в сумматоре 18 с сигналом U управления, поступает в качестве управляющего на вход управления устройства 19.In the event that one or more obstacles appear on the route of the managed object near this object, condition (1) starts to be satisfied, the threshold device 15 generates a signal at its output and at the control input of block 17, under which block 17 generates a signal $$\Delta U$$

corrections to the control signal. This signal, summing up in the adder 18 with the control signal U, enters as a control to the control input of the device 19.

поправки формируется с помощью измерителя 16 диапазона изменения угла визирования препятствия и блока 17 формирования сигнала поправки к сигналу управления. Измеритель 16 формирует на своих первом и втором выходах сигналы $${\varphi }_1$$

и $${\varphi }_2$$

, соответствующие максимальному и минимальному углам визирования препятствия с управляемого объекта. Эти сигналы поступают соответственно на первый и второй информационные входы блока 17. На его третий информационный вход с выхода блока 14 поступает сигнал $${\varphi }_3,$$

соответствующий углу наклона вектора скорости объекта управления. Сигнал $$\Delta U$$

поправки формируется блоком 17 по команде, поступающей из порогового устройства 15 на управляющий вход, в соответствии с уравнением:Signal $$\Delta U$$

correction is formed using the meter 16 of the range of variation of the angle of sight of the obstacle and the block 17 of the formation of the correction signal to the control signal. The meter 16 generates at its first and second outputs signals $${\varphi }_{\mathrm{one}}$$

and $${\mathrm{<figure-callout\; id="2"\; label="\phi "\; filenames="00000074.png"\; state="\{\{state\}\}">\varphi </figure-callout>}}_2$$

corresponding to the maximum and minimum angles of sight of an obstacle from a managed object. These signals are received respectively at the first and second information inputs of block 17. At its third information input from the output of block 14, a signal $${\varphi }_{}$$$${}_3,$$

corresponding to the slope of the velocity vector of the control object. Signal $$\Delta U$$

the correction is generated by block 17 by a command coming from the threshold device 15 to the control input, in accordance with the equation:

$$\Delta U=\{\begin{array}{l}0e\mathrm{from}l\mathrm{and}{\mathrm{<figure-callout\; id="3"\; label="\phi "\; filenames="00000074.png"\; state="\{\{state\}\}">\varphi </figure-callout>}}_3>{\varphi }_{\mathrm{one}}\mathrm{and}l\mathrm{and}{\mathrm{<figure-callout\; id="3"\; label="\phi "\; filenames="00000074.png"\; state="\{\{state\}\}">\varphi </figure-callout>}}_3<{\varphi }_2\\ \delta ,e\mathrm{from}l\mathrm{and}|\; |\; |{\varphi }_3-{\mathrm{<figure-callout\; id="2"\; label="\phi "\; filenames="00000074.png"\; state="\{\{state\}\}">\varphi </figure-callout>}}_2|\; |\; |<|\; |\; |{\varphi }_{\mathrm{one}}-{\mathrm{<figure-callout\; id="3"\; label="\phi "\; filenames="00000074.png"\; state="\{\{state\}\}">\varphi </figure-callout>}}_3|\; |\; |\\ -\delta ,e\mathrm{from}l\mathrm{and}|\; |\; |{\varphi }_3-{\mathrm{<figure-callout\; id="2"\; label="\phi "\; filenames="00000074.png"\; state="\{\{state\}\}">\varphi </figure-callout>}}_2|\; |\; |>|\; |\; |{\varphi }_{\mathrm{one}}-{\mathrm{<figure-callout\; id="3"\; label="\phi "\; filenames="00000074.png"\; state="\{\{state\}\}">\varphi </figure-callout>}}_3|\; |\; |\end{array},$$

- сигнал фиксированного уровня положительной полярности, обеспечивающий максимальную скорость «отворота» объекта управления от столкновения с препятствием.Where $$\delta$$

- a signal of a fixed level of positive polarity, providing the maximum speed of the “lapel” of the control object from a collision with an obstacle.

или $${\varphi }_3<{\varphi }_2,$$

что исключает встречу объекта управления с препятствием и обеспечивает его дальнейшее движение с минимальным отклонением от заданной траектории.As a result of “processing” the correction signal, the condition $${\varphi }_{}$$$${}_3>{\varphi }_{\mathrm{one}}$$

or $${\mathrm{<figure-callout\; id="3"\; label="\phi "\; filenames="00000074.png"\; state="\{\{state\}\}">\varphi </figure-callout>}}_3<{\varphi }_2,$$

which eliminates the meeting of the control object with an obstacle and ensures its further movement with minimal deviation from a given trajectory.

Thus, the proposed control device allows, in contrast to the prototype, to completely eliminate the collision with an unplanned obstacle and significantly reduce the deviation of the actual trajectory of the control object from the given one, and consequently, the reduction of time spent on its implementation.

The proposed device is quite easy to implement.

The coordinator of the active radar Doppler homing head, protected by RF patent No. 2313054, cl., Can serve as a measure of the range of angles of sight of the nearest obstacle in the horizontal and vertical planes and the distance r to it. F41G 7/22, 2006, realizing high resolution of targets and the accuracy of determining their range and angular coordinates.

As the remaining elements for the implementation of the proposed device can serve the same elements as the corresponding elements of the prototype device.

Claims (1)

A moving object control device comprising a path planner, first, second and third matrix coefficient calculators, a control signal calculator, first and second matrix transpose blocks, a nonlinear element vector generating unit, a nonlinear element vector generating unit, a control coefficient matrix generating unit, an information sensor unit , sensor support unit, matrix forming unit - derivative of the column vector of external velocities with respect to the vector row of internal coordinates, matrix formation — a derivative of the column vector of external velocities with respect to the row vector of external coordinates, a block for generating a vector of external velocities, a threshold device, an actuator, and a mechanical system of a controlled object, in which the first, second, third, and fourth outputs of the path planner are connected respectively to the first, the second, third and fourth inputs of the third matrix coefficients calculator, the fifth output of the path planner is connected to the third inputs of the first and second matrix calculators coefficients, the first input of the second matrix coefficient calculator is connected to the second output of the path planner, the second input of the first matrix coefficient calculator is connected to the sixth input of the second matrix coefficient calculator, the input of the first matrix transpose block is connected to the fifth input of the third matrix coefficient calculator, and the output to its the sixth input and the fifth input of the second matrix coefficient calculator, the seventh input of the third matrix coefficient calculator is connected from the fourth and the inputs of the first and second calculators of matrix coefficients, the first, second and third inputs of the calculator of the control signal are connected respectively to the output of the first, first output of the second and the output of the third calculator of matrix coefficients, the fourth and fifth inputs are respectively the outputs of the non-linear element vector generating unit and the generating unit matrices of control coefficients, the first and fifth inputs of the first calculator of the matrix coefficient are connected respectively to the second and third outputs of the second the matrix coefficient numerator, the input of the sensor unit of the information is connected to the output of the actuator of the controlled object, the mechanical system of the controlled object is connected directly to the input of the block of information sensors, and through the external environment to the input of the sensor support block, the output of the block of information sensors is connected to the first input of the trajectory planner and the first inputs of the blocks for the formation of a vector of nonlinear elements and a matrix of control coefficients, matrices of derivatives of the vector column external soon external velocity and vector, the first output of the sensor support unit is connected to the second inputs of the matrix forming units - derivatives of the column vector of external speeds and the vector of external speeds, the input of the first transpose matrix and the second input of the path planner, the second output of the sensor support unit is connected to the input of the second the matrix transpose block, the output of which is connected to the second input of the first matrix coefficient calculator, the first matrix coefficient calculator contains two register, two multipliers, a two-multiplier block and an adder, the first input of the first register is connected to the first input of the calculator, the second to the output of the adder, and the output to the first input of the second multiplier, the input of the two-multiplier block is connected to the second input of the calculator, and the output - with the second input of the first multiplier connected by its first input to the output of the second register, and the output with the first input of the adder, the second input of which is the fifth input of the calculator, the second and third inputs of the second multiplier are connected respectively with the third and fourth inputs of the calculator, and the output is the output of the calculator, the calculator of the second matrix coefficient contains two blocks of multiplication by two, a register, four multipliers and three adders, the first inputs of the first and second multipliers are connected to the first input of the calculator, and the second to the outputs respectively, of the first and second blocks of multiplication by two, connected by their inputs respectively to the sixth and fifth inputs of the calculator, the first input of the register is connected to the output of the first multiplier, the second is grounded, and in the output is connected to the third input of the third multiplier, the first input of which is connected to the first input of the first adder and the fourth input of the calculator, the second to the second input of the first adder and the third input of the calculator, and the output to the second input of the third adder, the first input of the second adder is connected to the second the input and the third output of the calculator, the second with the output of the second multiplier, and the output with the second output of the calculator and the first input of the fourth multiplier, the second input of the fourth multiplier is connected to the output of the first the first adder, and the output with the first input of the third adder, the output of which is the first output of the calculator, the third matrix coefficient calculator contains four multipliers, an adder, an inverter, a register and a matrix transpose block, the first and second inputs of the first multiplier are connected to the first input of the calculator, and output - with the input of the inverter connected to the second input of the register by its output, the first input of which is grounded, the first, second and third inputs of the second multiplier are connected respectively to the second, fifth and sixth inputs of the calculator, and the output with the first input of the adder, the first input of the third multiplier is connected to the fifth input of the calculator, the second is with its third input, and the output is with the second input of the adder, the third input of which is connected to the fourth input of the calculator, the input of the transpose matrix connected to the output of the register, and the output to the first input of the fourth multiplier, the second input of which is connected to the seventh input of the calculator, the fourth input of the adder is connected to the output of the fourth multiplier, and the output is the output of the calculator amplifier, the control signal computer contains five multipliers, two adders and a matrix inversion unit, the first inputs of the first, second and third multipliers are connected to the first input of the calculator, the second input of the first multiplier is connected to the second input of the second and sixth input of the calculator, the third input to the fifth input the calculator, and the output is with the input of the matrix inversion unit, the third input of the second multiplier is connected to the fourth input of the calculator, and the output is with the first inverse input of the second adder, the second input of the third the resident is connected to the seventh input of the calculator, and the output to the first input of the first adder, the second input of which is connected to the second input of the calculator, and the output to the first input of the fifth multiplier, the second input of the fifth multiplier is connected to the eighth input of the calculator, and the output to the second inverse the input of the second adder, the third inverse input of which is connected to the third input of the calculator, the first input of the fourth multiplier is connected to the output of the matrix reversal unit, the second to the output of the second adder, and the output is the output of the calculator alternator, characterized in that it additionally includes a meter for changing the range of the angle of sight of the obstacle and the distance to it, a block for calculating the correction of the control signal and an adder, while the sixth output of the path planner is connected to the seventh input of the third matrix coefficient calculator and the fourth inputs of the first and second calculators matrix coefficient, the first and second outputs of the meter range of the angle of sight of the obstacle and the distance to it are connected respectively with the first and second and the formation inputs of the control signal correction calculation unit, the second output of the external speed generating unit is connected to the third information input of the control signal correction calculation unit, the output of the threshold device is connected to the control input of the control signal correction calculation unit, the first and second adder inputs are connected respectively to the output of the control signal calculator and the output of the control signal correction calculation unit, and the output with the control input of the actuator of the controlled object.

Images