clc;
clear;
close('all');
g=9.81;  Mp=1.2;  MR=0.03;  R=0.05;
JR=0.0025;  Jpsi=0.004;  Jpdel=0.02;
L=0.1;  D=0.12;  
%x'=A*x+B*U
A23=(-Mp^2*L^2*g)/(Mp*Jpsi+2*(Jpsi+Mp*L^2)*(MR+JR/R^2));
A43=(Mp^2*g*L+2*Mp*g*L*(MR+JR/R^2))/(Mp*Jpsi+2*(Jpsi+Mp*L^2)*(MR+JR/R^2));
B2=(Jpsi+Mp*L^2)/(R+Mp*L)/(Mp*Jpsi+2*(Jpsi+Mp*L^2)*(MR+JR/R^2));
B4=((-(R+L)/R)*Mp-2*(MR+JR/R^2))/(Mp*Jpsi+2*(Jpsi+Mp*L^2)*(MR+JR/R^2));
B6=(D/2*R)/(Jpdel+(D^2/(2*R))*(MR*R+JR/R));
A=[0,1,0,0,0,0;
   0,0,A23,0,0,0;
   0,0,0,1,0,0;
   0,0,A43,0,0,0;
   0,0,0,0,0,1;
   0,0,0,0,0,0];
B=[0,0;B2,B2;0,0;B4,B4;0,0;B6,-B6];
C=[0,0,0,0,0,0;
   0,1,0,0,0,0;
   0,0,1,0,0,0;
   0,0,0,1,0,0;
   0,0,0,0,1,0;
   0,0,0,0,0,0];
D=zeros(6,2);
sp=eig(A)  %求系统的开环极点
Tc=[B,A*B,A^2*B,A^3*B,A^4*B,A^5*B];  %求系统的能控性矩阵
rank(Tc)  %求能控性矩阵的秩，判断系统的能控性
matlab仿真程序运行后，Command Window中显示的运行结果如下：
由以上运行结果可知：系统存在一个sp=10.0813的不稳定开环极点，系统的能控性矩阵的秩为6，故两轮自平衡智能车系统本身是不稳定系统，且系统各状态变量均完全能控。
3.5  本章小结
    本章首先建立了智能车后轮驱动系统的数学模型，并针对两轮自平衡智能车的简化模型，分别进行了动力学、运动学建模和车模运动轨迹的讨论，其中动力学模型基于牛顿力学方程来分析。本章将两轮自平衡智能车系统在平衡点附近进行了线性化处理，从而得到了系统的状态空间方程，为后续章节电磁导引两轮自平衡智能车的算法设计提供了理论依据。本章最后依据两轮自平衡智能车的系统参数设定和所建立的数学模型，对智能车系统本身的稳定性和能控性进行了matlab仿真，证明系统各状态变量均完全能控。
4  电磁导引两轮自平衡智能车直立控制
4.1  车模角度和角速度测量
本文中控制车模直立时需要通过测量车模的倾角和角速度来控制车模车轮的转动，进而消除车模的倾角，因此，车模倾角及角速度的测量成为控制车模直立的关键。本文中测量车模倾角和角速度是通过安装在车模上的加速度计（MMA7260）和陀螺仪（ENC-03）来实现的。
MMA7260是一款三轴低g半导体加速度计，可以同时输出三个方向上的加速度模拟信号，如图4.1所示。
 
图4.1  MMA7260三轴加速度计
    为了计算出车模倾角，只需要测量Z轴方向上的加速度值。车模直立时，固定加速度计在Z轴水平方向，此时输出信号为零偏电压信号。当车模发生倾斜时，重力加速度 便会在Z轴方向形成加速度分量，从而引起该轴输出电压发生变化，变化规律为：
                        (4.1)
式（4.1）中， 为重力加速度， 为车模倾角， 为加速度计的灵敏度系数。当倾角 比较小时，输出电压的变化近似与倾角成正比。
似乎只需要通过加速度计就可以获得车模的倾角，再对此信号进行微分便可获得角速度，但在实际车模运行过程中，由于车模本身的摆动所产生的加速度会产生很大的干扰信号，它叠加在有用的车模倾角信号上使得输出信号无法准确反映车模的倾角[10]。如图4.2所示。
 
图4.2  加速度计输出的车模倾角信号
注：绿线代表加速度计输出的车模倾角信号，蓝线代表车模实际倾角 电磁导引车控制算法研究与设计+文献综述(9):http://www.751com.cn/zidonghua/lunwen_4079.html