CC BY
48
***** ИЗВЕСТИЯ *****
НИЖНЕВОЛЖСКОГО АГРОУНИВЕРСИТЕТСКОГО КОМПЛЕКСА
АГРОПРОМЫШЛЕННАЯ ИНЖЕНЕРИЯ
UDC 621.865.8
THE CONTROL SYSTEM OF THE MANIPULATOR AGRICULTURAL ROBOT
I.A. Nesmiyanov1, Candidate of Technical Sciences,Associate Professor V.V.Zoga , Doctor of physico-mathematical sciences, professor V.E.Pavlovskiy3, Doctor of physico-mathematical sciences, professor N.S. Vorobyeva1, Candidate of Technical Sciences,Associate Professor
1Volgograd State Agricultural University, Volgograd 2Volgograd State Technical University 3Institute of Applied Mathematics. MV Keldysh RAS, Moscow
The paper presents a solution to the problem of positioning and trajectory synthesis loading manipulator robot. The control system for the implementation of program motions. The results of theoretical and experimental research.
Key words: manipulator, control system, program motion.
In the manipulators based on the parallel structure mechanisms (PSM) features of metallic constructions and drives are combined. So they take a load like spatial farms, and as a consequence, have higher positioning precision with high gripper speed and acceleration [5]. Manipulator with parallel structure mechanisms compared to manipulator with the series-connected links, is characterized by high load capacity and relatively small metal consumption. The disadvantages of parallel structure manipulators are limited working area, a small manipulative, lack of optimal control algorithms [3].
In this regard, the optimization of geometrical, kinematic and power parameters of parallel kinematic structure manipulators and development of their calculation and design methods are actual tasks [4].
Manipulator robot
The basis of the manipulator is spatial actuator mechanism in the form of a triangular pyramid with links of variable length as executive links (Fig. 1) [1].
« I n \ <V> 1 I
Fig. 1. - The design of the robot manipulator
The mechanism contains three variable-length links 1, 2, 3, the ends of which are fixed by means of special two-movable hinges on the rotary base 5 that allows to change the service area. Opposite ends of the links are connected with a special hinge 6, providing the intersection of the geometrical axes of these links at one point and eliminating bending
moments from external loads. Rotary base 1 has the ability to turn around due to the variable-length link 4.
The positioning problem
The common task of moving executive element (gripping arm) is divided into three stages - the positioning of the grip provided by the geometry of the mechanism, synthesis of its trajectory in the work area space and the definition of motion law on the obtained trajectory.
The positioning of manipulator's grip during technological operations consists of moving it from the initial state, which is defined by the coordinates of the point Mo(Xmo, Ymo, Zmo), to end position Mk(XMk, YMk, Zuk)- Moreover, from the initial configuration of the manipulator, defined by generalized coordinates li0, we need to find target configuration lk As the number of generalized coordinates of the manipulator, equal to four, exceeds the number of generalized coordinates of grip, equal to three, thus target end position of the object meets a variety of system configurations (Fig. 2).
Z
Q ^Aom Y
Fig. 2 - Transition of gripper arm from the initial to the end position (ZOY plane)
The coordinates of the grip center M in fixed Cartesian system of coordinates OXYZ and lengths of manipulator links, are coupled by the next equations
XM + (YM — YA )2 + (ZM — ZA )2 = (XM " XB)2 + (YM " YB )2 + ZM = L2, (1)
(XM — XC ) + (YM — YC ) + ZM = L> YM + (ZA — ZD ) = L4,
where XM, YM, ZM - coordinates of the M point in the fixed coordinate system;
Xb=-Xc, Za, Yb=Yc, Zd - coordinates of mounting points of the manipulator links.
The lengths of the links l2k, l3k are determined uniquely from expressions (1), and the lengths of the links ljk(YA, Za), l4k(YA, ZA) are found from the condition of minimum of a quadratic function
0 (Ya , Za ) = C (Lk — L10)2 + CA(LAk — L40)2, (2)
and limitations of equality type
f (Ya , Za ) = Z:A + Yb — Ya f — OA = 0, (3)
and inequalities [7]
^lmin - - ^lmax, ^4min - ^4 - ^4max- (4)
According to Kuhn-Tucker theorem necessary conditions of stationarity are written in the form
(5)
80 8(l, -1, )2 8(l4 -l4 )2 8f Y Z*)
8ya 1 8ya 4 8ya 1 8Ya ^ *
80L = C-1io)\c48(/4k -/4o)2 , ,%i8f(YAAZA) 0
Where J" 0-f YA = YAmm' J* 0-f YA = YAmax, 1= o,f Ya > Yamin' ^1= 0' if YA < YAmax'
The values L1k(YA, ZA), L4k(YA, Za) are found from conditions.
It was developed the design procedure of a programmable movement of the manipulator working body from the initial to the end state when the motion is occurred according to a sinus law of the actuating link rod's acceleration changing and when motion occurs along a straight line.
The law of the links' length changes from li0 up lik during the time T we accept as
h(t)=li0+(lik - li0)'
t 1 . (2nt sin
(6)
T 2k ^ T ''
Then the parametric equations of the gripper arm trajectory are determined from the solution of the inverse kinematic problem.
The trajectory, optimal on a gripper's time of movement, is a straight line. However, such displacement is possible in a case when all points of the segment McjMk belongs to the set, which is part of the service area. The dependencies of coordinates XM, YM, ZM on the length of a trajectory are determined from the equation of the straight line in space passing through two points M0 and Mk. The law of motion along the straight line M0Mk also has been taken so that it satisfies the requirements of «soft» touch. Thus, the laws of links' lengths changing li(t) are derived from the equations (1) without the solving the inverse kinematic problem.
Trajectory synthesis
The system of differential equations describing the motion of a manipulator with holonomic links is recorded using the Lagrange equations with undetermined multipliers.
Enter symbols x1=X, x3=Y, x5=Z, x7=y, Nk=1, 2, 3, 4 - control efforts in the manipulator links. Functions that transfer manipulator gripper from the initial to the end position during the time T, are found from the condition of functional minimum [6]:
1 T
J = 0(x7 J [i;22 +i42 +x26 +OxA2 ■xl\dt. (?)
20
Desired functions must satisfy the following boundary conditions: xzs-i(0) = x2s-i,0'x2s-i(T) = x2s-i,t-x2s(9) = = 0 . The value of the function x?(T)
is unknown. Writing down the required stationary conditions [7] for xk(t), k=1, 3, 5, taking into account the boundary conditions, we obtain:
Xkkt) =-2^K(«)] , + 3321,+ Ik(0). (8)
According to [8] the boundary condition for the influence function
*(T) = 0^- = , (9)
where p is a constant factor, which is determined by the results of the obtained solution analysis and can be adjusted at the design stage. Since the solution of a variational problem
x7 (T) = const = x7 (t) = x~''^P,
so, after integration we get
<p (t) 112
v 7 6T3 AT2
-cp^ 0).
(10)
Then at t=T we define an unknown base rotation angle in a final moment of time x7=yO\A, and from the equations (1) we find the laws of change of manipulator links' lengths 4=(t).
Programmatic forces, needed to implement the synthesized motion laws of the manipulator links by the control system, are found from the solution of the equations of dynamics.
Handling of the manipulator
For implementation of the robot programmatic motion laws we build a control system with feedback by position, which solve the problem of the contouring control.
Block diagram of the control system developed for mobile robot manipulators drives is shown in Fig. 3.
A linear DC motor - actuator plays as an operating mechanism of a manipulator link. In the developed device the "SKFgroup" actuators CAT33Hhx400hAG1F are used. These motors do not have an internal control system, but they are equipped with the feedback sensor to determine the current status of the device. Feedback is implemented in the form of linear potentiometer, reflecting the absolute value of the rod position. Signal of tracking feedback Ufb (Fig. 3) feed to the input of the analog-digital converter (ADC) of drives control unit (DCU). The rated voltage of the actuator drive is 24V. The speed of the rod is regulated by changing the control PWM signal.
The control system completely governs the mechanisms of the system having 4 managed degree of freedom of the manipulator-tripod and the rotary platform, on which the manipulator is placed. DCU is based on 32-bit microcontroller family STM32F407 with ARM Cortex-M4F architecture. The high clock speed and a considerable amount of internal memory provide necessary computing resources to meet the managing challenges. In addition, the controller has a wide range of peripheral devices, ADC and controllers of USB 2.0, Ethernet, RS-232, CAN interfaces.
Microchip of a bridge amplifier is included in the control loop of PWM signals generated by built-in timers of microcontroller. The amplifier functions are performed by dual full H-bridge on the chip VNH3SP30 passing maximum current of up to 30A for each channel. Reversal of the motor rotation (direction of the rod movement) is set by the control signals on the GPIO pins of the controller. As a rotation drive of the rotary base of the manipulator the "Maxon motors" servo motors RTG 060-100 are used. Position controller EPOS 70/10 governs the servo motor. Executive instructions from the host controller STM32F407 are transferred to the position controller via USB.
Fig. 3 - Block diagram of the manipulator drives control system
You can wirelessly control this robot from a remote computer. Connectivity is provided by WiFi module, connected to one of the USB ports of on-board controller. The system of remote motion control is implemented on the base of personal computer. Its aim is
to calculate in real time a coordinated movement of all actuators on the base of a given mathematical model, issuing control commands for DCU and feedback data processing.
Results of numerical and experimental modeling
Fig. 4 - Fig. 5 show the results of solving the considered problem for the initial values of the lengths of the manipulator links L10=235 mm, L20=370 mm, L30=370 mm, L40=240 mm in conditions when the motion of the gripper is proceeded along a straight line and at a priori unknown trajectory for sinusoidal law of acceleration changing.
These values obtained by measuring of position pickup are corresponded to the coordinates of the gripper X(0)=0 mm, Y(0)=340 mm, Z(0)=349 mm and manipulator's base tilting angle ^(0)=0,65 rad. Taking a finite values of a gripper coordinates: X(T)=50 mm, Y(T)=580 mm, Z(T)=150 mm, we determine the changing laws of a gripper coordinates Xk(t), of a robot manipulator model's base tilting angle y(t), and on expressions (2) - the lengths of its edges.
MM
= - » /3
2' /2 - r-
4' r t /
1' / * >> f : —
4
' t,c
Fig. 4 - Theoretical (curves 1-4) and experimental (curves 1'-4') dependencies of manipulator links' 1-4 lengths changing on time at sinusoidal law of acceleration changing of actuators' rods
%
14
max 12
a.
AS
10 8 6 4
2
// max \
AS-
"ma i
50
100
ISO
200
250
300
350
400
Fig. 5 - Dependencies of maximum deviation (hmax) of gripper arm of manipulator from straight-line trajectory, increasing the length of the trajectory (AS) and the ratio of maximum values amax of the gripper acceleration at the motion along a straight line and at sinusoidal law of acceleration changing in according to the distance between positioning points
Conclusions
Presented calculated and experimental dependencies of a change of manipulator links lengths on the time during the gripper motion along a straight line and at a priori unknown path for sinusoidal law of acceleration change have demonstrated, subject to the
measurement errors, theoretical and experimental results match, that may serve as a proof of feasibility of the examined motion laws. From Fig. 4 it is clear that when implementing the straight-line trajectory linear velocity of 1, 2 and 3 link's rods change the sign, i.e. they make reciprocating motion in relative motion. Moreover, despite the more length of the trajectory of gripper with sinusoidal law of change of acceleration of the executive links (Fig. 5), the total relative movement of rods in the implementation of the straight-line trajectory exceeds the corresponding relative displacements of the rods under the sinusoidal law of change of acceleration of the executive links. Herewith the maximum acceleration of a gripper increases slightly (Fig. 5). Moreover, programmatic forces needed to implement the straight-line trajectory become alternating. Under the sinusoidal law of change of acceleration of the executive link's rods, their lengths at positioning the gripper change monotonically (Fig. 5), and the trajectory of gripper is always in service zone of the manipulator, while the straight-line trajectory of the gripper is feasible not for any location of the endpoint positioning. Thus, it can be argued that the algorithm and control system of a manipulator gripper transfer under the sinusoidal law of changes of its executive links are preferable.
The work was supported by the Russian Foundation for Basic Research (project 1308-00387 - a), (project 12-08-00301 - a).
References
1. Avariino-spasatel'naya mashina [Rescue vehicle] Pat. 2476372 RF, MPK S1. / V.V. Zhoga, V.A. Skakunov, A.V. Eremenko, P.V. Fedchenkov, V.M. Gerasun, I.A. Nesmiyanov, V.V. Dyashkin-Titov; VolgGTU (2013)
2. Braison A., Kho Yu-shi. Prikladnaya teoriya optimal'nogo upravleniya [Applied optimal control theory]. M.: Mir, 1972. 544 s.
3. Glazunov V.A, Koliskor A.Sh, Krainev A.F. Prostranstvennye mekhanizmy parallel'noi struktury [Spatial mechanisms of parallel structure]. M.: Nauka, 1991, 95 s.
4. Issledovanie optimal'nykh konfiguratsii manipulyatora - tripoda s povorotnym osnovaniem [Investigation of optimal configurations of the manipulator - tripod with the rotary base] / V.M. Gerasun, V.V. Zhoga, I.A. Nesmiyanov, N.S. Vorob'eva, V.V. Dyashkin-Titov // Mekhatronika, avtomatizatsiya, upravlenie, 2013. - № 6. - S. 21-16.
5. Korendyasev A.I, Salamandra B.L, Tyves L.I. i dr. Manipulyatsionnye sistemy robotov [Robotic handling systems]. M.: Mashinostroenie, 1989. 472 s.
6. O programmnykh dvizheniyakh manipulyatora-tripoda [About software manipulator-tripod] V.V. Zhoga, V.M. Gerasun, I.A. Nesmiyanov, N.S. Vorob'eva, V.V. Dyashkin-Titov / 6-ya Vserossiiskaya mul'tikonferentsiya po problemam upravleniya // Materialy mul'tikonferentsii: Izd-vo YuFU, 2013. - T.2. - S.146-150.
7. Sintez programmnykh dvizhenii manipulyatora na osnove prostranstvennogo mekhanizma parallel'noi struktury s chetyr'mya postupatel'nymi parami [Synthesis of program motions of the manipulator based on the spatial structure of the parallel mechanism with four pairs of translational] / V.M. Gerasun, V.V. Zhoga, I.A. Nesmiyanov, N.S. Vorob'eva, V.V. Dyashkin-Titov // 5-ya Rossiiskaya mul'tikonferentsiya po problemam upravleniya. Materialy konferentsii «Upravlenie v tekhnicheskikh, ergaticheskikh, organizatsionnykh i setevykh sistemakh (UTEOSS-2012)». - Sankt-Peterburg, 2012 g.- S. 722-725.
E-mail: [email protected]
