Dynamic Modeling of a Cubical Robot Balancing on Its Corner

Aiming at the problem of self-balancing control of a cubical robot, this paper makes a research on the dynamic modeling of the cubical robot balancing on its corner. Using the prototype of cubical robot we built as the research object, the dynamic model is derived with Lagrangian method on foundation of analysis of coordinate transformation relation of cubical robot. The correctness of the model is verified in theory by numerical simulation. The controller, designed based on reaction torque characteristic of inertia wheels, is used in balance control of the cubical robot. The effectiveness of the controller is verified again with the obtained expect effect. The dynamic model developed and controller can provide a base for further study of balance control of a cubical robot.


INTRODUCTION
The cubical robot is a robot device that shaped like a cube and driven by internal moment generator.The cubical robot could balancing on its edge or corner, and is a typical nonlinear, unstable and multi freedom spacial inverted pendulum, which can be used as an ideal verification platform for controlling theoretical verification [1][2][3][4].The cubical robot can jump up by the inner torque producer device.And multi cubical robot can be used for space exploration and other fields as group robot could realize self assembly.
Scholars pay much attention to the cubical robot for its novel features.In [5] researched on the cubical robot system that has an internal active pendulum installed, and a balancing controller was designed by means of fuzzy control method in 2006.Based on this system, a nonlinear reduced order state observer was designed in [6].In 2012, a cubical robot that actuated by reaction wheels mounted on three faces of the cube was proposed by Gajamohan et al7-8.In [7], [8] the dynamic model of the cubical robot both balancing on its edge and balancing on its corner was derived respectively, and the parameters of nonlinear systems were identified using frequency domain based approach while the cubical robot balancing on its edge with a nominal controller, the corner balancing using a linear feedback controller was presented.In [9], [10], dynamic model of the cubical robot balancing on its edge and balancing on its corner was derived respectively using concept of generalized momenta, and a backstepping based controller was derived for balancing.
An accurate dynamic model is the important foundation for balancing control of the cubical robot.In the above work, Gajamohan, Muehlebach, et al. derived the dynamic model of the cubical robot balancing on the corner, but the specific dynamic model hasn't been given [7][8][9][10].
Aiming at the problem of self-balancing control of a cubical robot balancing on its corner.Using the prototype of the cubical robot we built as the research object, In first place, this paper deduce the dynamic model of the cubical robot with Lagrange method, and give the specific dynamic equation.Then, the correctness of the model is verified by numerical simulation.A balancing controller is proposed based on reaction torque characteristics of reaction wheels, and is used in balancing control of cubical robot and obtained expect effect.At last, the research results are given.

CUBICAL ROBOT
The cubical robot designed in this paper as show in Fig. 1., that composed by a cube and reaction wheels mountain on three faces of the cube.Reaction torques are applied to make the cubical robot balancing on its corner by control the speed of reaction wheels.f f f the reaction wheels coordinate frame attaches at the center of the cube, and whose axes coincide with the rotation axis of reaction wheels.Next, let m and l describe the mass and the distance from mass center point c to corner point o of the cubical robot, x I , y I and z I denote the system's total moment of inertia around the corner point in the body fixed coordinate frame respectively, w I denotes the reaction wheel moment of inertia around the rotation axis in reaction wheels coordinate frame.Let  ,  and  denote the deflection angle from body coordinate frame to space coordinate frame in axes respectively,

Coordinate transformation
In system of the cubical robot, space coordinate frame o-xyz , body coordinate frame o-x y z b b b and reaction wheels coordinate frame c-x y z f f f are defined.The body coordinate frame coincide with the space coordinate frame, while the cubical robot turned  ,  and  degree relative to the axes of it respectively.Than we have the transformation matrix ob T from body coordinate frame to space coordinate frame as y ( ) ( ) ( ) The body coordinate frame coincide with reaction wheels coordinate frame while the frame turn 45°relative to axis x, 35.3°relative to axis y.Then we have the transformation matrix bf T from body coordinate frame to reaction wheels coordinate frame as with

Energy Calculation
In applying the Lagrangian modeling approach, we need to exprss the kinetic energy and potential energy of the rigid bodies as the functions of generalized coordinates [11,12].First, the kinetic energy of the cubical robot can be written as By using the mass moment of inertia of the reaction wheel, the rotational kinetic energy of the three ones is given by When the cubical robot standing up on its corner, the vertical motion happens both by the angle  and  .Thus, the potential energy of the cubical robot is cos cos (5) Now, the Lagrangian is defined by

Generalized Force Calculation
When the cubical robot is working, the reaction torques produced by the acceleration of three reaction wheels impact on the cube, and the reaction wheels are impacted by Coriolis inertia force simultaneously.The angle acceleration of cubical robot are written as  ,  and  , the angle acceleration of reaction wheels can be written as A  , B  and C  .Thus, in the body coordinate frame, the components along the axes of reaction torque impacted on cube are denoted by   ,   and   , that is In reaction wheels coordinate frame, the components along the axes of Coriolis inertia force impacted on reaction wheels are denoted by Due to control of reaction wheel motors by voltage control method base on PWM, denote the moment constant of motor as t K , the back electromotive force constant as b K , the armature resistance as m R , the armature inductance as m L , armature voltage as j v .The input torque generated by the electric motors, where i is the armature current, is obtained from the differential equation of the armature electric circuit: As some parameters are negligible, in particular the armature inductance is neglected( m =0 L ), the input torques are derived as

Lagrangian equation of motion
Denote six generalized coordinates of the system as The above Lagrangian becomes the function of the generalized coordinates.The Lagrangian equation of motion is given by ) With Now, the dynamic model of system can be derived by substituting ( 1)-( 11) into (12) and solving i q .That is With .

Simulation and Experimental
Simulation the response of system in zero input and response in situation of zero state respectively to validate the correctness of the dynamic model of system, based on the above dynamic model.

Response of Zero Input
Observe the system response of zero input, x denote the state prameres, u denote the input pramenters.forspecific: First, let the cubical robot stand up on its corner, and start motion from state that x = 0 0 [ ,0, ,0,0,0,0,0,0]   ,where there are angles 0  , 0  from the body coordinate frame to the space coordinate frame, and with no input torque.According to the physical system, without the restriction of ground, the cubical robot will rotate towards direction of the deflection angle enlargement, than, the cubical robot do damping motion about corner point o along with the energy loss of system, at last, the cubical robot stabilized in vertical downward.Simultaneously, the corresponding reaction wheels do damping motion toward inverse direction with cube under the common action of electromagnetic force and Coriolis force.Than, let the cubical robot stand up on its corner without input torque, and start motion from station where there is angle 0  along with z axis from the body coordinate frame to the space coordinate frame, both the cube and reaction wheels will keep stable.lowercase.
Simulate the above case, and the main parameters of the cubical robot prototype show as table 1.In experiment, the initial angle of the cubical robot along with axis x is 0 20 In experimental, the initial detection angle of the cubical robot along with axis y is 0 20

Response of Zero State
Observer the system response of zero state, for specific: let cubical robot stand up on its corner o and the cubical robot is in balancing condition at initial state, x [0,0,0,0,0,0,0,0,0]  , then, give step input voltage to motors.According to the physical system and neglecting the restriction of ground and impact of dead zone of motors.The physical phenomenon should be that along with the acceleration rotation of the reaction wheel motors, the cube rotate around the corner under reaction torque.As the motors reaching matching speed with input voltage, angle acceleration drop to zero, and the motor into the uniform rotation, and cube do damped oscillation around the equilibrium position.
The correspondence between the angle velocities of reaction wheels and the control torque of motors is derived by substituting (2) into (7), that It can be see from (13) that, while the angle velocity of reaction wheel A is two time of the angle velocities of reaction wheels B, C but in opposite direction, the direction of the resultant reactionary moment impact on cube is around the axis x; when the angle velocity of reaction wheel A is zero and angle velocities of reaction wheels B, C are same as each other but in opposite direction, the direction of the resultant reactionary moment impact on cube is around the axis y; when the angle velocities of reaction wheels are same in size and direction, the direction of the resultant reactionary moment impact on cube is around the axis z.
Simulation the above case, that in experiment, the cubical robot initial with balance state, and give step input of voltage It can be see that the angle velocity of the reaction wheel A accelerates to 20r/min, and the angle velocities of reaction wheel B accelerate to -10r/min after 1 second.And then the angle velocities of reaction wheels damping oscillate.Simultaneously, the cube turn 5940 degree around axis x under the action of resultant reactionary moment; and then, damped around the vertical downward.The experimental results are consistent with the physical phenomenon.
Let the cubical robot initial from balancing state, and give step input of voltage [0,0.5,-0.5](V)u  to the motors at 1 second, response of zero state show as figure 6.It can be see that the angle velocity of the reaction wheels B and C accelerate to 20r/min, but in opposite direction after 1 second, and the angle velocities of reaction wheel A keep be zero.And then, the angle velocities of reaction wheels damping oscillate.Simultaneously, the cube turn 2340 degree around axis y under the action of resultant reactionary moment; and then, damped around the vertical downward.The experimental results are consistent with the physical phenomenon.It can be see that the angle velocity of the reaction wheels A, B and C accelerate to 10r/min after 1 second, and keep stable.Simultaneously, the cube accelerated turn -25.4 degree around axis z under the action of resultant reactionary moment during 1s to 1.7s; and then, keep the uniform angle velocity around axis z.The experimental results are consistent with the physical phenomenon.
As results of the simulation of cubical robot both zero input response and zero state response.It show the consistency of experimental results and physical phenomenon and the correctness of the dynamic model.

CONCLUSION
The dynamic model of cubical robot balancing on its corner by Lagrangian method based on solve the energy function.Observer the simulation of zero state input response and zero state simulation based on the above dynamic model, the simulation results are consistent with physical phenomenon, and the correctness of the dynamic model is verified.The dynamic model developed can provide a base for further study of balance control of a cubical robot.

Figure 1 .
Figure 1.Cubical robot prototype.From the modeling point of view, the cubical robot consists of four rigid bodies with cube and reaction wheels on three faces of cube as show in Fig.2., where o-xyz denotes the Newtonian frame fixed in space, o-x y z b b b the body frame whose origin is located at corner

Figure 2 .
Figure 2. Coordinate system definition of cubical robot.
the input voltage of motors are zero, and the simulation result show as figure 3. It can be see from Fig.3.that the cube start motion with angle 20    , then, do damping motion about position of 180    , and stop toward vertical downward at last, and the detection angle  ,  keep zero all the time.Simultaneously, along with the rotation of cube, the reaction wheel A and B, C rotate toward inverse direction, then, the angle velocities of the reaction wheels damped oscillation about zero position, and reduce to zero at last.And the angle velocity of reaction wheel A A  is two time large as the angle velocity of reaction wheel B and C B  , C  .The experimental results are consistent with the physical phenomenon.

Table 1 .
Main parameters of the cubical robot prototype.


and the simulation result show as figure 4. It can be see from the figure that the detection angle  start at 20 , and damped oscillation about position of 180 at last, the detection angle  ,  keep zero all the time.Simultaneously, the angle velocities of reaction wheels B and C B  , C  are always the same as the other but in the opposite direction, and damped oscillation about zero.And the angle velocity of reaction wheel A A  keeps zero all the time.The experimental results are consistent with the physical phenomenon.

Figure 4 .
Figure 4. Response of zero input at 0 20  