Equations for hexapod robot control

www.hexapod-kit.com

Equations for Hexapod robot control

Determine joints angles according to the position of leg tip. A method for linear interpolation (straight walk). A method for circular interpolation (curved walk). A method for body inclination (tilt).

hexapod-kit.Equations for hexapod robot control www. The different coordinate systems A1 A3 A2 .com Determine joints angles according to the position of leg tip.

hexapod-kit. Systems a3px are centred on A3 joints of each leg. either fixed or movable relative to the body. For reasons of simplicity. The plan [XY] of the system coincides with the underside of the body (the lowest point).com We want to drive the tip of a leg by its coordinates in a Cartesian coordinate system.Equations for hexapod robot control www. we express the coordinates in a fixed coordinate system centred on the body of the hexapod. . Coordinate systems fixed to the body c et a3 : Seen from above Front System c is centred on the robot body. All angle measurements in formulas are expressed in radians. To calculate the angles to be applied to the three joints of the leg we have to use different coordinate systems.

. .hexapod-kit.q2 angle between B and the middle phalanx. .Equations for hexapod robot control Coordinate systems movable relative to the body a2: Seen top Front www.q3 angle between B and the extreme phalanx. .com Joints A2 and A1 in the [XZ] plan of coordinate system a2 : Given : .l1et l2 respectively the lengths of the extreme and middle phalanxes.q1 angle between the axis X and B imaginary line passing through the joint centre A2 and the point P.T2 angle between X axis and the middle phalanx. .T1 angle between the extreme phalanx and middle phalanx. .

Equations for hexapod robot control B=a+b B 2 = Pxa 2 2 + Pza 2 2 a cos(q 2) = l2 T 2 = q1 + q 2 www.com Find T2 a 2 = l 22 − c 2 b 2 = l12 − c 2 a 2 − b 2 = l 2 2 − l12 a 2 − ( B − a ) 2 = l 2 2 − l12 a 2 − ( B 2 + a 2 − 2aB ) = l 2 2 − l12 − B 2 + 2aB = l 2 2 − l12 B 2 + l 2 2 − l12 2B T 2 = arccos( B 2 + l 2 2 − l12 Pza 2 ) + arctan( ) 2 *l2* B Pxa 2 a= cos(q 2) = B 2 + l 2 2 − l12 2*l2* B Pza 2 q1 = arctan( ) Pxa 2 .hexapod-kit.

com Find T1 T 1 = π − q 2 − q3 a b T 1 = π − arccos( ) − arccos( ) l2 l1 a b T 1 = π − (− arcsin( ) + arcsin( ) + π ) l2 l1 a b T 1 = arcsin( ) − arcsin( ) l2 l1 a b ab cos(T 1) = cos(arcsin( ) * arcsin( )) + l2 l1 l1l 2 a2 b2 ab cos(T 1) = 1 − 2 * 1 − 2 + l1l 2 l2 l1 cos(T 1) = cos(T 1) = l 22 − a 2 l12 − b 2 ab + * 2 2 l2 l1 l1l 2 cos(a − b) = cos(a) cos(b) + sin(a) sin(b) c2 c2 ab * + 2 2 l1l 2 l2 l1 c c ab cos(T 1) = * + l 2 l1 l1l 2 c 2 + ab l 2 2 − a 2 + ab = cos(T 1) = l1l 2 l1l 2 2 2 l 2 − (a − ab) − l 2 2 + (a (a + b)) =− cos(T 1) = l1l 2 l 22 2 2 − l 2 + aB l 2 aB cos(T 1) = − = − l1l 2 l1l 2 l1l 2 2 2 2 B + l 2 − l1 2l 2 2 cos(T 1) = − + 2l1l 2 2l1l 2 2 2 2 2l 2 − B − l 2 + l12 l 2 2 + l12 − B 2 cos(T 1) = = 2l1l 2 2l1l 2 a= B 2 + l 2 2 − l12 2B T 1 = arccos( l12 + l 2 2 − B 2 ) 2l1l 2 .hexapod-kit.Equations for hexapod robot control www.

hexapod-kit.com Find joint angle A3 (T3) - Dec_X2 is the offset on X axis. c = Dec _ X 2 2 + Dec _ Y 2 2 t2 = π 2 + arctan( Dec _ X 2 ) Dec _ Y 2 b = Pxa3 2 + Pya3 2 We know that : a b c = = sin(t1) sin(t 2) sin(t 3) So : Sinus proportionality rule. between joints A3 et A2.Equations for hexapod robot control www. Dec_Y2 is the offset on Y axis. Dec_Z2 is the offset on Z axis. . Given a the projection on Xa2 of the imaginary line B (seen previously). between joints A3 et A2 (=0). between joints A3 et A2.

we have : T 3 = t 5 + t1 − t 4 .Equations for hexapod robot control c × sin(t 2) ) b c × sin(t 2) t1 = π − t 2 − arcsin( ) b t 3 = arcsin( a = b 2 − c 2 × sin(t 2) 2 + c × cos(t 2) www.com Pxa 2 = Bx = b 2 − c 2 × sin(t 2) 2 + c × cos(t 2) Pya 2 = 0 Dec _ Y 2 ) Dec _ X 2 a × sin(t 2) t1 = arcsin( ) b Pya3 t 5 = arctan( ) Pxa3 t 4 = arctan( Given T3 the angle of the joint A3.hexapod-kit.

hexapod-kit. A is the angle between X axis of the body centred system and the X axis of the A3 joint centred system. A. Dec_X1. System c is centred on the hexapod body (Y axis pointing to the front of the hexapod). Dec_Y1 et Dec_Z1 are not the same for all legs.Equations for hexapod robot control www.com Change of coordinate system (system c to a3) - System a3 is centred on A3 joint. expressed in the c system. expressed in the c system. Dec_Z1 is the altitude of a3 system centre. Dec_Y1 is the ordinate of a3 system centre. expressed in the c system (=0). Dec_X1 is the abscissa of a3 system centre. Pxa3 = Pxc × cos( A) + Pyc × sin( A) − DEC _ X 12 + DEC _ Y 12 Pya3 = −( Pyc × cos( A) − Pxc × sin( A)) - .

com Sum up Coordinate system c to a3 : Pxa3 = Pxc × cos( A) + Pyc × sin( A) − DEC _ X 12 + DEC _ Y 12 Pya3 = −( Pyc × cos( A) − Pxc × sin( A)) Pza3 = Pzc − Dec _ Z1 Coordinate system a3 to a2 : b = Dec _ X 2 2 + Dec _ Y 2 2 c = Pxa3 2 + Pya3 2 t2 = π 2 + arctan( Dec _ X 2 ) Dec _ Y 2 Pxa 2 = Bx = b 2 − c 2 × sin(t 2) 2 + c × cos(t 2) Pya 2 = 0 Pza 2 = Pz 3 − Dec _ Z 2 Joints angles : B = Pxa 2 2 + Pza 2 2 l12 + l 2 2 − B 2 ) 2l1l 2 T 1 = arccos( B 2 + l 2 2 − l12 Pza 2 T 2 = arccos( ) + arctan( ) 2 *l2* B Pxa 2 T 3 = arctan( Pya3 ) + arcsin( Pxa3 π Dec _ X 2 a × sin( + arctan( )) 2 Dec _ Y 2 Pxa3 2 + Pya3 2 ) − arctan( Dec _ Y 2 ) Dec _ X 2 .Equations for hexapod robot control www.hexapod-kit.

The three method parameters are : L : The distance. l1 = Dec _ X 1 l 2 = cos( B) × ( x min + x max − x min ) 2 x max − x min l 3 = sin( B) × ( x min + ) 2 l 4 = Dec _ Y 1 .hexapod-kit. and which angle with Yc axis is A . A : Angle between walking direction and the longitudinal axis of the hexapod (Yc). P : The unitary step of the leg (linear distance made in one walking cycle). Example for leg n°1 Front r Given V the vector which norm is equal to L.Equations for hexapod robot control www. Straight walking consist in moving with a given distance in a given direction with a given step.com A method for linear interpolation (straight walk). Another parameter is also used : G : The ground clearance (body to ground distance).

Equations for hexapod robot control www. we are able to calculate the values of the different joints for starting and arrival point thanks to the previous formulas. a ' x = ax + l1 + l 2 a ' y = ay + l 3 + l 4 d ' x = dx + l1 + l 2 d ' y = dy + l 3 + l 4 Then.com Coordinates of starting and arrival points d et a expressed in cp1 system.hexapod-kit. L Number of cycles to be performed to reach the distance is equal to P In cp1 system Z coordinate of the leg tip is equal to –G. . Remarks : Leg movement vector points in the opposite direction to the wanted body movement vector. L 2 L ay = cos( A) × 2 dx = −ax dy = − ay ax = − sin( A) × Coordinates of starting and arrival points d’ et a’ translated on c point expressed in cp1 system.

P : Unitary step of the leg (linear distance made in one walking cycle). C(xc. The three method parameters are : A : Rotation angle. Front l1 = Dec _ X 1 l 2 = cos( B) × ( x min + x max − x min ) 2 x max − x min l 3 = sin( B) × ( x min + ) 2 l 4 = Dec _ Y 1 .yc) : Coordinates of the rotation centre. Another parameter is also used : G : The ground clearance (body to ground distance).com A method for circular interpolation (curve walk).Equations for hexapod robot control www. Curve walking consists in moving of a given angle on a circle of given centre with a given step.hexapod-kit.

At first we have to determine which leg is the farthest from the rotation centre.Equations for hexapod robot control www. the farthest leg from the rotation centre will describe the longest distance in one walking cycle (Equal to P in our case). For each leg we calculate the length of CP segment : CP = (l1 + l 2 − XC )2 + (l 3 + l 4 − YC )2 For the leg which have the maximal CP. . For a given unitary angular step. we calculate the angle T represented by points a and d spaced of a distance P.com Compared to straight walking not all legs have the same movement vector.hexapod-kit. Circ = 2 × π × CP P × 2×π T= Circ P T= CP T will be the unitary angular step for all legs in order to respect the unitary linear step P. belonging to the circle.

The angle between the starting point d and X axis : Ad = arctan( www.d and a coordinates : Xd = cos( Ad ) × CP Yd = sin( Ad ) × CP Xa = cos( Aa ) × CP Ya = sin( Aa ) × CP Now we can calculate the coordinates in the body coordinate system of the starting and arrival movement vector points.The angle between the arrival point a and X axis : Aa = arctan( (l 3 + l 4 − YC ) ) + T (l1 + l 2 − XC ) 2 .hexapod-kit. .com (l 3 + l 4 − YC ) ) − T (l1 + l 2 − XC ) 2 .Equations for hexapod robot control In the coordinate system centred on the cercle we express : . we are able to calculate the values of the different joints for starting and arrival point thanks to the previous formulas. Xd = (cos( Ad ) × CP) + XC Yd = (sin( Ad ) × CP) + YC Xa = (cos( Aa ) × CP ) + XC Ya = (sin( Aa ) × CP) + YC Then.

Thus we must have the six leg extremity belonging to the same plane.0.hexapod-kit. The three method parameters are : A : rotation angle around Y. P = (0.-G) . expressed in the body coordinate system.Equations for hexapod robot control www. G : The ground clearance (body to ground distance).com A method for body inclination (tilt) Inclination consists in tilting the body of the robot of a given angle A around the longitudinal axis and a given angle B around the transversal axis. B : rotation angle around X. Calculation of the vector N normal to the Pr plan (tilted plan) X r N=Y Z X = Z/tan(A) Y = Z/tan(B) Z =1 Coordinates of the body coordinate system centre projected to the Pr plan. tilted of the desired value relative to [XY] body plan.

Given: N {a.z} a point belonging to the plan.Equations for hexapod robot control Plane equation.com Calculation of the coordinates of a leg tip in the body coordinate system.za} a known point belonging to the plan.y.(axa+bya+cza) = 0 If d=(axa+bya+cza) is the constant part of the equation. M {x. In our case d=-G www. l1 = Dec _ X 1 x max − x min ) 2 x max − x min l 3 = sin( B) × ( x min + ) 2 l 4 = Dec _ Y 1 l 2 = cos( B) × ( x min + Ex = l1 + l 2 Ey = l 3 + l 4 Ez = −G − ( Ex Ey − ) tan( A) tan( B) Then.hexapod-kit.c} vector normal to the plan. The plan equation is : ax+by+cz . we are able to calculate the values of the different joints for the starting and arrival points thanks to the previous formulas.ya. A {xa.b. .

Sign up to vote on this title
UsefulNot useful