光电平衡小车设计_单片机课程设计(编辑修改稿)

光电平衡小车设计_单片机课程设计(编辑修改稿)内容摘要：

当前状态 : )()1|1()1|( kBUkkAXkkX  （ 1） 式中 ， 101sTA ，0sTB ， ( | 1)X k k 是利用 k预测的结果 ， ( 1| 1)X k k是 k 本科生单片机课程设计报告 21 1 时刻的最优结果。 则有对应于 )1|( kkX 的协方差为： QAkkAPkkP T  )1|1()1|( （ 2） 式中 ， )1|1(  kkP 是 )1|1(  kkX 对应的协方差， TA 表示 A 的转置矩阵， Q 是系统过程的协方差，式子（ 1）、（ 2）即对系统的状态更新。 则状态 k的最优化估算值 ( | )Xk k ： ( | ) ( | 1 ) ( ) ( ( ) ( | 1 ) )X k k X k k K k Z k H X k k     （ 3） 其中 [1 0]H ,K 为卡尔曼增益： ( ) ( | 1 ) / ( ( | 1 ) )TTK k P k k H H P k k H R    （ 4） 此时，我们已经得到了 k状态下最有的估算值 ( | )Xk k ，但是为了使卡尔曼滤波器不断的运行下去直到找到最优的角度值 ， 我们还要更新 k状态下 ( | )Xk k 的协方差： ( | ) ( ( ) ) ( | 1 )P k k I K g k H P k k   （ 5） 其中 I 为单位阵 ， 对于本系统则有 11I 。 当系统进入 k+1状态时， ( | )Pk k 就是式子（ 2）的 ( 1| 1)P k k。 （ 3）、（ 4）、（ 5）式为卡尔曼滤波器状态更新方程。 计算完时间更新方程后，再重复上一次计算得到的后验估计，作为下一次的计算的先验估计，这样，周而复始、循环往复地运算下去直至找到最优的结果。 卡尔曼滤波化简 1039。 1039。 110 1 101***** / * * ** ( * )( * ) *X A X B gy roP A P A QK g P H H P H RX X K g ac c H XP I K g H P   上述五个公式便是卡尔曼滤波的五条数学公式在本项目中的使用。 但是，以上五个公式，仅仅只是矩阵形式。 虽然可以在 MATLAB 里进行仿真，但是却不能使用单片机进行有效运算。 然后，将卡尔曼滤波矩阵形式转化为方程形式： 本科生单片机课程设计报告 22 11 01 0212 0211 01 03 02 04 112 02 0413 03 0414 04 21 11 112 13 1101 11 102 12 103 13 11 204 14 12 201 11*** * ( * )**/ ( )/ ( )* ( 1 )* ( 1 )**(X X dt gy r o dt XXXP P dt P dt P dt P QP P dt PP P dt PP P QK g P P RK g P P RP P K gP P K gP P P K gP P P K gX X a      11 102 12 11 2)*( ) *c c X K gX X ac c X K g   这里， X01即为我们需要的角度卡尔曼滤波值。 可以看到的是，卡尔曼滤波方程形式共有 14个公式，同时很多参数的运算涉及浮点数，这对于单片机的高效运行时极为不利的，因此我们需要通过一些方法对卡尔曼滤波的公式进行化简，从而在不失精度的情况下，尽可能减小运算量。 需要注意的是， Q,R两个参数是关于传感器和系统的方差，他们随着系统的工作状况不同而会产生相应变化，对应到我们的系统，在车模运行状态不同（倾角不同， PWM不同）情况下， Q,R都是不同的。 根据相关参考文献， Q,R虽然都是关于时间的变量，但是由于卡尔曼滤波有很好的收敛性，所以可以将 Q， R都取比较极端的参数。 用常量来定义。 然后再看下 图 注意方框中的公式，根据我们的观察，不难发现，整个方框中都是为了获得卡尔曼增益（矩阵 Kg），我们设想，能否使用一个常数来等效替代卡尔曼增益那。 根据我们在实验 本科生单片机课程设计报告 23 中的观察，卡尔曼增益是一个收敛的变量，并且针对到我们的这个系统，他的值非常小，直立状态下趋近于一个常数。 （所有这些工作都在 MATLAB下完成） 所以，我们将方框中的所有公式 完全省略，通过实验整定，选取一个近似 Kg来替代方框中的所有运算。 同时，通过 NATLAB观测各变量的变化趋势，我们尝试 让 Kg2=0（ Kg1， Kg2本身就是非常小的变量，所以可以让其等于 0）。 并发现 Kg2对于整个卡尔曼滤波的影响非常有限。 所以将 Kg2设定为 0。 以下就是我们组程序中卡尔曼滤波的 简化 算法实现 X1=X0+gyro*dt。 X0=X1+(accX1)*kg。 化简后的卡尔曼滤波框图如下： 卡尔曼滤波参数整定 化简后的卡尔曼滤波主要是对 dt以及 Kg两个参数进行整定。 需要特别指出的是，这里的 dt 并不只是一个采样间隔。 由于陀螺仪的输 出和加速度计输出的量纲并不相同，所以陀螺仪采样值 *dt 并不直接反应一个角度，而是与实际角度相差一个系数。 因此此处的 dt可以等效理解为 dt=采样间隔 *比例系数。 dt 越大，积分速度越快，卡尔曼输出追随实际角度的情况越好（当然不能太大，不然可能会出现超前相位）。 但是 dt越大，陀螺仪漂移造成的影响也就越大。 Kg 决定了加速度计的权重。 Kg 越大，实际输出的漂移就越小，但是滤波效果的噪声也就越大。 所以 dt和 kg是一对矛盾， 不能太过于极端。 首先是静态整定。 将车模保持在稳定直立状态，让车轮以恒定 PWM（ 80%以上）转动，然后调节参数。 本科生单片机课程设计报告 24 目标是达到上图效果。 红色为卡尔曼滤波输出，蓝色为加速度计直接输出。 如果红色抖动非常厉害，可以适当减小 Kg的大小。 接着是动态整定 还是保持车轮恒定 PWM旋转，同时，摇摆车身。 大致调节到上图所示的样子。 目前存在一个相位差，这个相位差 在后期直立算法的调试中 是致命的，必须克服掉。 解决方法，逐渐增加 dt 即可。 达到上图效果即可认为卡尔曼滤波参数整定完成。 本科生单片机课程设计报告 25 第六章 程序控制说明 在完成了信号处理的层面之后，就是对于控制算法的设计。 在设计算法之前，让我们对于系统进行一个简单分析。 系统虽然是 高阶非线性系统 ，但是由于直立期间，倾斜角度非常小，所以可等等效为一个线性系统。 控制输入有三个，分别对应角度，角速度，车轮转速。 被控变量有两个：角度以及车轮转速。 所以这是一个 MIMO 系统，因此不能使用简单的 PID等 SISO 控制器进行控制。 同时，无论是控制角度还是控制电机，执行机构都为电机，所以系统也具有强耦合性。 对于倒立摆的控制，参考的文献中有使用各种算法进行控制，比如状态反馈、自适应算法、神经网络算法、模糊算法等等。 这些算法都需要更高深的理论基础，同时，在高阶倒立摆系统中，这些算法拥有不可替代性。 但是对于我们的两轮车式倒立摆，这仅仅只是一个一阶倒立摆，可以使用更简单的方法进行控制。 我们在本设计中使用了角度、速度双闭环的控制算法。 角度控制频率高 ，速度控制 周期频率慢。 电机死区补偿 由于摩擦力等因素的存在，电机与 PWM给定信号存在非线性死去特性。 为了使系统具有更好的线性，首先需要对电机死区进行补偿。 角度单闭环控制 首先我们忽略速度控制，仅仅只考虑角度闭环系统。 那么在这个前提下，只有两个速度量：角度和角速度；被控变量只有角度。 那么就可以使用 PID 控制器。 这里需要进行解释，为何有 两个输入变量 任然可以使用 SISO 系统 才能使用 PID。 虽然角度和角速度是不同的量，但是角速度就是对角度求微分。 而使用陀螺仪获得的角速度比对卡尔曼滤波后求微分获得的角速度更准确，而 D 控制器输入变量正好是 角速度 ，所以此处使用 PID 完全没有问题。 常用的 PID 控制器及其变形有 P 控制器、 PI 控制器、 PD控制以及 PID控制器。 分析系统要求， 仅有 P 控制器，系统极易发散造成不稳定，而积分环节对于这个需要快速调整的 本科生单片机课程设计报告 26 系统，要求不大。 因此最后我们选择了 PD 控制器。 控制系统框图如下： 在实际调试过程中，倒立摆可以进行短时间的直立，但是会朝向一个方向加速运行，最后车模会倒下。 这是由于未加速速度闭环的结果，未对车轮转速进行必要的控制，则最终由于电机转速的饱和，而发生了无法对倒立摆进行调整的后果。 为了解决这个问题，必须要引入速度闭环控制。 角度、速度双闭环控制 对于直立车模速度的控制相对于普通车模的速度控制则比较复杂。 由于在速度控制过程中需要始终 保持车模的平衡，因此车模速度控制不能够直接通过改变电机转速来实现。 下面先分析一下引起车模速度变化的原因。 假设车模在上面直立控制调节下已经能够保持平衡了，但是由于安装误差，传感器实际测量的角度与车模角度有偏差，因此车模实际不是保持与地面垂直，而是存在一个倾角。 在重力的作用下，车模就会朝倾斜的方向加速前进。 首先对一个简单例子进行分析。 假设车模开始保持静止，然后增加给定速度，为此需要车模往前倾斜以便获得加速度。 在车模直立控制下，为了能够有一个往前的倾斜角度，车轮需要往后运动，这样会引起车轮速度下降（因为车轮往负方向运动了）。 由于负反馈，使得车模往前倾角需要更大。 如此循环，车模很快就会倾倒。 原本利 用负反馈进行速度控制反而成了 “ 正 ” 反馈。 在直立控制下的车模速度与车模倾角之间传递函数具有非最小相位特性，在反馈控制下容易造成系统的不稳定性。 但根据实际经验，是可以通过速度控制进行车模倾角控制的。 而速度闭环控制仍然可以看 做是一个 SISO系统。 所以使用 PID控制器来完成控制。 由于系统对于静态误差存在一定要求，所以使用 PI控制器。 正如前文所述， 直立控制和速度控制 存在 强耦合 ，所以这里我们使用的分立控制周期的方法。 每 10个直立控制周期 进行一次速度控制。 这样，可以将速度控制视为对于直立控制的一种扰动。 下图为速度闭环的控制框图。 本科生单片机课程设计报告 27 本科生单片机课程设计报告 28 图 直立、速度闭环控制框图 本科生单片机课程设计报告 29 第七章车模参数调试图解 在完成了所有控制算法方案的制定，以及程序代码的编写之后，接下来需要对一些参数在实际系统运行中进行整定以及微调。 整定方法遵从先 直立控制 再 速度控制 ； 先 P 参数，后 I 参数，最后 D 参数的整定顺序。 本章节着重介绍在调试环节中不同现象对应的原因以及参数调整方法。 在下文中，会使用 如下几个变量在此说明。 dt:陀螺仪积分参数 Kg：加速度计参数 （卡尔曼增益） 角度闭环，速度 开环 P：直立算法 P参数 角度闭环，速度 开环 D：直立算法 D参数 速度 闭环 P：速度控制 P参数 速度 闭环 I：速度控制 I参数 原因：开环 P过大，系统震荡 解决方法：减小开环 P或者增加开环 D 原因：开环 D参数过大，系统抗干扰性降低 解决方法：减小开环 D，或者适当增加开环 P 本科生单片机课程设计报告 30 原因：倾角值为 0时刻，车身重心不在轴线上 解决方法：微调加速度计零偏置，使得角度为 0时，重心落在 轴线上。 也可能是陀螺仪零偏置错误，同理，微调即可。 原因：陀螺仪积分速度过慢，导致卡尔曼滤波相位滞后 解决方法：适当增加 dt 原因：速度闭环对系统整体的控制程度过小 解决方法：适当增加速度闭环的各个参数 本科生单片机课程设计报告 31 原因：速度闭环调节速度过慢 解决方法：适当增加闭环 P参数 原因：速度闭环存在静态误差 解决方法：适当增加闭环 I参数 原因：速度闭环调节速度过慢 解决方法：适当增加闭环 P参数，或者也可以使用 PID（而非 PI）进行速度闭环的控制 本科生单片机课程设计报告 32 原因：速度控制对于阶跃响应存在超调 解决方法：减小闭环 P参数，或者加入闭环 D参数，也可以不用阶跃信号，而是通过。