RU2185279C1 - Apparatus for controlling position and motion path of mobile robot - Google Patents

Abstract

FIELD: robotics, particularly, development of control systems for providing motion of mobile robots and microrobots along predetermined path. SUBSTANCE: apparatus includes mechanical system of robot; actuating devices; unit of pickups for transmitting inner data; sensor subsystem; unit for planning motions in space of outer coordinates; unit for calculating vector of non-linear elements; unit for calculating matrix of control coefficients; unit for calculating derivative of column-vector of outer speeds according to row-vector of outer coordinate; unit for calculating derivative of column-vector of outer speeds according to row-vector of inner coordinate; unit for calculating vector of outer speeds; three units for multiplying by two; sixteenth multiplying units; twelve adders; unit for transforming matrix; three units for transposing matrices. EFFECT: enhanced accuracy. 4 dwg

Description

 The invention relates to robotics and can be used in the development of control systems for mobile and micro-robots, as well as nonholonomic objects, ensuring their movement from point to point, along a given path with a given path (path) speed, or to a given point along a desired path without presenting additional trajectory speed requirements.

 The general procedure for designing control systems for a mobile robot (MR) involves the organization of its targeted movement and is reduced to the operational solution of at least three tasks: adequate perception of information about the external environment and internal state; planning the optimal trajectory of movement on the plan of the environment in the current external situation; the robot’s effective execution of planned actions in a real environment [Kalyaev A.V., Noskov V.P., Chernukhin Yu.V. The algorithm of the control structure of the transport robot. // Proceedings of the USSR Academy of Sciences. Technical cybernetics. - 1980. - 4, p. 64-72]. Thus, the structural organization of a mobile robot implies the presence of a sensor subsystem, including external information sensors (laser rangefinders, navigation systems, television cameras, sonars, etc.); block of sensors of internal information (angle sensors of actuators, angular velocity sensors of wheels, axles, etc.); a block for planning movements in the space of external coordinates in accordance with the selected criterion (neural networks, decision-making systems, knowledge systems, programmed trajectories, etc.); the mechanical system of the robot (trolley, glider, underwater vehicle, etc.) that implements its load-bearing properties; executive devices-working bodies (engines with different operating principles, pushers, etc.) that move the mechanical system and influence the external environment and the tactical control unit (hard control level), which ensures the development of planned trajectories using synthesized control algorithms [Amosov N.M. and others. Neurocomputers and intelligent robots. Ed. N. M. Amosova, Institute of Cybernetics, Academy of Sciences of the Ukrainian SSR. Kiev, Naukova Dumka, 1991, 272 p.].

 In what follows, the set of coordinates that uniquely describes the position of the mobile robot in the space of external (connected, basic, inertial, absolute) coordinates (the position of the center of gravity of the mobile robot in the external coordinate system, the orientation of the axes of the associated coordinate system relative to the axes of the external system) will be called the external vector y coordinates and assume the measurability of all its elements, and a set of directly controlled coordinates (driving wheel rotation speed, driven wheel rotation angle, position Tel'nykh mechanisms, etc.) will be called internal coordinates of the vector z and also assumed measurability of all its elements.

 The known kinematic control algorithm for mobile robots and the corresponding structural diagram [L.E. Aguilar, P. Soueres, M. Courdesses, S. Fleury "Robust Path-Following Control with Exponential Stability for Mobile Robots". Proc. of the IEEE Inter. Confer, on Robotics and Automation. Leuven, Belgium, May 1998, p.p. 3279-3283], which assumes the presence of a block for planning the movement of the robot in the space of external coordinates, a block for determining the sign of linear velocity, a block for calculating trigonometric functions, two summation blocks, and three multiplication blocks.

 A control device that implements the presented algorithm allows organizing the exponentially stable movement of a mobile robot along a given trajectory without imposing additional requirements on the linear velocity of movement and without specifying a positioning point.

 Signs of an analogue common with the claimed technical solution are as follows: a mechanical system of a robot, actuators, a block of sensors of internal information, a sensory subsystem, a block of planning movements in the space of external coordinates.

 The reasons hindering the achievement of the required technical result are as follows: the obtained algorithmic solutions do not allow organizing movement along a wide range of trajectories with a given trajectory (contour, linear) speed and moving the robot along a given trajectory to a given positioning point. The proposed control algorithms do not take into account the influence of dynamic effects on the quality of working out given trajectories. That is, on the one hand, the proposed organization of movements narrows the functionality of the robot, and on the other hand, it does not work out the additional dynamic error determined by the inertial properties of the robot.

 A known control algorithm for a mobile robot with an automobile design of the trolley [M. Egersted, X. Hu, A. Stotsky. "Control of a Car-Like Robot Using a Dynamic Model." Proc. of the IEEE Inter. Conffer. on Robotics and Automation. Leuven, Belgium, May 1998, p.p. 3273-3278], which assumes the presence of a block for planning displacements in the space of external coordinates, a block of internal information sensors, a sensor subsystem, a block for calculating trigonometric functions, a summing block, and a multiplication block.

 The proposed algorithm allows to take into account the dynamic properties of a mobile robot and, therefore, allows to increase the accuracy of the trajectories being worked out without presenting additional requirements for the trajectory speed.

 Signs of an analogue common with the claimed technical solution are as follows: a mechanical system of a robot, actuators, a block of sensors of internal information, a sensory subsystem, a block of planning movements in the space of external coordinates.

 The reasons hindering the achievement of the required technical result are as follows: the obtained algorithmic solutions do not allow organizing movement along a wide range of trajectories with a given trajectory (contour, linear) speed and moving the robot along a given trajectory to a given positioning point.

 Known exponential control algorithm [O.J. Sordalen, C. Canudas de Wit. "Exponential Control Low for a Mobile Robot: Extension to Path Following." IEEE Transaction on Robotics and Automation, vol. 9, 6, 1993, p. p. 837-842], which assumes the presence of a block for planning displacements in the space of external coordinates, a block of sensors for internal information, a sensor subsystem, blocks for calculating trigonometric functions, summation blocks, and multiplication blocks.

 The proposed algorithm allows you to organize the movement of a mobile robot along trajectories represented by segments of lines and arcs of circles, with a given trajectory speed.

 Signs of an analogue common with the claimed technical solution are as follows: a mechanical system of a robot, actuators, a block of sensors of internal information, a sensory subsystem, a block of planning movements in the space of external coordinates.

 The reasons that impede the achievement of the required technical result are as follows: the obtained algorithmic solutions do not allow to organize movement along a wide range of trajectories and do not take into account the influence of dynamic effects on the quality of working out given trajectories. The proposed organization of movements narrows the functionality of the robot and does not allow it to work out an additional dynamic error determined by the inertial properties of the robot. In addition, the procedure for approximating complex motion paths by line segments and circular arcs introduces additional errors into the planning and, accordingly, into the development of these paths.

 The known structure of a control device for a mobile robot [S.F. Burdakov, R.E. Stelmakov, S.V. Steinle. Trajectory synthesis and control of mobile robots in the face of uncertainty. Materials of the 8th scientific and technical conference "Extreme Robotics". Under the scientific editorship of prof. E.I. Yurevich. SPb, SPbSTU, 1997, 439 p., S.p. 198-209], which assumes the presence of a block for planning displacements in the space of external coordinates, a block of internal information sensors, a sensor subsystem, an approximation block (trajectory laying subsystem), a position controller, a speed controller, an adaptation block, a restriction block, a kinematic transformation block, and comparison blocks calculation blocks.

 The proposed control device structure, built on the basis of tracking systems, allows you to take into account the dynamic effects of the control object, as well as organize movement along a given path with the desired trajectory speed and movement along the azimuth to the target.

 The features of the prototype, common with the claimed technical solution, are as follows: the mechanical system of the robot, actuators, a block of sensors of internal information, a sensor subsystem, a block of planning movements in the space of external coordinates.

 The reasons that impede the achievement of the required technical result are as follows: the proposed procedure for the local approximation of complex trajectories introduces additional errors in the planning and, accordingly, in the development of these trajectories. The resulting algorithmic solutions due to the minimized quality integral do not allow to stabilize the set values of the trajectory speed. The proposed organization of movements requires a block of kinematic transformations for transforming tasks from the space of external coordinates to the space of internal, directly controlled coordinates. The noted feature of structures with tracking systems allows one to obtain analytical transformation algorithms only for simple kinematic schemes of mobile robots. With a more complex organization of the mechanical system, the solution of the inverse kinematic problem is possible only within the framework of approximate expressions of internal coordinates as a function of external coordinates, which introduces an additional error in the formation of nominal trajectories in the space of internal coordinates and, therefore, also in their refinement.

 The objective of the invention is to improve the accuracy of planning and, therefore, refinement of trajectories, as well as expanding the functionality of the robot.

 The technical result achieved by the implementation of the proposed device is that the proposed control algorithms do not require a block of kinematic transformations, it does not require approximating devices and interpolators in the structure of the robot control system, which eliminates the corresponding components of the job error associated with the approximation of calculations [ S.F. Burdakov, V.A. Dyachenko, A.V. Timofeev. Design of manipulators of industrial robots and robotic complexes. M., Higher School, 1986, 246 p., P. 60] and, therefore, to reduce the error in the development of the planned motion paths, ie to increase the accuracy of the robot. The expansion of functionality lies in the fact that the proposed device, depending on the task, allows you to organize movement from an arbitrary point in the phase space of external coordinates to a given one, at zero speed; provide movement of the robot along trajectories in the entire class of quadratic forms with a given trajectory speed; move the robot along a given trajectory to a given point without presenting additional requirements for trajectory speed. The property of asymptotic stability in general of the planned phase trajectories, guaranteed by the proposed device, suggests the low sensitivity of closed systems to changes in the parameters of the object and to external perturbations of a certain class [A.A. Kolesnikov. Synergetic control theory. M.- Taganrog, 1994, 344 p., P. 261].

 The technical result is achieved by the fact that in the positional-trajectory control device of a mobile robot containing a mechanical system, the inputs of which are connected to the outputs of the block of actuators and the inputs of the block of sensors of internal information, and the outputs to the external environment; a block for planning trajectories in the space of external coordinates, the first inputs of which are connected to the outputs of the sensor subsystem, the inputs of which are connected to the external environment, a block for calculating the vector of nonlinear elements, a block for calculating the matrix of control coefficients, a block for computing the derivative of the column vector of external velocities with respect to the row vector of external coordinates, unit for calculating the derivative of the column vector of external velocities with respect to the row vector of internal coordinates, unit for calculating the vector of external velocities, three blocks of multiplication by a, sixteen multiplication blocks, twelve summation blocks, a matrix inversion block, three matrix transpose blocks, and the outputs of the internal information sensor block are connected to the inputs of the nonlinear elements vector calculation block, the outputs of which are connected to the third inputs of the thirteenth multiplication block, to the inputs of the coefficient matrix calculation block control, the outputs of which are connected with the third inputs of the fourth block of multiplication, to the first inputs of the calculation unit of the derivative of the column vector of external speeds on the row vector of external coordinates, the outputs of which are connected with the second inputs of the fourteenth multiplication block, to the first inputs of the calculation unit of the derivative of the column vector of external velocities with respect to the row vector of the internal coordinates, the outputs of which are connected with the second inputs of the fourth multiplication block and the second inputs of the thirteenth multiplication block to the first inputs of the external velocity vector calculation unit, the outputs of which are connected to the second inputs of the fifteenth multiplication unit, the inputs of the first two and second multiplication unit and the inputs of the fifth block of multiplication, and to the second inputs of the block for planning trajectories in the space of external coordinates, the first inputs of which are connected to the second inputs of the block of the derivative of the vector column of external velocities along the row vector of the external coordinates, with the second inputs of the block of the calculation of the derivative of the vector column of outer speeds along a vector-row of internal coordinates, with the second inputs of the block for calculating the vector of external speeds, the inputs of the third block of transposing matrices, the second inputs of the ninth block the inputs of the second matrix transpose block, the outputs of which are connected to the second inputs of the second multiplication block and the second inputs of the tenth multiplication block, the first inputs of which are connected to the inputs of the second multiplication block by two, the output of which is connected to the first inputs of the second multiplication block, the first inputs of the fifth block multiplication, and with the third outputs of the block planning trajectories in the space of external coordinates, the fourth outputs of which are connected to the second inputs of the second block of summation, the first inputs of which of the second are connected to the outputs of the second multiplication block, and to the second inputs of the eighth summing block, the first inputs of which are connected to the outputs of the tenth multiplication block, the fifth outputs of the path planning block in the space of external coordinates are connected to the second inputs of the third multiplication block, to the first inputs of the sixth multiplication block, the third inputs of which are connected with the output of the fifth multiplication block, to the first inputs of the third summing block, the second inputs of which are connected with the second inputs of the sixth multiplication block, t the inputs of the third multiplication block, the second inputs of the twelfth multiplication block and the sixth outputs of the path planning block in the space of external coordinates, the first outputs of which are connected to the first inputs of the twelfth multiplication block, and its second outputs are connected to the first inputs of the ninth summing block, the second inputs of which are connected to the outputs of the eleventh multiplication block, the first inputs of which are connected with the outputs of the eighth summing block, the seventh outputs of the block of planning trajectories in space three external coordinates are connected to the first inputs of the first summing block, the outputs of which are connected to the fourth inputs of the third multiplication block, to the first inputs of the fourth summing block, the outputs of which are connected to the third inputs of the seventh multiplication block, and to the first inputs of the sixth summing block, the outputs of which are connected to the first inputs of the ninth multiplication block, and the second inputs are connected to the outputs of the eighth multiplication block, the eighth outputs of the path planning block in the space of external coordinates the first inputs of the first block of multiplication, the second inputs of which are connected to the outputs of the first block of transposition of matrices, the inputs of which are connected to the outputs of the first block of multiplication by two, and the outputs of the first block of multiplication are connected with the second inputs of the first block of summation, the ninth outputs of the block for planning trajectories in space external coordinates are connected to the first inputs of the third block of multiplication by two, the outputs of which are connected with the second inputs of the fourth block of summation, and to the first inputs of the eighth block of AC The second inputs of which are connected to the second inputs of the third block of multiplication by two and the outputs of the third block of transposition of matrices, the tenth output of the block for planning trajectories in the space of external coordinates is connected to the second input of the seventh block of summation, the first inputs of which are connected to the outputs of the ninth block of multiplication, and the outputs connected to the third inputs of the tenth summation block, the first inputs of which are connected to the outputs of the twelfth multiplication block, and its second inputs are connected to the outputs of the ninth block As a summation, the first inputs of the third multiplication unit are connected to the first inputs of the seventh multiplication unit, the second inputs of which are connected to the outputs of the third summing unit, and to the outputs of the second summing unit, and the outputs of the third multiplication unit are connected to the first inputs of the fourth multiplication unit, the outputs of which are connected to the inputs of the matrix reversal block, to the first inputs of the thirteenth multiplication block, the outputs of which are connected to the second inverse inputs of the twelfth summing block, and to the first inputs of four of the eleventh multiplication block, the outputs of which are connected to the second inputs of the eleventh summation block, the outputs of which are connected by the first inputs of the fifteenth multiplication block, and the first inputs of the eleventh summation block are connected to the outputs of the fifth summation block, the second inputs of which are connected to the outputs of the seventh multiplication block, and its first inputs connected to the outputs of the sixth multiplication block, the third inverse inputs of the twelfth summing block are connected to the outputs of the fifteenth multiplication block, the first inverted the strokes of the twelfth summing block are connected with the outputs of the tenth summing block, and its outputs are connected to the first inputs of the sixteenth multiplication block, the second inputs of which are connected to the outputs of the matrix circulation unit, and the outputs of the sixteenth multiplication block are connected to the inputs of the actuating unit.

 Theoretical evidence of a causal relationship between the claimed features and the achieved technical result is given below.

где z - n-вектор внутренних, управляемых и наблюдаемых координат;
F - n-вектор нелинейных элементов;
В - невырожденная (nхn)-матрица коэффициентов управления;
U - n-вектор управлений;
y - вектор наблюдаемых внешних координат, dim y=m, m≥n, m≤6;
М - вектор-функция внешних скоростей;
r - конструктивные параметры.Let a mobile robot be described by its dynamic and nonholonomic kinematic models of the form

where z is the n-vector of internal, controlled and observed coordinates;
F is the n-vector of nonlinear elements;
B is a non-degenerate (nхn) -matrix of control coefficients;
U is the n-vector of controls;
y is the vector of the observed external coordinates, dim y = m, m≥n, m≤6;
M is the vector function of external speeds;
r - design parameters.

 The structures of the vector F (z) and the matrix B (z) are uniquely determined for each particular robot as a result of the procedure for deriving the mathematical model of the robot. Moreover, as a rule, for most models, the functional elements of the presented vector and matrix are functions of internal coordinates (steering angle, steering wheel speed, etc.) and are determined by the dynamics of the mechanical system of the mobile robot, actuators and additional equipment (manipulators, pushers, etc.). The structure of these vectors and matrices for specific mobile robots is given, for example, in the following works: Burdakov S.F., Stelmakov R.E., Steinle S.V. Trajectory synthesis and control of mobile robots in the face of uncertainty. Materials of the 8th scientific and technical conference "Extreme Robotics". Under the scientific editorship of prof. E.I. Yurevich. SPb. , SPbSTU, 1997, 439 p., S. 198-209 - a linear representation of the elements of the vector F (z) is used, and the elements of the matrix B (z) are considered as constants; Miroshnik I.V., Govyadinkin D.S., Drozdov V.N. Transport Cart Control System ", Control in optical and electromechanical systems. L., LITMO, 1989 - elements of the vector F (z) are identically equal to zero, and the matrix B (z) is unit and diagonal: Devyanin EA On the movement of wheeled robots In the collection of reports of the scientific school-seminar "Mobile robots and mechatronic systems", Moscow, 1998, pp. 169-200, a nonlinear representation of both elements of the vector F (z) and elements of the matrix B (z) is considered. general structure of equations of dynamics of nonholonomic systems, a modified version of which is pre is given by equation (1), is given in the well-known work of Bloch A.M., Reyhanoglu M., McClamroch M.H. "Control and Stabilization of Nonholonomic Dynamic Systems", IEEE Transactions on Automatic Control, v. 37, 11, 1992, p.p. 1746-1757.

от элементов вектора у не зависит [С. Ф.Бурдаков, Р.Э.Стельмаков, С.В.Штайнле. Синтез траекторий и управление мобильными роботами в условиях неопределенности. Материалы 8-й научно-технической конференции "Экстремальная робототехника". Под научной редакцией проф. Е.И. Юревича. СПб, из-во СПбГТУ, 1997, 439 с., с.с. 198-209].For example, the presented models (1) and (2) describe a number of mobile robots based on transport trolleys. As a rule, for a mobile robot based on a transport trolley n = 2 (one equation describes the dynamics of the driving wheels, the other describes the dynamics of the steering (steering) wheels), m = 3 (two equations describe the position of the center of gravity of the robot in coordinates y ₁ and y ₂ , the third is the orientation angle of ₃ buildings in the external coordinate system) and

does not depend on the elements of the vector [S. F. Burdakov, R.E. Stelmakov, S.V. Steinle. Trajectory synthesis and control of mobile robots in the face of uncertainty. Materials of the 8th scientific and technical conference "Extreme Robotics". Under the scientific editorship of prof. E.I. Yurevich. SPb, because of SPbSTU, 1997, 439 p., S.p. 198-209].

являющаяся производной вектор-столбца внешних координат по вектор-строке внутренних координат, имеет ранг, равный n, т.е. rankR=n, здесь M^*=(M₁,M₂, . .. M_n)^T.Let the Jacobi matrix of the kinematic model (2)

which is a derivative of a column vector of external coordinates with respect to a row vector of internal coordinates, has a rank equal to n, i.e. rankR = n, here M ^* = (M _1, M _2,. .. M _n) ^T.

Consider the problem of contour control of a mobile robot when it is necessary to organize the movement of the robot along a given trajectory with a given trajectory (contour) speed V _k [Pshikhopov V.Kh., Chernukhin Yu.V. Contour regulator for a neural network control system of an adaptive mobile robot. On Sat Proceedings of the International Scientific and Technical Conference "Intelligent Multiprocessor Systems", Taganrog, 1999, p. 210-217.].

здесь

Уравнениями вида (4) в зависимости от матриц

могут быть описаны окружности, эллипсы, прямые и т.д.Let the desired trajectories of the mobile robot on the plane are described by quadratic forms from the external coordinates y ₁ and y _{2 of the} form
a ₁₁ ¹ y ₁ ² + a ₂₂ ¹ y ₂ ² + a ₃₁ y ₁ + a ₄₁ y ₂ + a ₅₁ = 0,
or in matrix form

here

Equations of the form (4) depending on the matrices

circles, ellipses, lines, etc. can be described.

где V_k - определяемая заданием контурная скорость, а скорость движения МР относительно траектории (4) - равенством

Очевидно, что значение контурной скорости V_k должно удовлетворять неравенству
V_k≤V_max, (7)
где V_max - максимальная контурная скорость, определяемая энергетическими возможностями приводов МР.We formulate the requirements for the speed of MR along the trajectories (4) in the form of a velocity manifold

where V _k is the contour velocity determined by the task, and the MP speed relative to the trajectory (4) is the equality

Obviously, the value of the contour velocity V _k must satisfy the inequality
V _k ≤V _max , (7)
where V _max is the maximum loop speed determined by the energy capabilities of the MP drives.

и, соответственно, желаемое многообразие Ψ, отражающее требование к движению МР в его фазовом пространстве, может быть представлено их линейной комбинацией в следующей векторной записи

где А - положительно определенная матрица постоянных задаваемых коэффициентов; dim А=(nхn); 0₁-(1хn) - вектор нулевых элементов; T- символ транспонирования.Then the trajectory Ψ _T and speed Ψ _{C of the} submanifold can be formed in the form of vectors [Pshikhopov V.Kh., Chernukhin Yu.V. Contour regulator for a neural network control system of an adaptive mobile robot. On Sat Proceedings of the International Scientific and Technical Conference "Intelligent Multiprocessor Systems", Taganrog, 1999, p. 210-217.]:

and, accordingly, the desired variety Ψ, reflecting the requirement for the motion of the MR in its phase space, can be represented by their linear combination in the following vector notation

where A is a positive definite matrix of constant given coefficients; dim A = (nxn); 0 ₁ - (1хn) - vector of zero elements; T is the transpose symbol.

где С - положительно определенная (nхn)-матрица задаваемых постоянных коэффициентов.As the dimension of the space of functioning of the MR increases, the trajectory manifold Ψ _T is supplemented by manifolds of the form (4), which describe additional restrictions on the trajectory of the robot's motion, to dimension n-1. As additional manifolds, restrictions on the orientation angles of the robot can be set. Accordingly, by virtue of expressions (7) and (8), the dimension of the velocity manifold Ψ _C will also be increased.
In order to transform manifolds (9) into attractors, we set the desired dynamics of the closed-loop system in the form of a differential equation of the form [A.A. Kolesnikov., Synergetic control theory. M.- Taganrog, 1994, 344 p., P. 66]:

where C is a positive definite (nхn) -matrix of given constant coefficients.

где Ψ₀ - отклонение от многообразия (9) в начальный момент времени t₀∈t, здесь t - время решения поставленной перед МР задачи, сформулированной многообразием (9).Obviously, the solution of equation (10)

where Ψ ₀ is the deviation from the manifold (9) at the initial moment of time t ₀ ∈t, here t is the time of solving the problem posed to the MR formulated by the manifold (9).

где D₁= 2y^тN₁ ¹+N₂ ¹, C, A, N₁ ¹,N₂ ¹,N₃ ¹, V - матрицы и векторы постоянных коэффициентов соответствующей размерности, L - матрица Якоби кинематической модели (2), являющаяся производной вектор-столбца внешних скоростей по вектор-строке внешних координат у.Substituting the MP model (1), (2) and expression (9) into equation (10) and solving the latter with respect to the control action vector U, we obtain [V. Pshikhopov, Y. Chemukhin "Path Following Regulator for Neural Network Implemented Control System of Adaptive Mobile Robot Moving with a Set Speed". In Proc. of Int. Conffer. "Mathematical Theory of Networks and Systems", June 18-23, 2000, Perpignan, France]

where D ₁ = 2y ^т N ₁ ¹ + N ₂ ¹ , C, A, N ₁ ¹ , N ₂ ¹ , N ₃ ¹ , V are matrices and vectors of constant coefficients of the corresponding dimension, L is the Jacobi matrix of the kinematic model (2), which is a derivative of the column vector of external velocities with respect to the row vector of external coordinates y.

Algorithm (12) allows you to organize the movement of MR along the desired trajectories defined by the quadratic forms of the external coordinates y *, with matrix coefficients N ₁ ¹ , N ₂ ¹ , N ₃ ¹ , with a constant contour speed V _k . Moreover, the value of the loop speed V _k is determined, on the one hand, by the MR performance requirements, and on the other, by the energy capabilities of the MP drives.

с нулевой конечной скоростью. Очевидно, что желательно обеспечение асимптотической устойчивости в целом желаемых точек позиционирования. Иными словами, желаемые точки должны представлять собой аттракторы - притягивающие многообразия в фазовом пространстве соответствующих координат МР.Consider the solution to the problem of synthesis of a positional controller that provides the movement of MR from an arbitrary point in the phase space of external coordinates to a given point

with zero final speed. Obviously, it is desirable to provide asymptotic stability in general to the desired positioning points. In other words, the desired points must be attractors — attracting manifolds in the phase space of the corresponding MR coordinates.

где

(nх1)-векторы соответственно координат точки позиционирования и значений в ней внешней скорости.Now we form the objective functions corresponding to the task in the form of the following equalities:

Where

(nx1) -vectors, respectively, of the coordinates of the positioning point and the values of the external speed in it.

а их линейная комбинация

где А - (nхn)-матрица, имеющая тот же смысл, что и в выражении (9), является линией пересечения требований (15) в фазовом пространстве управляемого объекта.Accordingly, trajectory Ψ _T and speed Ψ _C manifolds can be represented as follows:

and their linear combination

where A is an (nхn) -matrix, which has the same meaning as in expression (9), is the intersection line of requirements (15) in the phase space of the controlled object.

 In order to transform manifolds (16) into attractors, we set the desired dynamics of the closed system in the form of a differential equation of the form (10).

где матрицы R, L, М^* и М имеют тот же смысл, что и в алгоритме (12).Substituting the MR model (1), (2) and expressions (15), (16) into equation (10) and solving the latter with respect to the control action vector U, we obtain

where the matrices R, L, M ^* and M have the same meaning as in algorithm (12).

 The presented positional control algorithm, like algorithm (12), requires appropriate sensory support (measurement of y and z coordinates) and guarantees, by virtue of solution (11), the mobile robot reaches a given point in the space of external coordinates.

 Obviously, controls (12) and (17) have a common structure. Moreover, control (17) is a special case of algorithm (12).

 The noted structural commonality of the algorithms for positional (17) and contour (12) controls allows us to develop general principles for constructing the corresponding control systems. In addition, the generalized structure of the controller allows one to construct effective positional-contour control algorithms, when it is necessary to translate the MR from an arbitrary point in the space of external coordinates to a predetermined one along the desired trajectory without imposing additional requirements on the contour speed.

здесь n - размерность вектора управлений, M_e - (nхn)-матрица, diagM_e=e; D_i=2y^тN₁ ⁱ+N₂ ⁱ- вспомогательная матрица соответствующей размерности, i=1,n.In any of the stated problem statements, it seems appropriate, based on the above approach, to form trajectory Ψ _T and velocity Ψ _C manifolds in the form of the following vectors:

here n is the dimension of the control vector, M _e is the (nхn) -matrix, diagM _e = e; D _i = 2y ^т N ₁ ⁱ + N ₂ ⁱ - auxiliary matrix of the corresponding dimension, i = 1, n.

When solving the problems of loop control, in the general case, N _j ⁱ ≠ 0, N ₂ ^j ≠ 0, N ₃ ^j = 0, N ₁ ⁿ = 0, N ₂ ⁿ = 0, N ₃ ⁿ = 0, _Me = E , V _k ≠ 0, where E is the identity matrix of the corresponding dimension, and the expressions for the trajectory and velocity manifolds constructed by expression (18) will coincide with those presented in expression (8).

When solving problems of positional control N ₁ ⁱ = 0, N ₂ ⁱ = Е, N $\genfrac{}{}{0ex}{}{\text{i}}{\text{3}}$ = y $\genfrac{}{}{0ex}{}{\text{*}}{\text{fi}}$ , M _e = 0, V _k = 0 and the expressions for the trajectory and velocity manifolds constructed by expression (18) will coincide with those presented in expression (15).

In the case of solving the position-contour problem, Σ _j can set the desired trajectory, and Σ _n is one of the coordinates of the positioning point. Moreover, M _e = 0, V _k = 0.

 In any of the cases listed, the dimension of the formed manifolds should coincide with the number of control actions and, as a rule, with the dimension of the space of controlled coordinates.

где индексы i, j, n имеют тот же смысл, что и в выражении (18).Using the above procedures for the formation of the desired manifolds in the space of external coordinates and their transformation into attractors, we obtain the following position-trajectory control algorithm [V.Kh. Pshikhopov. "Analytical synthesis of synergetic regulators for position-trajectory control systems for mobile robots." Materials of the 11th scientific and technical conference "Extreme Robotics". Under the scientific editorship of prof. E.I. Yurevich. SPb., SPbSTU, 2000]

where the indices i, j, n have the same meaning as in expression (18).

The required condition for the positive definiteness of the matrices C and A is satisfied, in particular, for diagC = diagA = s _i > 0.

Attention should be paid to the condition rank (k ₁ RB) = n, the violation of which, due to the properties of the given matrices C and A, as well as the proposed controllability of the MP and condition (3), is possible when the MP is located at the characteristic points of the planned trajectories (the center of the circle, focal points of the ellipse, infinitely distant points belonging to the bisector of the parabola, etc.), as well as the simultaneous zeroing of external velocities (only for the contour problem) [Pshikhopov V.Kh., Kolesnikov A.A. The contour control device manipulation robot. RF patent 2146606, bull. 8, 2000]. Such cases are possible only at the moment the movement begins. In this case, the moments of the MP drives can go beyond their limitations, which is not always desirable when organizing the smooth movement of the MP. The latter circumstance determines the need for the use of “push” procedures. In particular, when finding MR at the indicated points, it is possible to use a positional control algorithm, with a subsequent transition, depending on the task, to contour or position-contour algorithms. The possibility of using positional algorithms in such situations is determined by the absence of uncertainty of control actions at all points of the phase space.

 The advantages compared with the prototype achieved by the implementation of the proposed device are that the proposed control algorithms do not require a kinematic transform block, the approximating devices and interpolators are not required in the structure of the robot control system, which eliminates the corresponding components of the job error associated with approximation of calculations [S.F. Burdakov, V. A. Dyachenko, A. V. Timofeev Design of manipulators of industrial robots and robotic x complexes. M., Higher School, 1986, 246 p., P. 60] and, therefore, to reduce the error in the development of the planned motion paths, ie increase the accuracy of the movement of the robot. The expansion of functionality lies in the fact that the proposed device, depending on the task, allows you to organize movement from an arbitrary point in the phase space of external coordinates to a given one, at zero speed; provide movement of the robot along trajectories in the entire class of quadratic forms with a given trajectory speed; move the robot along a given trajectory to a given point without presenting additional requirements for trajectory speed.

 It should be noted again that the presented control algorithms guarantee the property of asymptotic stability in the whole of the planned trajectories. This suggests that the control system ensures the fulfillment of the tasks set not only for a given class of external disturbances (reducible to initial conditions), but also with some uncertainty of the object’s parameters.

 The invention is illustrated by drawings, where in Fig.1.1, 1.2 presents a block diagram of a mobile robot closed by a synthesized device for position-path control; figure 2 is a block diagram of the algorithm of operation of the device; figure 3 is a diagram of the operation of the device.

 The device positional-trajectory control of a mobile robot, shown in figure 1, contains a block 1 planning paths (PT) in the space of external coordinates, the first outputs of which are connected to the first inputs of the multiplication unit (P) 2, and its n-1 second outputs to the first inputs of the summing unit (C) 3, the outputs of which are connected to the second inputs of the summing unit 4, the first inputs of which are connected to the outputs of the multiplication unit 2, and its outputs - to the first inverse inputs of the summing unit 5.

 (nl) n third outputs of the block 1 planning paths in the space of external coordinates are connected to the inputs of the block 6 multiplying by two (ECU) and the first inputs of the block multiplying 7, and the outputs of the block 2 multiplying by two are connected to the first inputs of the block multiplying 8 and the first inputs of the block multiplication 9. The outputs of the multiplication unit 8 are connected to the first inputs of the summing unit 10, and the outputs of the multiplying unit 7 are connected to the first inputs of the summing unit 11, the second inputs of which are connected to the fourth (n-1) n outputs of the block 1 planning trajectories in space in eshnih coordinates and the second inputs of the summation unit 10, and its outputs are connected to first inputs of multiplying 12 block whose outputs are connected to second inputs of the summation unit 3.

 The fifth n outputs of block 1 planning paths in the space of external coordinates are connected to the first inputs of the multiplication block 13, the second inputs of the summing block 14 and the second inputs of the multiplying block 15, the first inputs of which are connected to the outputs of the summing block 10 and the first inputs of the multiplying block 16, the third inputs are with sixth n outputs of the block 1 planning trajectories in the space of external coordinates, the second inputs of the summation block 14, the output of which is connected to the second inputs of the multiplication block 16, with the second inputs of the block multiply 2 and the second inputs of the multiplication unit 13, the third inputs of which are connected to the outputs of the multiplication unit 9.

 The seventh n outputs of the block 1 planning paths in the space of external coordinates are connected to the first inputs of the summation block 17, the outputs of which are connected to the fourth inputs of the multiplication block 15, to the first inputs of the summation block 18, the outputs of which are connected to the third inputs of the multiplication block 16, and to the first inputs summing unit 19, the outputs of which are connected to the first inputs of the multiplication unit 20, the second inputs of which are connected to the first inputs of the block 1 planning trajectories in the space of external coordinates, to the inputs of the block 21 trans matrix maturation (BT), the outputs of which are connected to the second inputs of the multiplication block 8 and the second inputs of the multiplication block 7, to the inputs of the transposition block 22 of the matrices, to the second inputs of the multiplication block 12, the second inputs of the block 23 for computing the external velocity vector (BVS), the second inputs unit 24 for calculating the derivative of the column vector of external velocities with respect to the vector row of internal coordinates (RNH), the second inputs of block 25 for calculating the derivative of the vector column of external velocities with respect to the vector row of external coordinates (RNC), m outputs of the sensor subsystem we (SP) 26, whose inputs are connected with the external environment.

 The eighth n outputs of the path planning block 1 in the space of external coordinates are connected to the first inputs of the multiplication unit 27, the second inputs of which are connected to the outputs of the matrix transpose block 28, and the outputs are connected to the second inputs of the summation block 17.

 The ninth n outputs of block 1 planning paths in the space of external coordinates are connected to the first inputs of block 29 multiplying by 2, the outputs of which are connected to the second inputs of the summing block 18, and its second inputs are connected to the outputs of the block 22 transposing matrices and the second inputs of the block multiplying 30, the first inputs of the multiplication unit 30, the outputs of which are connected to the second inputs of the summing unit 19.

 The tenth output of the block 1 planning paths in the space of external coordinates is connected to the second input of the summing unit 31, the first inputs of which are connected to the outputs of the multiplication unit 20, and its outputs are connected to the third inputs of the summing unit 4.

 The outputs of the external velocity vector calculation unit 23 are connected to the second inputs of the multiplication unit 9, the inputs of the multiplication by 32 unit, the outputs of which are connected to the inputs of the matrix transpose unit 28, and to the second inputs of the multiplication unit 33, the first inputs of which are connected to the outputs of the summing unit 34, and its outputs - with the third inverse inputs of the summing unit 5, the second inputs of which are connected to the outputs of the multiplication unit 35.

 n outputs of the block 36 for calculating the vector of nonlinear elements (BNE) are connected to the third inputs of the block multiplying 35, the second inputs of which are connected with the outputs of the block 24 for calculating the derivative of the vector column of external velocities along the row vector of internal coordinates (SRI) and the second inputs of the block of multiplication 37, the first inputs of which are connected to the outputs of the multiplication block 15, the first inputs of the multiplication block 35 and the first inputs of the multiplication block 38, the second inputs of which are connected to the outputs of the block 25 for calculating the derivative of the external column vector velocities along a row vector of external coordinates.

 The outputs of block 39 for calculating the matrix of control coefficients (BCU) are connected to the third inputs of the block multiplying 37, the outputs of which are connected to the inputs of block 40 of the matrix (BOM), the outputs of which are connected to the second inputs of the block multiplying 41, the first inputs of which are connected to the outputs of the summing unit 5 , and its outputs - to n inputs of block 42 of executive devices (DUT).

 The outputs of the block 42 of the actuators are connected to the inputs of the mechanical system (MS) 43, the outputs of which are connected to the external environment, and to the inputs of the block 44 of internal information sensors (BDVI), the outputs of which are connected to the inputs of the block 36 for calculating the vector of nonlinear elements, the inputs of the block 39 for calculating matrices of control coefficients, the first inputs of block 25 for calculating the derivative of the column vector of external velocities with respect to the row vector of external coordinates, the first inputs of the block 24 for calculating the derivative of the vector column of outer velocities with vector s the internal coordinate path, block 23 computing the external velocity vector and the second inputs of the block 1 planning trajectories in the space of external coordinates.

 The first inputs of the summing unit 45 are connected to the outputs of the multiplication unit 13, the second inputs are connected to the outputs of the multiplying unit 16, and the outputs are connected to the first inputs of the summing unit 34, the second inputs of which are connected to the outputs of the multiplying unit 38.

 By the block of planning trajectories in the space of external coordinates is meant the totality of blocks and modules that form the desired trajectories of the robot. That is, a top-level computer, an operator console, sensitive devices, etc., can be included in this unit. [Control of robots from a computer. Ed. Yurevich E. I. .. L., Energy, 1980, 264 p., P. 78]. As the main element of the block for planning trajectories in the space of external coordinates, it is possible to use a control structure on neural-like elements [Kalyaev A.V., Noskov V.P., Chernukhin Yu.V. The algorithm of the control structure of the transport robot. // Proceedings of the USSR Academy of Sciences. Technical cybernetics. - 1980.- 4, p. 64-72].

 The structure of the blocks of summation, multiplication and transposition of matrices (vectors), matrix inversion is determined by the well-known rules of matrix calculus [Korn G., Korn T. Handbook of mathematics for scientists and engineers. M., 1984, 833s., P. 392-393]. For example, the matrix transpose block can be a switch with hard connections between (n • n) inputs and (n • n) outputs, as shown in Fig. 5 in [Pshikhopov V.Kh., Kolesnikov A.A. a robot. RF patent 2146606, bull. 8, 2000]. Zero elements of diagonal matrices are not formed by the structure of the control device.

 It should be noted that the structural implementation of blocks 23, 24, 25, 36, 39 is uniquely determined by the known model (1), (2), analytical expressions (3), (13) for each specific robot, which are sets of operations of multiplication, summation , definitions of trigonometric functions.

 The block diagram of the algorithm of the device and the diagram of its operation are presented, respectively, in figure 2 and 3.

The device position-trajectory control operates as follows. The block for planning trajectories in the space of external coordinates 1 (see Fig. 1), depending on the task, generates at its second, third, fourth, seventh, ninth and twelfth outputs the corresponding coefficients of quadratic forms, which are coefficients for the squares of coordinates - N ₁ ^j , N ₁ ⁿ , with coordinates and free constants -N ₂ ^j , N ₂ ⁿ and N ₃ ^j , N ₃ ^n, respectively. These coefficients, by virtue of expressions (18), determine the type of the desired trajectory of movement or the coordinates of the positioning point. At n fifth and n sixth outputs, diagonal elements of definitely positive matrices C and A are formed, by virtue of equations (10) and (11) determining the dynamics of the robot's movement to the intersection line of the manifolds (18).

At the same time, at the first outputs of the trajectory planning block in the space of external coordinates 1, the required value of the trajectory (contour) speed V _k along the specified trajectories is formed, as well as the readings from the n outputs of the block 42 of the actuating devices of the robot, and the sensor subsystem 26 m inputs - coordinates of the position of the robot in the external environment. In addition, the values of the coefficients of the matrix M _e are formed at n eighth outputs of the block for planning trajectories in the space of external coordinates 1.

Moreover, when solving the problems of loop control, in the general case, N _i ^j ≠ 0, N ₂ ^j ≠ 0, N ₃ ^j ≠ 0, N ₁ ⁿ = 0, N ₂ ⁿ = 0, N ₃ ⁿ = 0, M _e = E, V _k ≠ 0, where E is the identity matrix of the corresponding dimension. When solving problems of positional control N ₁ ⁱ = 0, N ₂ ⁱ = Е, N $\genfrac{}{}{0ex}{}{\text{i}}{\text{3}}$ = y $\genfrac{}{}{0ex}{}{\text{*}}{\text{fi}}$ . For the case of solving the position-contour problem, M _e = 0, V _k = 0.

 The signals from the n outputs of the block of internal information sensors 44 corresponding to the vector z of the state of internal coordinates are supplied respectively to the inputs of the block 26 for calculating the nonlinear elements of the vector F, the inputs of the block 39 for calculating the matrix of control coefficients B, for the first inputs of the block 25 for calculating the derivative of the column vector of external speeds by a row vector of external coordinates L by expression (13), at the first inputs of a block 24 of calculating the derivative of a column vector of external velocities by a row vector of internal coordinates R by expression (3), at the first e inputs external velocity vector calculating unit 23, M * by the expression (2), with the disclosures in the formula (3). And m signals from the sensor subsystem 26 are fed to the second inputs of the block 25 for calculating the derivative of the column vector of external velocities in the row vector of external coordinates, block 24 for calculating the derivative of the column vector of outer velocities in the row vector of internal coordinates, block 23 for calculating the outer velocity vector and determine the procedures for the corresponding calculations.

 It should be noted that the structural implementation of blocks 23, 24, 25, 36, and 39 is uniquely determined from the known model (1), (2) for each specific mobile robot.

The calculation of the matrix coefficients k ₁ , k ₂ , k _{3 is} carried out in accordance with the expressions given in (19). Moreover, at the nxn outputs of the block of multiplication 15 by means of blocks of multiplication by two 6 and 32, multiplying 8 and 27, adding 10 and 17, transposing 21 and 28, a matrix coefficient k _{1 is formed} . At the n outputs of the summing unit 45 by means of two 6 multiplying units, multiplying 9, 13 and 16, adding 14 and 18, and transposing 22, a matrix coefficient k _{2 is formed} . At the n outputs of the summing unit 4 by means of multiplying by two, multiplying 2, 3, 12, 20 and 30, adding 3, 11, 19 and 31, a matrix coefficient of _{3 is formed} .

 The calculation of control signals, in accordance with algorithm (19), is performed by summation blocks 5, 34, matrix inversion 40, multiplication 33, 35, 37, 38, and the formation of n control signals U by algorithm (19) is performed by block 41.

By means of the block 42 of the actuators, control actions are generated on the mechanical system 43, which processes them. The block planning trajectories in the space of external coordinates 1, taking readings from the block of sensors of internal information 44 and the sensor subsystem 26, determines the need for further formation of control signals. If the technological problem is not solved, the block planning trajectories in the space of external coordinates 1 re-generates a set of the same constants at their outputs that initialize the operation of the remaining blocks of the device (see figure 3). Otherwise, the operation of the device is completed and the constants at the outputs of the block for planning trajectories in the space of external coordinates 1 are not formed or a transition is made to the solution of the following technological problem, i.e. the formation of a new set of constants N ₁ ^j , N ₂ ^j , N ₃ ^j , N ₁ ⁿ , N ₂ ⁿ , N ₃ ^j , C, A, V _k , M _e .

 The functioning of the blocks of summation, multiplication, circulation and transposition is carried out according to the well-known rules of matrix calculus [Korn G., Korn T. Handbook of mathematics for scientists and engineers. M., 1984, 833s., P. 392-393].

не менее 50 Гц [М. Вукобратович, Д.Стокич, Н.Кирчански. Неадаптивное и адаптивное управление манипуляционными роботами. М.: Мир, 1998, 376 с., с. 338-339] до полного выполнения технологического задания.The calculation of the control actions according to the algorithm (19) is carried out with a frequency

not less than 50 Hz [M. Vukobratovich, D. Stokic, N. Kirchanski. Non-adaptive and adaptive control of manipulation robots. M.: Mir, 1998, 376 p., P. 338-339] to complete the execution of the task.

 The technical result achieved by the implementation of the proposed device is that the proposed control algorithms do not require a block of kinematic transformations, it does not require approximating devices and interpolators in the structure of the robot control system, which eliminates the corresponding components of the job error associated with the approximation of calculations [ S.F. Burdakov, V.A. Dyachenko, A.V. Timofeev. Design of manipulators of industrial robots and robotic complexes. M., Higher School, 1986, 246 p., P. 60] and, therefore, to reduce the error in the development of the planned motion paths, ie increase the accuracy of the movement of the robot. The expansion of functionality lies in the fact that the proposed device, depending on the task, allows you to organize movement from an arbitrary point in the phase space of external coordinates to a given one, at zero speed; provide movement of the robot along trajectories in the entire class of quadratic forms with a given trajectory speed; move the robot along a given trajectory to a given point without presenting additional requirements for trajectory speed. The property of asymptotic stability in general of the planned phase trajectories, guaranteed by the proposed device, allows us to assume the low sensitivity of closed systems to changes in the parameters of the object and to external perturbations of a certain class [A.A. Kolesnikov, Synergetic control theory. M.- Taganrog, 1994, 344 p., P. 261].

 The presented position-trajectory control algorithms can be implemented programmatically using microprocessor technology devices, for example, high-performance controllers (one or several) [M. Vukobratovich, D. Stokich, N. Kirchanski. Non-adaptive and adaptive control of manipulation robots. M.: Mir, 1998, 376 p., P. 320-326], in accordance with the flowchart of FIG. 2 and the operation diagram of the device of FIG. 3.

Claims (1)

 A device for position-trajectory control of a mobile robot, comprising a mechanical system, the inputs of which are connected to the outputs of the block of actuators and the inputs of the block of sensors of internal information, the block for planning trajectories in the space of external coordinates, the first inputs of which are connected to the outputs of the sensor subsystem, characterized in that a block for calculating the vector of nonlinear elements, a block for calculating the matrix of control coefficients, a block for calculating the derivative of the column vector of external velocities with respect to a vector row of external coordinates, a block for calculating the derivative of a column vector of external velocities with respect to a vector row of internal coordinates, a block for calculating a vector of external velocities, two multiplication blocks, multiplication blocks, summing blocks, a matrix inversion block, matrix transpose blocks, and outputs of the sensor block internal information is connected to the inputs of the block for computing the vector of nonlinear elements, the outputs of which are connected with the third inputs of the thirteenth block of multiplication, to the inputs of the block for computing the matrix coefficient in the control, the outputs of which are connected with the third inputs of the fourth multiplication block, to the first inputs of the calculation unit of the derivative of the column vector of external speeds along the row vector of external coordinates, the outputs of which are connected with the second inputs of the fourteenth multiplication block, to the first inputs of the derivative vector column calculation block external speeds along a vector row of internal coordinates, the outputs of which are connected to the second inputs of the fourth block of multiplication and the second inputs of the thirteenth block of multiplication, to the first inputs a block for calculating the external velocity vector, the outputs of which are connected to the second inputs of the fifteenth multiplication block, the inputs of the first block of multiplication by two and the second inputs of the fifth block of multiplication, and to the second inputs of the block for planning trajectories in the space of external coordinates, the first inputs of which are connected to the second inputs of the block of calculation the derivative of the column vector of external velocities with respect to the row vector of external coordinates, with the second inputs of the calculation unit of the derivative of the column vector of external velocities with respect to the row vector of int coordinates, with the second inputs of the external velocity vector calculation unit, the inputs of the third matrix transpose block, the second inputs of the ninth multiplication block, the inputs of the second matrix transpose block, the outputs of which are connected to the second inputs of the second multiplication block and the second inputs of the tenth multiplication block, the first inputs of which are connected with the inputs of the second block of multiplication by two, the output of which is connected to the first inputs of the second block of multiplication, the first inputs of the fifth block of multiplication, and with third outputs by the path planning block in the space of external coordinates, the fourth outputs of which are connected to the second inputs of the second summing block, the first inputs of which are connected to the outputs of the second multiplication block, and to the second inputs of the eighth summing block, the first inputs of which are connected to the outputs of the tenth multiplication block, fifth outputs block planning of trajectories in the space of external coordinates are connected to the second inputs of the third block of multiplication, to the first inputs of the sixth block of multiplication, the third inputs of which connected with the output of the fifth multiplication block, to the first inputs of the third summing block, the second inputs of which are connected to the second inputs of the sixth multiplication block, the third inputs of the third multiplication block, the second inputs of the twelfth multiplication block and the sixth outputs of the path planning block in the space of external coordinates, the first outputs of which connected to the first inputs of the twelfth multiplication block, and its second outputs connected to the first inputs of the ninth summing block, the second inputs of which are connected to the moves of the eleventh multiplication block, the first inputs of which are connected to the outputs of the eighth summation block, the seventh outputs of the path planning block in the external coordinate space are connected to the first inputs of the first summation block, the outputs of which are connected to the fourth inputs of the third multiplication block, to the first inputs of the fourth summation block, the outputs which are connected to the third inputs of the seventh multiplication block, and to the first inputs of the sixth summing block, the outputs of which are connected to the first inputs of the ninth block multiplication, and the second inputs are connected to the outputs of the eighth multiplication unit, the eighth outputs of the path planning unit in the space of external coordinates are connected to the first inputs of the first multiplication unit, the second inputs of which are connected to the outputs of the first matrix transpose unit, the inputs of which are connected to the outputs of the first multiplication unit by two, and the outputs of the first block of multiplication are connected with the second inputs of the first block of summation, the ninth outputs of the block planning trajectories in the space of external coordinates sub are connected to the first inputs of the third block of multiplication by two, the outputs of which are connected to the second inputs of the fourth block of summation, and to the first inputs of the eighth block of multiplication, the second inputs of which are connected to the second inputs of the third block of multiplication by two and the outputs of the third block of transposition of matrices, the tenth output of the block planning of trajectories in the space of external coordinates is connected to the second input of the seventh summation block, the first inputs of which are connected to the outputs of the ninth multiplication block, and the outputs are connected to the third the inputs of the tenth summation block, the first inputs of which are connected to the outputs of the twelfth multiplication block, and its second inputs are connected to the outputs of the ninth summation block, the first inputs of the third multiplication block are connected to the first inputs of the seventh multiplication block, the second inputs of which are connected to the outputs of the third summation block, and to the outputs of the second summing unit, and the outputs of the third multiplication unit are connected to the first inputs of the fourth multiplication unit, the outputs of which are connected to the inputs of the access unit atrits, to the first inputs of the thirteenth multiplication block, the outputs of which are connected to the second inverse inputs of the twelfth summing block, and to the first inputs of the fourteenth multiplication block, whose outputs are connected to the second inputs of the eleventh multiplication block, whose outputs are connected to the first inputs of the fifteenth multiplication block, and the first the inputs of the eleventh summing block are connected to the outputs of the fifth summing block, the second inputs of which are connected to the outputs of the seventh multiplication block, and its first inputs are connected the outputs of the sixth multiplication block, the third inverse inputs of the twelfth summing block are connected to the outputs of the fifteenth multiplication block, the first inverse inputs of the twelfth summing block are connected to the outputs of the tenth summing block, and its outputs are connected to the first inputs of the sixteenth multiplication block, the second inputs of which are connected to the outputs block circulation matrices, and the outputs of the sixteenth block multiplication are connected to the inputs of the block actuators.

Images