RU2582868C2 - Method of controlling mobile object - Google Patents

Abstract

FIELD: control systems.

SUBSTANCE: invention can be used in designing control systems of mobile objects providing their displacement along specified trajectory with a given trajectory speed, or to a given point along preset trajectory without trajectory speed requirements presentation, or to a given point with zero final speed. In method of controlling movable object is measured external coordinates of mobile object and derivatives thereof, a row of matrices, matrix coefficients and a control signal for control of underwater vehicle. Near control object is placed on water surface ship control station equipped with receiver of signals from satellite navigation systems, using sonar and underwater radio communication system with controlled object. Using satellite navigation system signal receiver is determined external coordinates of ship control station, and using sonar determined distance to control object and its angle of vision at this range and angle of sight of external control and object coordinates are determined using a system of underwater communication is transmitted to obtained coordinates.

EFFECT: technical result is possibility of determining external coordinates of underwater vehicle.

1 cl

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 trajectory with a given trajectory speed, or to a given point along a given trajectory without presenting requirements for trajectory speed, or to a given point with zero final speed.

A known method of controlling a mobile robot based on servo systems described in [S.F. Burdakov, R.E. Stelmakov, S.V. Steinle. "Trajectory synthesis and control of mobile robots in the face of uncertainty." Materials of the VIII Scientific and Technical Conference "Extreme Robotics" ./ Under the scientific. ed. prof. E.I. Yurevich. - SPb, publishing house of SPbSTU, 1997, 439 p. - S.198-209]. This method involves approximating a complex trajectory with simpler geometric images, measuring the external and internal coordinates of the control object, recalculating the required trajectory from the space of external coordinates to the space of internal coordinates, and changing the internal coordinates so that the external coordinates approach the desired ones.

The signs of this analogue, coinciding with the features of the proposed method, are the measurement of the external and internal coordinates of the control object.

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 local approximation of complex trajectories makes significant errors in planning and, therefore, in the development of these trajectories and does not allow to stabilize the set value of the trajectory speed. In addition, the proposed organization of movement requires a block of kinematic transformations from the space of external coordinates to the space of internal, directly controlled coordinates. The noted feature allows one to obtain analytical transformation algorithms only for simple kinematic schemes of control objects. With a more complex mechanical system of this object, the solution of the inverse kinematics problem is possible only within the framework of approximate expressions of internal coordinates as an external function, which introduces an additional error in the formation of nominal trajectories in the space of internal coordinates, and, therefore, in their refinement.

There is also a known method of controlling the movement of a manipulation robot, described in the description of the device protected by RF patent No. 2146606, class. G05B 11/10, G05B 25, G05J 13/10, 2000. This method comprises measuring the generalized coordinates of the manipulation robot and their derivatives, converting them into external coordinates, forming a matrix of nonlinear elements, forming a matrix of control coefficients, forming matrices of quadratic and linear forms , the formation of the first and second diagonal matrices of constant coefficients, the formation of the vector of external velocities and calculation, based on the generated matrices and vectors, matrix coefficients and control signal.

The signs of this analogue, which coincide with the features of the proposed method, are the formation of a matrix of nonlinear elements, the formation of a matrix of control coefficients, the formation of matrices of quadratic and linear forms, the formation of the first and second diagonal matrices of constant coefficients and the formation of a vector of external velocities.

The reason that prevents obtaining the technical result achieved in the invention in this analogue is the fact that in this method the calculation of matrix coefficients is based on measuring the generalized coordinates of the controlled object and their derivatives, which provides control of the movement of the working body of the manipulation robot. In practice, it is often necessary to control a moving (mobile) object. This function does not allow to implement this function.

The closest in technical essence to the claimed (prototype) is a method of controlling a moving object, implemented in a device for controlling a moving object, protected by RF patent No. 2393522, cl. G05D, 1/00, 2010

This method is as follows.

, а n - число измеряемых внутренних координат управляемого объекта. Формируют первую А и вторую С диагональные матрицы постоянных коэффициентов размерностью n×n. Формируют первый матричный коэффициент $${\widehat{E}}_1$$

в соответствии с уравнением:The internal coordinates of the managed object are measured. Using the receiver of signals of satellite navigation systems installed on it, its external coordinates and their derivatives are determined. A vector F and a matrix B of non-linear transformation of internal coordinates are formed. A vector M of external speeds is formed. A matrix is formed - the derivative R of the column vector of external velocities with respect to the row vector of internal coordinates. A matrix is formed - the derivative L of the column vector of external velocities with respect to the row vector of external coordinates. Matrices N _1j , N _2j , N _{3j are formed of} quadratic forms and linear forms of external coordinates, where $$j=\overline{\mathrm{one,}n}$$

, and n is the number of measured internal coordinates of the managed object. The first A and second C are formed by diagonal matrices of constant coefficients of dimension n × n. The first matrix coefficient is formed $${\widehat{E}}_{\mathrm{one}}$$

according to the equation:

where D _i = 2Y ^T · N _1i + N _2i is the first auxiliary matrix;

Y is the vector of measured external coordinates of dimension 6≥m≥n;

E is the identity matrix of dimension m × m;

- производная вектора Y для тех значений, которые являются координатами центра тяжести объекта управления; $${\dot{Y}}^{*}$$

- the derivative of the vector Y for those values that are the coordinates of the center of gravity of the control object;

T is the symbol of the transpose operation,

. $$i=\overline{\mathrm{one,}\text{n-1}}$$

.

в соответствии с уравнением:Form a second matrix coefficient $${\widehat{E}}_2$$

according to the equation:

where D _j is the second auxiliary matrix,

0 ₁ is the vector of zero elements of dimension n.

в соответствии с уравнением:The third matrix coefficient is formed $${\widehat{E}}_3$$

according to the equation:

where V _k is the given trajectory (contour) speed.

The control vector U is formed in accordance with the equation:

All of the listed signs (actions) of the prototype coincide with the essential features of the proposed method.

The reason that prevents the achievement in the prototype method of a technical result provided by the claimed method is the limited scope of the method. The fact is that the determination of the external coordinates of the controlled object is carried out using a signal receiver of satellite navigation systems. However, since the receiver is installed on a managed object, then if this is an underwater vehicle in an underwater position, then the reception of such signals is impossible.

At the same time, it often becomes necessary to control the underwater vehicle, and the prototype cannot fulfill this task.

The technical problem to which the creation of the invention is directed is to expand the scope of the method for controlling underwater objects.

The technical result is achieved by the fact that in the known method of controlling a moving object, implemented in a device protected by RF patent No. 2393522, when controlling an underwater vehicle near the control object, a ship control station equipped with a satellite signal receiver, sonar and underwater radio communication system is located on the water surface with a controlled object, using the signal receiver of satellite navigation systems determine the external coordinates of the ship's control point, using the sonar determines the distance to the control object and the angle of its sight, according to this range and the angle of sight, determine the external coordinates of the control object and use the underwater communication system to transmit the received coordinates to it.

, а n - число измеряемых внутренних координат, формировании первой А и второй С диагональных матриц постоянных коэффициентов размерностью n×n, формировании первого матричного коэффициента $${\widehat{E}}_1$$

в соответствии с уравнением:To achieve the specified technical result in a known method for controlling a moving object, based on measuring the internal coordinates of a controlled object, measuring its external coordinates and their derivatives, forming a vector F and a matrix В of non-linear transformation of internal coordinates, forming a vector M of external speeds, forming a matrix - derivative R column vector of external velocities in a row vector of internal coordinates, matrix formation - derivative L of a column vector of external velocities in row vectors e external coordinates, the formation of matrices N _1j , N _2j , N _3j quadratic forms from external coordinates, where $$j=\overline{\mathrm{one,}n}$$

, and n is the number of measured internal coordinates, the formation of the first A and second C diagonal matrices of constant coefficients of dimension n × n, the formation of the first matrix coefficient $${\widehat{E}}_{\mathrm{one}}$$

according to the equation:

where D _i = 2Y ^T · N _1i + N _2i is the first auxiliary matrix;

Y is the vector of measured external coordinates, dimension m≥n;

E is the identity matrix of dimension m × m;

- производная вектора Y для тех значений, которые являются координатами центра тяжести объекта управления; $${\dot{Y}}^{*}$$

- the derivative of the vector Y for those values that are the coordinates of the center of gravity of the control object;

T is the symbol of the transpose operation,

, $$i=\overline{\mathrm{one,}\text{n-1}}$$

,

в соответствии с уравнением:the formation of the second matrix coefficient $${\widehat{E}}_2$$

according to the equation:

where D _j is the second auxiliary matrix;

0 ₁ is the vector of zero elements of dimension n,

в соответствии с уравнением:the formation of the third matrix coefficient $${\widehat{E}}_3$$

according to the equation:

where V _k - a given trajectory (contour) speed,

and generating a control signal U in accordance with the equation:

U = - [K ₁ · R · B] ^-1 · [K ₁ · R · F + [K ₂ + K ₁ · L] · M + K ₃ ],

in this case, in the case of controlling an underwater vehicle, a ship control center equipped with a satellite navigation system signal receiver, a sonar and an underwater radio communication system with a controlled object is located near the control object on the water surface, the external coordinates of the ship control center are determined using the signal receiver of satellite navigation systems, using sonar determine the distance to the control object and the angle of its sight, these range and the angle of sight determine the external the coordinates of the control object and using the underwater communication system transmit the received coordinates to it.

Studies of the claimed technical solution for patent and scientific and technical literature have shown that the set of newly introduced actions in the claimed method is not amenable to independent classification. At the same time, it does not follow explicitly from the prior art. Therefore, the proposed method should be considered as satisfying the criterion of "novelty" and having an inventive step.

Thus, the implementation of the proposed control method provides for the following actions on the control object and other material objects.

1. Measure the internal coordinates Z of the managed object.

.2. Measure its external coordinates Y and their derivatives $$\dot{Y}$$

.

, а n - число измеряемых координат.3. Form matrices N _1j , N2 _j , N _{3j of} quadratic forms, where $$j=\overline{\mathrm{one,}n}$$

, and n is the number of measured coordinates.

4. Form the first A and second C diagonal matrices of constant coefficients of dimension n × n.

5. Form the first and second auxiliary matrices D _j and D _j in accordance with the equations:

D _i = 2Y ^T · N _1i + N _2i

D _j = 2Y ^T · N _1j + N _2j .

6. A vector M of external velocities, a vector F and a matrix B of non-linear transformation of internal coordinates are formed.

7. Form a matrix - the derivative R of the column vector of external velocities with respect to the row vector of internal coordinates.

8. Form a matrix - the derivative L of the column vector of external velocities with respect to the row vector of external coordinates.

в соответствии с уравнением:9. Form the first matrix coefficient $${\widehat{E}}_{\mathrm{one}}$$

according to the equation:

where Y is the vector of measured external coordinates, dimension m≥n;

E is the identity matrix of dimension m × m;

- производная вектора Y для тех значений, которые являются координатами центра тяжести объекта управления; $${\dot{Y}}^{*}$$

- the derivative of the vector Y for those values that are the coordinates of the center of gravity of the control object;

T is the symbol of the transpose operation,

. $$i=\overline{\mathrm{one,}\text{n-1}}$$

.

в соответствии с уравнением:10. Form a second matrix coefficient $${\widehat{E}}_2$$

according to the equation:

where 0 ₁ is the vector of zero elements of dimension n.

в соответствии с уравнением:11. Form the third matrix coefficient $${\widehat{E}}_3$$

according to the equation:

where V _k is the given trajectory (contour) speed.

12. Form the control vector U in accordance with the equation:

U = - [K ₁ · R · B] ^-1 · [K ₁ · R · F + [K ₂ + K ₁ · L] · M + K ₃ ].

The described algorithm (the order of actions on the material objects control system that is part of the proposed method), including the control signal U, is fully consistent with the control algorithm given in the materials of the XI Scientific and Technical Conference "Extreme Robotics". It corresponds to the movement of a mobile (moving) object along a given trajectory.

13. If the control object is an underwater vehicle, then its external coordinates are determined using the ship control center equipped with a receiver of signals from satellite navigation systems, a sonar and an underwater radio communication system with the control object. The ship control station is located on the water surface near the underwater vehicle. Using a sonar, the underwater vehicle is captured for escort, the distance to it and the angle of its sight from the ship's control point are determined, and the external coordinates of the ship's control point are determined using the signal receiver of satellite navigation systems. According to the measured range and angle of sight, these coordinates are converted into the external coordinates of the underwater vehicle, using the underwater radio communication system, the conversion results are transmitted to the underwater vehicle.

Thus, in the proposed method, it is possible to determine the external coordinates of the underwater vehicle, in contrast to the prototype method, where this is not possible.

This greatly expands the scope of the proposed method in comparison with the prototype method.

The proposed method is quite easy to implement. As a receiver of signals from satellite navigation systems, the SNS SN-47 module can be used, which is a 24-channel navigation receiver that provides reception and processing of SNS GLONASS, GPS, and SBAS signals. The sonar and means of underwater radio communications are quite widely described in the literature [see, for example, Hydroacoustic Encyclopedia. / Under the General Ed. Doctor of Technical Sciences IN AND. Tymoshenko. - Publishing House of the Taganrog State Radio Engineering University, 1999. - 788 p .; Robert J. Urik. Basics of sonar. - Per. from English Publishing house "Shipbuilding", L. - 1978, S. 420-433]. As the remaining elements of the device that implements the proposed method, the corresponding elements of the control device described in RF patent No. 2393522 that implements the prototype method can serve.

Claims (1)

A method of controlling a moving object, based on measuring the internal coordinates of a controlled object, measuring its external coordinates and their derivatives, forming a vector F and a matrix В of non-linear transformation of internal coordinates, forming a vector M of external speeds, forming a matrix - derivative R of the column vector of external speeds with respect to the vector -line of internal coordinates, matrix formation - derivative L of the column vector of external velocities with respect to the row vector of external coordinates, matrix formation of N _1j , N _2j , N _3j quadratic forms from external coordinates, where

, and n is the number of measured internal coordinates, the formation of the first A and second C diagonal matrices of constant coefficients of dimension nxn, the formation of the first matrix coefficient K ₁ in accordance with the equation:

where D _i = 2Y ^T · N _1i + N _2i is the first auxiliary matrix;
Y is the vector of measured external coordinates of dimension m≥n;
E is the identity matrix of dimension mxm;

- the derivative of the vector Y for those values that are the coordinates of the center of gravity of the control object;
T is the symbol of the transpose operation,

the formation of the second matrix coefficient K ₂ in accordance with the equation:

where D _j = 2Y ^T · N _1j + N _2j is the second auxiliary matrix;
0 ₁ is the vector of zero elements of dimension n,
the formation of the third matrix coefficient K ₃ in accordance with the equation:

where V _k - a given trajectory (contour) speed,
and the formation of the control vector U in accordance with the equation:

characterized in that in the case of controlling the underwater vehicle near the control object on the water surface, there is a ship control station equipped with a satellite navigation system signal receiver, a sonar and an underwater radio communication system with a controlled object, using the satellite navigation system signal receiver, the external coordinates of the ship control center are determined, using the sonar determine the distance to the control object and the angle of its sight, this range and the angle of sight op edelyayut external coordinates and the control object via underwater communication systems transmit these coordinates to it.