Modeling and Control of a Segway

Size: px

Start display at page:

Download "Modeling and Control of a Segway"

Ada Mathews
5 years ago
Views:

I. Model Modeling and Control of a Segway 4#56&7#68& "9&7#68& % " &#' " # $ " #! " #! "#$% &!

& Figure : Schematic of Segway The figure above shows a Segway vehicle consisting of

Also included in the schematic is a rigid cylindrical model of a human, H.

The orientation of B in N is determined by first aligning ai with ni (i = x,y,z) and

Next, bi is aligned with ai (i = x,y,z) and then B is subjected to a rotation

The wheels are attached to the platform, B, by frictionless joints.

The human, H, is attached to the platform through a frictionless revolute joint at

To facilitate this analysis, right- handed sets of unit vectors are fixed in N, B,

1 I. Model Modeling and Control of a Segway 4#56&7#68& "9&7#68& % " &#' " # $ " #! " #! "#$% &! '()*+ & & -& % ( # '& ' 3) &,& /& & ' ( #.& ' ( # % ) # & / 3) & $ ( # $ ) #.& Figure : Schematic of Segway The figure above shows a Segway vehicle consisting of a rigid platform B and two rigid wheels C and D. Also included in the schematic is a rigid cylindrical model of a human, H. An intermediate frame, A, exists between the rotation from N to B. The orientation of B in N is determined by first aligning ai with ni (i = x,y,z) and then subjecting A to a right- handed rotation characterized by - θturnnz. Next, bi is aligned with ai (i = x,y,z) and then B is subjected to a rotation characterized by - θtiltbx. The wheels are attached to the platform, B, by frictionless joints. The wheels are assumed to be rolling on N. The human, H, is attached to the platform through a frictionless revolute joint at Bcm. This joint is meant to model the human ankle. To facilitate this analysis, right- handed sets of unit vectors are fixed in N, B, and H respectively, with: nx horizontally- right ny in forward direction nz in upward direction ax parallel to the axis of rotation of C in B and D in B ay in heading direction of Segway az = nz in upward direction bx = ax parallel to the axis of rotation of C in B and D in B bz normal to the top surface of the Segway platform

2 by = bz x bx hx = bx = ax parallel to the axis of rotation of H at Bcm hz directed from Ho to Hcm hy = hz x hx Modeling Assumptions All bodies involved are rigid The revolute joints are frictionless There is no slop or flexibility in the revolute joints The human will be moving slow enough relative to the platform to ignore its dynamics Assume the human can be modeled as a solid cylinder Objectives Find a stable control law using a state space design. Demonstrate stability in response to an initial condition offset and the ability to maneuver in the x- y plane of N. II. Identifiers The following identifiers are useful for analyzing this system. Table : Identifier Table Description Symbol Type Value Distance from B cm to H cm L Constant m Width of wheel base L w Constant.6m b z measure of the length of the platform L Constant.5m b x measure of the length of the platform L Constant.4m b y measure of the length of the platform L 3 Constant.4m Mass of platform m B Constant 4.5kg Radius of the wheels R Constant.38m Thickness of the wheels h Constant.76m Mass of one wheel m W Constant.7kg Mass of human m H Constant 8kg "Radius" of human r H Constant.m Height of human H H Constant.8m Local gravitational constant g Constant 9.8 m/s b x measure of the angle between b z and h z (Lean Angle)! Human Specified b x measure of the control torque applied to wheel C T c Specified b x measure of the control torque applied to wheel D T d Specified n x measure of B cm s position vector from N o x Variable n y measure of B cm s position vector from N o y Variable b x measure of the angle between n z and b z! Tilt Variable n z measure of the angle between n y and a y! Turn Variable a y measure of B cm s velocity in N v Variable Time t Independent Variable

3 III. Physics Control Law Design To design the controller and determine feedback gains, the segway was first analyzed in only one dimension. Motion Genesis was employed to along with Kane s method to find the governing equations for the motion variables θ Tilt and v. Even the one- dimensional equations of motion for the segway are highly nonlinear, coupled, and long. To use a linear control method like state space, one must first linearize the equations of motion about a desired equilibrium point. For this case, the unstable, upright equilibrium is chosen. After linearization the state equations for the one- dimensional segway take the form θ Tilt θ Tilt = A v v θ Tilt θ Tilt + BT control + Fθ Human where θ Tilt, θ Tilt, v are the state variables; A, B, and F are coefficient matrices; Tcontrol is the control torque applied to the wheels, and θ Human is the lean angle of the human. By placing the 3 system poles at desired locations and reiterating, a suitable state feedback gain vector, K, was found to be K = [ ] Controller Implementation The control torque was then integrated into the 3D dynamic model of the seqway in order to test the performance of the controller in the real system. Once again, Kane s method was used to determine the equations of motion for θ Tilt, ωturn, and v. Kinematical differential equations were also found to provide a means to calculate the trajectory variables x and y. The.al file is attached in the appendix for reference. IV. Simplify and Solve Motion Genesis was used to generate MATLAB code for solving the equations of motion. Three different cases were tested:. Initial error in θ Tilt to test the stability and effectiveness of the controller.. Step input for θ Human to see if the system is stable for disturbances 3. Specify a varying θ Human and also command the segway to turn. 3

4 IV. Interpret and Design The results for Case # Stability Test are plotted below..5 State Variables! Tilt dt! Tilt v (m/s) Figure : Response of all three state variables to an initial condition of θ Titlt = deg. If given an initial offset angle and no forcing input, the controller can successfully compensate as shown in Figure. The segway moves forward to counteract the offset angle; notice how the velocity increases slightly in Figure. The state variables eventually come to rest because there is no forcing input. The settling time on the linear velocity state is a slower than the other two. This is not exactly ideal; the user will want to come to rest as soon as possible. The next iteration on the design could attempt to find different gains that accomplish a faster settling time for v. 4

5 The results for Case # Human Lean Response are plotted below..5 State Variables! Tilt dt! Tilt v (m/s) Figure 3: State variable response to step input in θ Human = deg. As the rider leans forward, his/her center of mass creates a disturbance torque. The controller will respond by increasing the torque to the wheels, thereby increasing linear forward velocity. So long as the rider mass is off center, the velocity will continually increase just as seen in Figure 3 above. Also note that the Tilt Angle is decreasing, which indicates the rider center of mass is moving towards equilibrium once again. The graph indicates the controller is stable when given a step input from the rider. One may wonder, as I did, why the segway can be controlled with a control law that was based upon a one- dimensional model. I started this project by developing a controller based on the 3D model of the segway; however, I came to the realization that force exerted at the base of the pendulum from turning about nz is not significant compared to the control torque as long as the turning is relatively slow (which it should be for safety). Thus, the segway can be steered by slightly lowering the torque on one wheel while slightly raising it on the other. 5

6 The results for Case #3 Full Function are plotted below.! Tilt dt! Tilt State Variables X Y Trajectory of Midpoint.5 v (m/s) 8.5 Y Position (m) Figure 4: Response of state variables X Position (m) Figure 5: Trajectory of platform CM (Bcm) 4 Motor Torques 3 x 4 Energy Check Should be Zero Right Motor Left Motor Torque (n m) dt(ke) Power Figure 6: Time history of control torque for each motor Figure 7: Energy check: dt(ke) Power = Const For Case #3 the human lean angle was specified to be a sinusoid with period sec; therefore, the angle started at zero and ended at zero over the ten second simulation time. The response of the three state variables is given in Figure 4. Similar to what was seen in Case #, the velocity increases with human lean angle and then begins to decrease as human tilt angle decreases. Once more, the system is stable because θtilt in Figure 4 is decreasing towards - degrees, which means the human is almost upright. To show the turning capability of the segway, the trajectory of Bcm is 6

7 plotted in Figure 5. Note that the steering command is less influential at higher linear speeds and more influential when the platform is moving slow. If one were to build a segway, the max torque would need to be known. Figure 6 shows for this run the max torque was - 6Nm. Finally, to double- check the integrator results, a plot of the time derivative of kinetic energy minus power is shown. This energy expression should remain zero throughout the simulation. Figure 7 shows that the expression remains essentially at zero. 7

8 Appendix A MG Code % File: Proj.al % Problem: Dynamics and Control of a Segway % Newtonian, bodies, frames, particles, points NewtonianFrame N % Newtonian reference frame RigidFrame A % Intermediate Frame RigidBody B % Platform and Human RigidBody H % Human Body RigidBody C, D % Wheels Point Cn(C) % Point of contact between C and N Point Dn(D) % Point of contact between D and N % Variables, constants, and specified MotionVariable' qturn'', qtilt'', wc', wd', v' Variable x'', y'' Constant L % distance from Bo to Center of mass of B Constant LW =.6 % Width of wheel base Constant L =.5 % Height of platform Constant L =.4 % width of platform Constant L3 =.4 % depth of platform Constant R =.38 % Radius of wheels Constant h =.76 % Thickness of wheels Constant mw =.7 % Mass of one wheel Constant mh % mass of human Constant rh % radius of human Constant hh % height of human Constant mb = 4.5 % mass of platform Constant g = 9.8 % Gravitational acceleration Constant k{3} % Control gains Specified Turn =.5 % Turning factor Specified qhuman'' % Human Lean Angle disturbance qhuman = Specified Tcont = k*qtilt + k*qtilt' + k3*v Specified Tc = Tcont + Tcont*.*Turn Specified Td = Tcont - Tcont*.*Turn % Mass and inertia Constant ICxx = (/)*mw*r^ Constant ICyy = (/)*mw*(3*r^+h^) Constant ICzz = (/)*mw*(3*r^+h^) Constant IBxx = (/)*mb*(l^ + L3^) Constant IByy = (/)*mb*(l^ + L3^) Constant IBzz = (/)*mb*(l^ + L^) Constant IHxx = (/)*mh*(3*rh^+hh^) Constant IHyy = (/)*mh*(3*rh^+hh^) Constant IHzz = (/)*mh*rh^ B.SetMass(mB) % Mass of platform + Human H.SetMass(mH) % Mass of Human C.SetMass(mW) % mass of wheel D.SetMass(mW) % Mass of wheel B.SetInertia( Bcm, B, IBxx, IByy, IBzz) % Inertia of Platform H.SetInertia( Hcm, H, IHxx, IHyy, IHzz) % Inertia of Human C.SetInertia( Ccm, B, ICxx, ICyy, ICzz) % Inertia of Wheel D.SetInertia( Dcm, B, ICxx, ICyy, ICzz) % Inertia of wheel 8

9 % Rotational kinematics A.RotateZ(N, qturn) B.RotateNegativeX(A, qtilt) H.RotateNegativeX(B, qhuman) 9 C.SetAngularVelocityAcceleration( B, -wc*bx> ) D.SetAngularVelocityAcceleration( B, -wd*bx> ) % Translational kinematics Bo.Translate( No, x*nx> + y*ny> ) Bo_Vel> = v*ay> Kin[] = dot(v_bo_n> - Bo_Vel>, Ny>) Kin[] = dot(v_bo_n> - Bo_Vel>, Nx>) Solve(Kin, x', y') % Kinematicall Diff Eqs Bo.SetVelocityAcceleration(N, v*ay>) Bcm.Translate(Bo, >, B) Hcm.Translate(Bo, L*Hz>) % Position of the human CM Ccm.Translate( Bo, (L/)*Bx> ) % wheel CM Cn.Translate(Ccm, -R*Nz>, C) % Contact point Dcm.Translate( Bo, -(L/)*Bx> ) Dn.Translate(Dcm, -R*Nz>, D) % Motion constraints Dependent[] = Dot(V_Cn_N>, Ay>) Dependent[] = Dot(V_Dn_N>, Ay>) Constrain(Dependent[wC,wD]) % Forces System.AddForceGravity( -g*nz> ) C.AddTorque(B, Tc*Bx>) D.AddTorque(B, Td*Bx>) % Equations of motion Zero = System.GetDynamicsKane() solve(zero, qtilt'', qturn'', v') % Energy Check KEdt = dt(system.getkineticenergy()) Power = System.GetPower() EnergyCheck = KEdt - Power % Integration parameters and values for constants and variables Input tfinal=5, integstp=.5, abserror=.e-8, relerror=.e-8 Input k=-89, k=-68.9, k3= -66. Input L =, mh = 8, rh =., hh =.8 Input qtilt= deg, qtilt'= rad/sec, v= m/sec Input qturn = deg, qturn' = Input x = m, y = m % Quantities to be output from CODE Output t sec, x m, y m, v m/s, qhuman deg, qtilt deg, qtilt' deg/s, qturn' deg/s, Tc newtons/m, Td newtons/m, EnergyCheck J % Numerical solution Code ODE() Project.m Save Project_Real.all

Robot Control Basics CS 685

Robot Control Basics CS 685 Control basics Use some concepts from control theory to understand and learn how to control robots Control Theory general field studies control and understanding of behavior