study, flash microcomputer SH7125 of the Renesas Electronics. Co. is used. A wireless module uses ZEAL-C02 that is the. Bluethooth module of the ADC ...
Development of Snake-like Robot Climbing Up Slope in Consideration of Constraint Force Ken Tashiro, Syunsuke Nansai, Masami Iwase, and Shoshiro Hatakeyama
Dept. of Robotics and Mechatronics, Graduate School of Science and Technology for Future Life Tokyo Denki University 5 Senju-Asahi-Cho, Adachi-ku, Tokyo, 120-8551 Japan Email: {tashiro.nansai.iwase.sho}@ctrl. fr. dendai. ac.jp Abstract-A snake-like robot is a robot imitating a real snake. A snake-like robot is developed in order to achieve excellent locomotion characterization of a real snake. It is necessary for a snake-like robot to move even in geographical features like a slope for adapting to various environment. The purpose of this study is the verification of slope climbing movement of the snake like robot. In previous our study, a developed robot was affected seriously by cable tension. Hence, we have to realize wireless control of the snake-like robot for solving the problem. In this paper, we make a wireless system for transmitting the sensor information from the robot and commands from a computer. The wireless system behavior is verified by simulations.
I.
INTRODUCTION
Locomotion of the snake is to wriggle the body. Snakes adapt to the environment to locomotion. Its locomotion can be adapted to a variety of environments such as sand and water and wasteland. Snake is promoted by taking advantage of the frictional force of the body surface and the environment. A snake-like robot is robot that imitated a snake. Snake like robot has been researched to achieve excellent mobility characteristics of the snake. Hirose et al found that changes to a sinusoidal curve of meandering snake from the perspective of exercise physiology. [1] A lot of researches to which the Snake-like robot moves on the plane surface are done. [2] [3] [4] However, locomotion in undulation geographical features etc. is indispensable to make them adjust to more various environments. For instance, the posture of the robot becomes important in the environment like the slope in which gravity influences it when the robot moves. There is a situation that slips off from the slope depending on the posture of the robot. Therefore, it is necessary to control the posture of the robot to obtain the frictional force that doesn't slip down from the slope. Yamakita et al have achieved the orbit follow control of the Snake-like robot on the slope by considering the frictional force to be constraint force that hangs to the robot. [5] A lot of trajectory tracking controls are performed in the research so far. As our previous work, Watanabe et al. are proposing the fixed point position control of the Snake-like robot on the slope by building the frictional force into the eval uation function of State-Dependent Ricatti Ecuation(SDRE). [6] [7] [8] [9] [10] Method of Watanabe et al has the advantage, and does not require a target trajectory, can reach 978-1-4673-2421-2112/$31.00 ©2012
IEEE
the target point automatically. This advantage is confirmed by the numerical simulation. In this study, the control method of Watanabe et al. propose is applied to a real machine, positional control on the slope is verified experimentally. A real machine uses three link two joint type Snake-like robot that Maruyama et al. produced. However, a present system has the problem that the thing that the tension of the cable influences the movement and the area that can be moved narrow because information and the electric power are sent and received by cable. Then, it is necessary to make it to the wireless. To make the Snake-like robot wireless, the pulse counter and the pulse generator are achieved by using the microcomputer. Moreover, the counter circuit and the motor driver control circuit are made. A real machine is made wireless by installing these microcomputer, motor driver, battery, and wireless module. Whether the real machine made wireless can achieve a reverse-climb is verified by the numerical simulation. Finally, the movement of the Snake-like robot on the slope is experimentally verified. II. MODEL OF 3-LINK SNAKE-LIKE ROBOT It thinks about the control of the robot on the slope by using the control technique that Watanabe et al. proposed. In the control technique that Watanabe proposes, the dead weight compensation is combined with the control to which the robot moves the plane. As a result, power to slip down on the slope that hangs to the robot is denied. Because power to slip down was denied, the control that moves by the plane surface even if it is a slope can be applied. To climb the slope of inclination ¢, it thinks about the modeling of the snake like robot of three link two joint type. The motion equation of the snake-like robot is derived by using Projection Method. Projection Method is a technique for deriving the motion equation from the constraint condition of the system. [?] In Projection Method, the motion equation of the degree of freedom of the restrained system is derived from the motion equation in the state from which the system is not restrained and the constraint condition of the system. Fig. 1 shows the model of three link two joint snake-like robot. Moreover, the parameter of three link two joint snake-like robot is shown in TABLE I. First of all, the motion equation in the state not restrained is derived. Generalized coordinates q that are the parameters
5422
y
Here, pseudo speed v is defined like expression (3). v= [
W12
W2
W2l
W22
W3
W3l
W32
y,
(5)
A =diag(Al, A2, A3)
yl y.
Ai
0
X.
Fig. 1.
X,
XI
X
X,
variable Ji mi di Ii Ci
c�s
ei - sin 8i
s�n
ei cos 8i
l
i
=
1,2,3
(6)
However, it is . M = AT MaA, and it is assumed h = AT (ha - MaAv). Next, constraint procession C who becomes Cv = from
0
1,2,3)
substance Moment of inertia of each element[kg·m 1 Mass of each element[kg] Distance from head of each link to center of gravity of each link[m] Distance from center of gravity of each link to end of each link.[m] Viscous modulus of each element[Nm's/rad]
the constraint condition of the system is derived. Holonomic constraint matrix Ch is derived from the barycentric position of each link. =0
that show this system are defined like expression (1).
[
[001
Mv=h
TABLE I =
=
When expression (2) is converted into the coordinate system of v by using expression (4), it becomes expression (5).
A model of a three-links two-joints snake-like robot PHYSICAL PARAMETERS (i
]T
(4) Expression (2) is converted into the coordinate system of a pseudo speed. Transformation matrix A that becomes q = Av becomes like expression (4).
/3
y3
Wn
WI
(7)
]T
(1) Nonholonomic constraint matrix Cnh is derived from the condition that each link in the horizontal direction without To find the equation of motion of a system that is not bound slipping. is equal to derive the equations of motion in the direction of rotation and translation of all elements of expression (1). Therefore, expression (2) is derived. qa =
81
Xl
Yl
82
Y2
x2
83
x3
Y3
(2) Generalization mass matrix Ma becomes the following.
(8) constraint procession C of the entire system becomes expres sion (8) from expression (6) and (7).
(9) C = [Ch cnh f The gravity that hangs to the center of gravity of the robot be The motion equation of the restrained system becomes expres comes g, and generalization power ha including the influence sion (9). of gravity becomes the following. (10) (3)
-72 - c1ih
+C2(ih - 8d o
ha =
-mlgsinq; ( 72 - 73 - C2 82 - 8d C3(83 - 82)
0
+
-m2gsinq; 73 - C3(83 - 82) o
However, A is an undecided multiplier of Lagrange. Here, the degree of freedom when the system is not con strained is from expression (1) to 9 degree of freedom. In this system, degree of freedom is constrained by four holonomic constraints and three nonholonomic constraints. Therefore, the degree of freedom of the system after it is constrained is two degree of freedom. The tangent speed that can move freely even after it is restrained is defined like expression (10).
-m3gsinq;
(11) 5423
Constraint matrix C is defined as C = [CI C2] , and it divides into the tangent speed and the rest. Orthogonal matrix D who fulfills C D = 0 and v = Dq becomes expression (11). 2x2 D CI IC (12) _
-
[-
] I
i
When A is deleted putting DT from the left on expression (9), and v = Dq is substituted, the motion equation of the restrained degree of freedom is obtained.
DTMDij+DTMDq=DTh
WI
When the state is assumed to x = [Xh Yh Wll ] T, the state space expression including the first position becomes expression (18).
= A(x)x+B(x)u However, A(x) and B(x) become expressions (19). A(x)= [��:� !], B(x)= [Z;2] x
(18)
(19)
Because constraint force is built into the evaluation function, (13) constraint force in the direction of a pseudo speed is derived with expression (20).
III. CONTROLL THEORY CTA= CT(CM-ICT)-IC(Dq - M-I(Eu+FGq) In order to reach the target position at the snake-like robot, (20) = aq+f3u using a method which was proposed by Watanabe, make the control system design based on the SDRE. This control is a= CT(CM-ICT)-IC(D - M-IFG) superior in the point of automatically getatable to the target point compared with the trajectory tracking control. Control f3= -CT(CM-ICT)-ICM-IE of slope climbing of the robot is considered separately in the first position control and gravity compensation control. It can be shown that the element of constraint force is the following. Generalization power h is divided like expression (13).
h=Eu+FO+K 73 73 f, e= [el
u= h e2 e3 f E, F, and K are defined as follows. E= �h , F= fJ( h -().Eu), K= h - Eu - FO fJ K is a gravity term.
CTA=
uU
fewl fCWll fCW12 few2 fCW21 fCW22 few3 fCW31 fCW32
Here, input U is defined with linear coupling u=ug+uf of gravity amends control ug and first position control U f. Input Constraint force in the direction where each link is rotated is shown in the following expressions. uf to drive the robot is derived. Matrix G defined as 0 = Gq is derived from v= Dq. Fw= [fewl few2 few3] T (21) G= [D(l) D(4) D(7)f (14) However, i means i line of orthogonalization complementary matrix D. The state space expression at the tangent speed becomes expression (15). -
-
ij=Mq+Nu M N
= (DTMD)-IDT(FG - MD) = (DTMD)-IDTE
The first position is obtained from expression (16).
Xh= Xl - dl cosel Yh= YI - dl sinel When the relation of qa expressions (17).
[Y�hh]= [WWIll],
(15)
Constraint force in the direction where each link is moved is shown in the following expressions.
Fp= [fewll
few21 few31] T (22)
Constraint force in the direction of the normal of each link is shown in the following expressions.
Fn = [Jew12
few22 few32] T
Because the snake is moving by using constraint force in the direction of the rotation and the direction of the movement, Fw and Fp are evaluated in the control. To correspond to Fw (16) and Fp, a and f3 are permuted by appropriately dividing.
Aav is used, :h, Yh become
(23) Expression (23) is converted into the shape of state x and input u.
S
(24) 5424
It thinks about the evaluation function like expression (25).
J= [:(XTQ(X)X+UTR(X)U)dt
(25)
(x)aw+a�Q3(X)ap Q(X)=Ql(X)+a�Q2 A A +f3ATp R3(x)f3pA R(x)=Rl(X)+f3TwR2(X)f3w Q and R are the state dependence type weight processions, and all states x are chosen as positive semi definite and positive definite. Input uf that minimizes expression (25) is shown by the following expressions.
= _R(X)-lB(xf P(x)x
u
Fig. 2. A previous our experimental snake-like robot. It is a wired robot, and is affected seriously by cable tension. The moving arrange is not wide due to the limitation of cable length.
P(x)
is a positive fixed symmetrical solution for the state dependent Riccati equation shown in expression (26).
A(xfP(x)+P(x)A(x)+Q(x) -P(x)B(x)R(x)-lB(x)P(x)= 0
(26)
As a result, power uf to move the snake was derived. Power ug to make amends for gravity is derived. Expression (12) is transformed, and expression (27) is obtained.
DTM Dij+DTMDq= DT +DTFO+DTK Eu
(27)
It thinks about the gravity term of expression (27). (28) The gravity term can denied expression (28), except when the singular configuration and the link where the posture of the snake-like robot becomes the straight line and a circular arc come in succession mutually. Therefore, the snake-like robot can climb the slope by combining input uf that moves and input ug that denies the gravity term. IV. A. Wireless
IMPROVEMENT OF REAL MACHINE
Fig. 3. A new snake-like robot. This machine is a wireless robot. It has batteries,motor drivers,a micro computers and a wireless module.
done by the microcomputer. To have to operate by the unit, the motor driver, the wireless telecommunications module, and the battery are installed in the snake-like robot after it makes it to the wireless besides the microcomputer. Fig. 3 shows the system that makes it to the wireless. In the present study, flash microcomputer SH7125 of the Renesas Electronics Co. is used. A wireless module uses ZEAL-C02 that is the Bluethooth module of the ADC Technology inc. because the enough high speed though angular information on the encoder and the speed instruction value to the motor driver are sent and received. Because the voltage necessary for the drive of the motor is 24V, and the voltage necessary for an encoder, a microcomputer, and a wireless module is 5V, an independent battery in two systems is installed in the snake-like robot. Seven size AA charge batteries (3. 6V) made of UltraFire are used for the power supply for the motor drive, and four size AAA alkali dry batteries on the market are used for the power supply such as encoders.
Fig. 2 shows the snake-like robot of three link two joint type used in the present study. A present system inputs the pulse that generates it by the pulse generator connected with PC to the motor driver, and drives the motor. Moreover, the turning angle of the motor is measured by counting the pulse signal from the encoder using a pulse counter. In addition, it takes a picture on the camera, and an attitude angle and the first position of the first link are measured. The first position control has been achieved by calculating the amount of the state from acquired information, and one by one undoing SDRE. It is preferable to send and receive digital data in wire less telecommunications when the system of Fig.2 is made B. Pulse counter wireless. Then, the microcomputer is installed in the snake Phase counting mode of SH7125 is used to count the pulse like robot, and the pulse count from the output signal of of the encoder with the microcomputer. Phase counting mode the encoder and the pulse sending to the motor driver are is a function to count the edge of the pulse added to the 5425
I
SH7125 MTU2 Phase counting mode
I Cycle counter
ch11 ch2
0 Encoderl
--. Encoder2 �I Motor d·nverl ch4
I
Motor driver2
I I
+
-5 Q)
