CN109709796B - 一种惯性轮自平衡方法 - Google Patents
一种惯性轮自平衡方法 Download PDFInfo
- Publication number
- CN109709796B CN109709796B CN201811590519.4A CN201811590519A CN109709796B CN 109709796 B CN109709796 B CN 109709796B CN 201811590519 A CN201811590519 A CN 201811590519A CN 109709796 B CN109709796 B CN 109709796B
- Authority
- CN
- China
- Prior art keywords
- swing rod
- error
- flywheel
- speed
- value
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 28
- 230000033228 biological regulation Effects 0.000 claims abstract description 18
- 238000001914 filtration Methods 0.000 claims description 28
- 238000005259 measurement Methods 0.000 claims description 8
- 230000001105 regulatory effect Effects 0.000 claims description 6
- 230000010354 integration Effects 0.000 claims description 5
- 230000001276 controlling effect Effects 0.000 claims description 4
- 230000000087 stabilizing effect Effects 0.000 claims 1
- 238000013178 mathematical model Methods 0.000 abstract description 4
- 238000005070 sampling Methods 0.000 description 14
- 230000001133 acceleration Effects 0.000 description 12
- 230000000694 effects Effects 0.000 description 5
- 230000005484 gravity Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 230000000295 complement effect Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 238000005381 potential energy Methods 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 238000012935 Averaging Methods 0.000 description 1
- 230000004075 alteration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000009193 crawling Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000002401 inhibitory effect Effects 0.000 description 1
- 230000005764 inhibitory process Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 101150001852 mrdB gene Proteins 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000000737 periodic effect Effects 0.000 description 1
- 230000002572 peristaltic effect Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 101150042737 rodA gene Proteins 0.000 description 1
- 238000009987 spinning Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000009182 swimming Effects 0.000 description 1
Images
Landscapes
- Magnetic Bearings And Hydrostatic Bearings (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种惯性轮自平衡方法，涉及自平衡技术领域，采用拉格朗日法建立惯性轮自平衡系统的数学模型，将惯性轮自平衡系统抽象为匀质的摆杆和转动连接在摆杆顶部的飞轮，飞轮通过电机驱动形成竖直平面内的反力矩；采用三串级PID控制模块实现对摆杆倾斜角度的控制，使摆杆的倾斜角度、倾斜角速度及飞轮的转速作为系统的反馈，以摆杆的倾斜角速度作为系统的内环，摆杆的倾斜角度作为系统的中间环，飞轮的转速作为系统的外环，对飞轮的转速进行控制；针对惯性轮自平衡机器人的高阶、多变量、非自稳特点，本发明惯性轮自平衡系统设计了三串级控制系统，由此提高了系统抗扰能力，加快了系统的自稳调节速度，实现系统的稳、准、快地控制目标。
Description
技术领域
本发明涉及自平衡技术领域，具体为一种惯性轮自平衡方法。
背景技术
移动机器人系统集环境感知、动态决策与规划、行为控制与执行等功能于一体，目前已广泛应于工业、农业、医疗、航天、娱乐等各种领域。根据机器人移动方式的不同，可分为：轮式机器人、腿式机器人、履带机器人、爬行机器人、蠕动机器人和游泳式机器人等类型。与多轮机器人相比，惯性轮自平衡系统是类似单轮自行车的轮式机器人系统，它具有成本低、车体结构简单、能量利用率高、占地面积较小，能最大限度地减少系统与地面之间的接触点，绕本体中心旋转和以任意半径转向，具有广阔的发展前景。惯性轮自平衡机器人通过惯性轮的旋转产生反力矩实现其自身的平衡控制，它是一类动态稳定的系统，其动力学模型具有多变量、高阶、强耦合、时变等非线性特点。因此，针对惯性轮自平衡机器人，研究并实现其平衡控制的方法和技术，是实现各种环境下惯性轮自平衡机器人的控制任务要求的首要要求，是其实际应用的有效保障，如轨迹跟踪、路径规划、自主行走避障等，具有重要的理论价值和现实意义。
发明内容
本发明的目的在于提供一种惯性轮自平衡方法，以解决上述背景技术中提出的问题。
为实现上述目的，本发明提供如下技术方案：一种惯性轮自平衡方法，其特征在于，采用三串级PID(proportion integral differential、比例积分微分)控制模块实现对摆杆倾斜角度的控制，所述三串级PID控制模块包括惯性轮转速环PID控制器、系统机体角度环PID控制器、系统机体角速度环PID控制器、电机和传感器；包括以下步骤:
(Ⅰ)采用拉格朗日法建立惯性轮自平衡系统的数学模型，即，将惯性轮自平衡系统中除惯性轮以外的部分等效为匀质的摆杆，摆杆竖直放置且底部与支撑面转动连接，并将惯性轮等效为转动连接在摆杆顶部的飞轮，电机驱动飞轮转动形成竖直平面内的反力矩；
(Ⅱ)通过传感器测量并计算得出摆杆倾斜角度的实际值θr、摆杆倾斜角速度的实际值
及飞轮转速的实际值
(Ⅲ)以设定的飞轮转速的期望值
作为惯性轮转速环PID控制器的输入，以飞轮转速的实际值
作为惯性轮转速环PID控制器的反馈输入，形成负反馈，并通过PID控制，使惯性轮转速环PID控制器输出摆杆倾斜角度的补偿值θc；
(Ⅳ)以设定的摆杆倾斜角度的期望值θe作为系统机体角度环PID控制器的输入，以摆杆倾斜角度的实际值θr作为系统机体角度环PID控制器的反馈输入，以摆杆倾斜角度的补偿值θc作为系统机体角度环PID控制器的补偿输入，形成负反馈，并通过PID控制，使系统机体角度环PID控制器输出摆杆倾斜角速度的补偿值
(Ⅴ)以摆杆倾斜角速度的实际值
作为系统机体角速度环PID控制器的反馈输入，以摆杆倾斜角速度的补偿值
作为系统机体角速度环PID控制器的补偿输入，形成负反馈，并通过PID控制，使系统机体角速度环PID控制器输出飞轮转速的控制值
(Ⅵ)将飞轮转速的控制值
送入电机，电机驱动飞轮转动并使飞轮的转速变为
产生适合惯性轮自平衡系统稳定所需的转矩，使惯性轮自平衡系统保持平衡。
进一步的，惯性轮转速环PID控制器包括第一积分环节和第一比例环节；步骤(Ⅲ)中，惯性轮转速环PID控制器算出飞轮转速的误差e1，飞轮转速的误差e1按照下式求出：
飞轮转速的误差e1作为第一积分环节的输入，使第一积分环节输出飞轮转速误差的积分值e1i,飞轮转速误差的积分值e1i由下式求出：
其中，KI1是设定的惯性轮转速环PID控制器的积分调节系数，t0是惯性轮自平衡系统开始工作时的时刻，t1是当前时刻，τ是代表时刻的变量，e1(τ)是在τ时刻求出的飞轮转速的误差；
飞轮转速误差的积分值e1i作为第一比例环节的输入，并由第一比例环节算出摆杆倾斜角度的补偿值θc；摆杆倾斜角度的补偿值θc由下式算出：
θc＝(KP1*e1+e1i)/100；
其中，KP1是设定的惯性轮转速环PID控制器的比例调节系数。
进一步的，第一积分环节算出飞轮转速误差的积分值e1i后，对飞轮转速误差的积分值e1i进行限幅滤波；当飞轮转速误差的积分值e1i的绝对值大于100时，将飞轮转速误差的积分值e1i的绝对值更新为100，使飞轮转速误差的积分值e1i保持在正负100之间。
进一步的，第一比例环节算出摆杆倾斜角度的补偿值θc后，对摆杆倾斜角度的补偿值θc进行限幅滤波；当摆杆倾斜角度的补偿值θc的绝对值大于5时，将摆杆倾斜角度的补偿值θc的绝对值设为5，使摆杆倾斜角度的补偿值θc保持在正负5之间。
进一步的，系统机体角度环PID控制器包括第二积分环节和第二比例环节；步骤(Ⅳ)中，系统机体角度环PID控制器算出摆杆倾斜角度的误差e2，摆杆倾斜角度的误差eθ按照下式求出：
e2＝θe+θc-θr；
摆杆倾斜角度的误差e2作为第二积分环节的输入，使第二积分环节输出摆杆倾斜角度误差的积分值e2i，摆杆倾斜角度误差的积分值e2i由下式求出：
其中，KI2是设定的系统机体角度环PID控制器的积分调节系数，t0是惯性轮自平衡系统开始工作时的时刻，t1是当前时刻，τ是代表时刻的变量，e2(τ)是在τ时刻求出的摆杆倾斜角度的误差；
摆杆倾斜角度误差的积分值e2i作为第二比例环节的输入，并由第二比例环节算出摆杆倾斜角速度的补偿值
摆杆倾斜角速度的补偿值
由下式算出：
其中，KP2是设定的系统机体角度环PID控制器的比例调节系数。
进一步的，第二积分环节算出摆杆倾斜角度误差的积分值e2i后，对摆杆倾斜角度误差的积分值e2i进行限幅滤波；当摆杆倾斜角度误差的积分值e2i的绝对值大于100时，将摆杆倾斜角度误差的积分值e2i的绝对值更新为100，使摆杆倾斜角度误差的积分值e2i保持在正负100之间。
进一步的，系统机体角速度环PID控制器包括第三积分环节和比例微分环节；步骤(Ⅴ)中，系统机体角速度环PID控制器算出摆杆倾斜角速度的误差e3，摆杆倾斜角速度的误差e3按照下式求出：
摆杆倾斜角速度的误差e3作为第三积分环节的输入，使第三积分环节输出摆杆倾斜角速度误差的积分值e3i，摆杆倾斜角速度误差的积分值e3i由下式求出：
其中，KI3是设定的系统机体角速度环PID控制器的积分调节系数，t0是惯性轮自平衡系统开始工作时的时刻，t1是当前时刻，τ是代表时刻的变量，e3(τ)是在τ时刻求出的摆杆倾斜角速度的误差；
摆杆倾斜角速度误差的积分值e3i作为比例微分环节的输入，并由比例微分环节算出飞轮转速的控制值
飞轮转速的控制值
由下式算出：
其中，KP3是设定的系统机体角速度环PID控制器的比例调节系数,KD3是设定的系统机体角速度环PID控制器的微分调节系数，e3(t2)是上一次测量时得到的摆杆倾斜角速度的误差。
进一步的，第三积分环节算出摆杆倾斜角速度误差的积分值e3i后，对摆杆倾斜角速度误差的积分值e3i进行限幅滤波；当摆杆倾斜角速度误差的积分值e3i的绝对值大于100时，将摆杆倾斜角速度误差的积分值e3i的绝对值更新为100，使摆杆倾斜角速度误差的积分值e3i保持在正负100之间。
与现有技术相比，本发明的有益效果是：
1.针对惯性轮自平衡机器人的高阶、多变量、非自稳特点，本发明惯性轮自平衡系统设计了三串级PID控制系统，在现有的系统机体角度环PID控制器、系统机体角速度环PID控制器之外还加入了惯性轮转速环PID控制器，由此提高了系统抗干扰能力，加快了系统的自稳调节速度，实现系统的稳、准、快地控制目标。
2.本发明惯性轮自平衡系统对第一积分环节、第二积分环节、第三积分环节输出的积分值进行限幅滤波，对积分漂移进行了限制；同时对第一比例环节输出的摆杆倾斜角度的补偿值进行限幅滤波，提高了惯性轮自平衡系统的抗干扰能力。
附图说明
图1是本发明惯性轮自平衡方法的控制系统的三级PID控制模块的结构图；
图2是本发明惯性轮自平衡方法中的惯性轮自平衡系统通过拉格朗日法建立的数学模型；
图3是本发明惯性轮自平衡方法的惯性轮自平衡系统静态稳定下控制效果图；
图4是本发明惯性轮自平衡方法的惯性轮自平衡系统动态稳定下控制效果图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明提出了一种惯性轮自平衡方法，采用如图1所示的三级PID控制模块实现对摆杆倾斜角度的控制，三级PID控制模块包括惯性轮转速环PID控制器、系统机体角度环PID控制器、系统机体角速度环PID控制器、电机和传感器。其中，传感器包括惯性测量单元和惯性轮测速编码器。惯性测量单元选用MPU6050传感器，由陀螺仪和加速度计组成。本发明惯性轮自平衡方法的具体步骤如下：
(Ⅰ)采用拉格朗日法建立惯性轮自平衡系统的数学模型，即如图2所示，将惯性轮自平衡系统中除惯性轮以外的部分等效为匀质的摆杆，摆杆竖直放置且底部与支撑面转动连接，并将惯性轮等效为转动连接在摆杆顶部的飞轮，电机驱动飞轮转动形成竖直平面内的反力矩。
摆杆的质量为M1，飞轮的质量为M2，摆杆倾斜角度的实际值是θr，飞轮转动角度的实际值是βr，坐标原点到摆杆的重心G之间的距离是l1，坐标原点到飞轮中心O的距离是l2；摆杆绕坐标原点转动，转动惯量是I1；飞轮绕自身的重心转动，转动惯量是I2。惯性轮自平衡系统的拉格朗日方程如下:
其中，t是时间；L是拉格朗日算子，L＝T-V，T是惯性轮自平衡系统的总动能，V为惯性轮自平衡系统的总势能；Q是惯性轮自平衡系统的广义力，Q＝[-I2βr u-I2βr]T，u为状态方程的输入；q是惯性轮自平衡系统的广义坐标，q＝[θ βr]；f是惯性轮自平衡系统的耗散力做功，且f＝[Г1 Г2]T。Г1是摆杆的耗散力做功，
k1是摆杆绕坐标原点转动的阻力系数；Г2是飞轮的耗散力做功，
k2是飞轮自转的阻力系数。
惯性轮自平衡系统的总动能T按照如下公式计算：
惯性轮自平衡系统的总势能V按照如下公式计算：
V＝(M1l1+M2l2)gcosθr。
综上得，拉格朗日算子L的计算公式如下：
由于摆杆在倾斜角度很小时便进行控制，可近似将sinθ取为θ。将拉格朗日算子L、惯性轮自平衡系统的广义力Q、广义坐标q、耗散力做功f带入拉格朗日方程中，可得如下方程组：
其中，
b＝(M1l1+M2l2)g。选取惯性轮自平衡系统的状态变量为
可以得到没有进行控制时的惯性轮自平衡系统的状态方程如下：
摆杆的倾斜角度θ与飞轮转动的角度β的输出方程如下：
通过没有进行控制时的惯性轮自平衡系统的状态方程和输出方程，由劳斯判据或者朱利判据可知，不进行控制时的惯性轮自平衡系统为不稳定系统，为使该系统稳定，需要通过反馈控制改变惯性轮自平衡系统的状态方程和输出方程。
(Ⅱ)通过传感器测量并计算得出摆杆倾斜角度的实际值θr、摆杆倾斜角速度的实际值
及飞轮转速的实际值
惯性轮测速编码器设置在电机上，对电机转速进行采样，由于电机驱动飞轮转动，飞轮转速与电机转速相等，因此惯性轮测速编码器的采样值即为飞轮转速。对惯性轮测速编码器采样的飞轮转速进行限幅滤波，即按照经验确定两次采样之间允许的最大偏差值，设最大偏差值为A；每次采样得到新的飞轮转速时，将新的飞轮转速与上次采样得到的飞轮转速相减，若差值的绝对值小于等于A，则将本次采样的飞轮转速作为本次测量得到的飞轮转速的实际值
若差值的绝对值大于A，则本次的采样值无效，用上次采样得到的飞轮转速作为本次测量得到的飞轮转速的实际值
惯性测量单元设置在摆杆的重心上，并使用惯性测量单元内的陀螺仪对摆杆倾斜角速度进行采样，用惯性测量单元内的加速度计对摆杆倾斜角加速度进行采样。对陀螺仪采样的摆杆倾斜角速度、加速度计采样的摆杆倾斜角加速度分别进行递推平均值滤波：选定队列的长度固定为N，将连续的N个采样值按照时间顺序排列成队列，每次采样到一个新数据后，将采样得到的新数据放入队尾，并扔掉队首的数据，组成新的队列；然后将队列中的N个数据进行算数平均运算，得到的结果即是滤波结果，作为陀螺仪和加速度计各自的测量值。递推平均值滤波对周期性干扰有良好的抑制作用，平滑度高，适用于陀螺仪和加速度计的滤波。陀螺仪经过递推平均值滤波后得到的摆杆倾斜角速度即作为摆杆倾斜角速度的实际值
对陀螺仪经过递推平均值滤波后得到的摆杆倾斜角速度进行积分，得到摆杆的倾斜角度。并对加速度计经过递推平均值滤波后得到的摆杆倾斜角加速度进行处理，也得到摆杆的倾斜角度。由摆杆倾斜角加速度得出摆杆的倾斜角的方法如下：由于摆杆的倾斜角加速度仅受重力加速度的影响，加速度计测得倾斜角加速度在加速度计的X轴和Y轴上的分量分别是Ax和Ay，则摆杆的倾斜角度为
加速度计对加速度比较敏感，取瞬时值计算倾角误差比较大，而陀螺仪积分得到的摆杆的倾斜角度不受小车加速度的影响。因此，在短时间测量摆杆的倾斜角度时，陀螺仪积分得到的摆杆的倾斜角度更准确。
虽然陀螺仪积分得到的摆杆的倾斜角度不受小车加速度的影响，随着时间的增加积分漂移和温度漂移带来的误差比较大，而通过加速度计得到的摆杆的倾斜角度可以直接计算出来，没有累积误差。因此，在长时间测量摆杆的倾斜角度时，通过加速度计得到的摆杆的倾斜角度更准确。
因此使用非矩阵卡尔曼互补融合滤波在陀螺仪积分得到的摆杆的倾斜角度，和通过加速度计得到的摆杆的倾斜角度中选出最优角度，即在短时间测量摆杆的倾斜角度时，选择陀螺仪积分得到的摆杆的倾斜角度为最优角度；在长时间测量摆杆的倾斜角度时，选择通过加速度计得到的摆杆的倾斜角度作为最优角度。具体做法如下：
使用低通滤波抑制通过加速度计得到的摆杆的倾斜角度中的高频信号，加速度计测倾斜角度时，动态响应较慢，在高频时信号不可用；使用高通滤波抑制陀螺仪积分得到的摆杆的倾斜角度中的低频信号，陀螺仪响应快，不过由于积分漂移和温度漂移等，在低频段信号不好。然后对通过加速度计得到的摆杆的倾斜角度、陀螺仪积分得到的摆杆的倾斜角度取加权平均值，得到更精确的摆杆的倾斜角度。在短时间测量摆杆的倾斜角度时，增大陀螺仪积分得到的摆杆的倾斜角度的权值，并且定时对加速度采样来的加速度值进行取平均值来校正陀螺仪的得到的角度。在长时间测量摆杆的倾斜角度时，增大通过加速度计得到的摆杆的倾斜角度的权值。改变陀螺仪积分得到的摆杆的倾斜角度的权值、通过加速度计得到的摆杆的倾斜角度的权值的时间可以按照经验设定。
将非矩阵卡尔曼互补融合滤波得到的最优角度作为摆杆倾斜角度的实际值θr。
(Ⅲ)如图1所示，以设定的飞轮转速的期望值
作为惯性轮转速环PID控制器的输入，以飞轮转速的实际值
作为惯性轮转速环PID控制器的反馈输入，形成负反馈，并通过PID控制，使惯性轮转速环PID控制器输出摆杆倾斜角度的补偿值θc。
惯性轮转速环PID控制器包括第一积分环节和第一比例环节；先由惯性轮转速环PID控制器算出飞轮转速的误差e1，飞轮转速的误差e1按照下式求出：
飞轮转速的误差e1作为第一积分环节的输入，使第一积分环节求出飞轮转速误差的积分值e1i,飞轮转速误差的积分值e1i由下式求出：
其中，KI1是设定的惯性轮转速环PID控制器的积分调节系数，t0是惯性轮自平衡系统开始工作时的时刻，t1是当前时刻，τ是代表时刻的变量，e1(τ)是在τ时刻求出的飞轮转速的误差。
第一积分环节算出飞轮转速误差的积分值e1i后，对飞轮转速误差的积分值e1i进行限幅滤波。当飞轮转速误差的积分值e1i的绝对值大于100时，将飞轮转速误差的积分值e1i的绝对值更新为100，使飞轮转速误差的积分值e1i保持在正负100之间，再将飞轮转速误差的积分值送至第一比例环节。
然后将飞轮转速误差的积分值e1i作为第一比例环节的输入，并由第一比例环节算出摆杆倾斜角度的补偿值θc。摆杆倾斜角度的补偿值θc由下式算出：
θc＝(KP1*e1+e1i)/100；
其中，KP1是设定的惯性轮转速环PID控制器的比例调节系数。
第一比例环节算出摆杆倾斜角度的补偿值θc后，对摆杆倾斜角度的补偿值θc进行限幅滤波。当摆杆倾斜角度的补偿值θc的绝对值大于5时，将摆杆倾斜角度的补偿值θc的绝对值设为5，使摆杆倾斜角度的补偿值θc保持在正负5之间，再将摆杆倾斜角度的补偿值θc送至系统机体角度环PID控制器。
(Ⅳ)以设定的摆杆倾斜角度的期望值θe作为系统机体角度环PID控制器的输入，以摆杆倾斜角度的实际值θr作为系统机体角度环PID控制器的反馈输入，以摆杆倾斜角度的补偿值θc作为系统机体角度环PID控制器的补偿输入，形成负反馈，并通过PID控制，使系统机体角度环PID控制器输出摆杆倾斜角速度的补偿值
系统机体角度环PID控制器包括第二积分环节和第二比例环节。先由系统机体角度环PID控制器算出摆杆倾斜角度的误差e2，摆杆倾斜角度的误差eθ按照下式求出：
e2＝θe+θc-θr。
摆杆倾斜角度的误差e2作为第二积分环节的输入，使第二积分环节算出摆杆倾斜角度误差的积分值e2i，摆杆倾斜角度误差的积分值e2i由下式求出：
其中，KI2是设定的系统机体角度环PID控制器的积分调节系数，t0是惯性轮自平衡系统开始工作时的时刻，t1是当前时刻，τ是代表时刻的变量，e2(τ)是在τ时刻求出的摆杆倾斜角度的误差。
第二积分环节算出摆杆倾斜角度误差的积分值e2i后，对摆杆倾斜角度误差的积分值e2i进行限幅滤波。当摆杆倾斜角度误差的积分值e2i的绝对值大于100时，将摆杆倾斜角度误差的积分值e2i的绝对值更新为100，使摆杆倾斜角度误差的积分值e2i保持在正负100之间，然后将摆杆倾斜角度误差的积分值e2i送至第二比例环节。
摆杆倾斜角度误差的积分值e2i作为第二比例环节的输入，并由第二比例环节算出摆杆倾斜角速度的补偿值
摆杆倾斜角速度的补偿值
由下式算出：
其中，KP2是设定的系统机体角度环PID控制器的比例调节系数。
(Ⅴ)以摆杆倾斜角速度的实际值
作为系统机体角速度环PID控制器的反馈输入，以摆杆倾斜角速度的补偿值
作为系统机体角速度环PID控制器的补偿输入，形成负反馈，并通过PID控制，使系统机体角速度环PID控制器输出飞轮转速的控制值
系统机体角速度环PID控制器包括第三积分环节和比例微分环节。先由系统机体角速度环PID控制器算出摆杆倾斜角速度的误差e3，摆杆倾斜角速度的误差e3按照下式求出：
摆杆倾斜角速度的误差e3作为第三积分环节的输入，使第三积分环节输出摆杆倾斜角速度误差的积分值e3i。摆杆倾斜角速度误差的积分值e3i由下式求出：
其中，KI3是设定的系统机体角速度环PID控制器的积分调节系数，t0是惯性轮自平衡系统开始工作时的时刻，t1是当前时刻，τ是代表时刻的变量，e3(τ)是在τ时刻求出的摆杆倾斜角速度的误差。
第三积分环节算出摆杆倾斜角速度误差的积分值e3i后，对摆杆倾斜角速度误差的积分值e3i进行限幅滤波；当摆杆倾斜角速度误差的积分值e3i的绝对值大于100时，将摆杆倾斜角速度误差的积分值e3i的绝对值更新为100，使摆杆倾斜角速度误差的积分值e3i保持在正负100之间，然后将摆杆倾斜角速度误差的积分值e3i送至比例微分环节。
摆杆倾斜角速度误差的积分值e3i作为比例微分环节的输入，并由比例微分环节算出飞轮转速的控制值
飞轮转速的控制值
由下式算出：
其中，KP3是设定的系统机体角速度环PID控制器的比例调节系数,KD3是设定的系统机体角速度环PID控制器的微分调节系数，e3(t2)是上一次测量时得到的摆杆倾斜角速度的误差。
(Ⅵ)通过脉冲调制的方式来控制电机的转速，即通过将飞轮转速的控制值
设置为脉宽调制信号的脉宽，并将脉宽调制信号送至电机，改变电机的占空比，来控制电机的转速。并且通过电机驱动飞轮转动来控制飞轮的转速，产生适合惯性轮自平衡系统稳定所需的转矩，使惯性轮自平衡系统保持平衡。
本发明惯性轮自平衡系统对第一积分环节、第二积分环节、第三积分环节输出的积分值进行限幅滤波，对积分漂移进行了限制；同时对第一比例环节输出的摆杆倾斜角度的补偿值进行限幅滤波，将摆杆倾斜角度的补偿值θc限制在正负5之间，提高了惯性轮自平衡系统的抗干扰能力。
对本实施例惯性轮自平衡系统进行仿真，惯性轮自平衡系统静态控制效果如图3所示，惯性轮自平衡系统动态控制效果如图4所示。由图3及图4可以看出，本实施例惯性轮自平衡系统可以有效地使摆杆的角度、速度和角速度保持稳定。
针对惯性轮自平衡机器人的高阶、多变量、非自稳特点，本发明惯性轮自平衡系统设计了三串级控制系统，在现有的系统机体角度环PID控制器、系统机体角速度环PID控制器之外还加入了惯性轮转速环PID控制器，由此提高了系统抗扰能力，加快了系统的自稳调节速度，实现系统的稳、准、快地控制目标。
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。
Claims (3)
1.一种惯性轮自平衡方法，其特征在于，采用三串级PID控制模块实现对摆杆倾斜角度的控制，所述三串级PID控制模块包括惯性轮转速环PID控制器、系统机体角度环PID控制器、系统机体角速度环PID控制器、电机和传感器；包括以下步骤:
(Ⅰ)采用拉格朗日法建立惯性轮自平衡系统的物理模型，即，将惯性轮自平衡系统中除惯性轮以外的部分等效为匀质的摆杆，摆杆竖直放置且底部与支撑面转动连接，并将惯性轮等效为转动连接在摆杆顶部的飞轮，电机驱动飞轮转动形成竖直平面内的反力矩；
(Ⅱ)通过传感器测量并计算得出摆杆倾斜角度的实际值θr、摆杆倾斜角速度的实际值
及飞轮转速的实际值
(Ⅲ)以设定的飞轮转速的期望值
作为惯性轮转速环PID控制器的输入，以飞轮转速的实际值
作为惯性轮转速环PID控制器的反馈输入，形成负反馈，并通过PID控制，使惯性轮转速环PID控制器输出摆杆倾斜角度的补偿值θc；
(Ⅳ)以设定的摆杆倾斜角度的期望值θe作为系统机体角度环PID控制器的输入，以摆杆倾斜角度的实际值θr作为系统机体角度环PID控制器的反馈输入，以摆杆倾斜角度的补偿值θc作为系统机体角度环PID控制器的补偿输入，形成负反馈，并通过PID控制，使系统机体角度环PID控制器输出摆杆倾斜角速度的补偿值
(V)以摆杆倾斜角速度的实际值
作为系统机体角速度环PID控制器的反馈输入，以摆杆倾斜角速度的补偿值
作为系统机体角速度环PID控制器的补偿输入，形成负反馈，并通过PID控制，使系统机体角速度环PID控制器输出飞轮转速的控制值
(Ⅵ)以飞轮转速的控制值
作为控制量，控制电机的转速，并通过电机驱动飞轮转动来控制飞轮的转速，产生适合惯性轮自平衡系统稳定所需的转矩，使惯性轮自平衡系统保持平衡；
惯性轮转速环PID控制器包括第一积分环节和第一比例环节；步骤(III)中，惯性轮转速环PID控制器算出飞轮转速的误差e1，飞轮转速的误差e1按照下式求出：
飞轮转速的误差e1作为第一积分环节的输入，使第一积分环节输出飞轮转速误差的积分值e1i，飞轮转速误差的积分值e1i由下式求出：
其中，KI1是设定的惯性轮转速环PID控制器的积分调节系数，t0是惯性轮自平衡系统开始工作时的时刻，t1是当前时刻，τ是代表时刻的变量，e1(τ)是在τ时刻求出的飞轮转速的误差；
飞轮转速误差的积分值e1i作为第一比例环节的输入，并由第一比例环节算出摆杆倾斜角度的补偿值θc；摆杆倾斜角度的补偿值θc由下式算出：
θc＝(KP1*e1+e1i)/100；
其中，KP1是设定的惯性轮转速环PID控制器的比例调节系数；
第一积分环节算出飞轮转速误差的积分值e1i后，对飞轮转速误差的积分值e1i进行限幅滤波；当飞轮转速误差的积分值e1i的绝对值大于100时，将飞轮转速误差的积分值e1i的绝对值更新为100，使飞轮转速误差的积分值e1i保持在正负100之间；
第一比例环节算出摆杆倾斜角度的补偿值θc后，对摆杆倾斜角度的补偿值θc进行限幅滤波；当摆杆倾斜角度的补偿值θc的绝对值大于5时，将摆杆倾斜角度的补偿值θc的绝对值设为5，使摆杆倾斜角度的补偿值θc保持在正负5之间；
第二积分环节算出摆杆倾斜角度误差的积分值e2i后，对摆杆倾斜角度误差的积分值e2i进行限幅滤波；当摆杆倾斜角度误差的积分值e2i的绝对值大于100时，将摆杆倾斜角度误差的积分值e2i的绝对值更新为100，使摆杆倾斜角度误差的积分值e2i保持在正负100之间；
系统机体角速度环PID控制器包括第三积分环节和比例微分环节；步骤(V)中，系统机体角速度环PID控制器算出摆杆倾斜角速度的误差e3，摆杆倾斜角速度的误差e3按照下式求出：
摆杆倾斜角速度的误差e3作为第三积分环节的输入，使第三积分环节输出摆杆倾斜角速度误差的积分值e3i，摆杆倾斜角速度误差的积分值e3i由下式求出：
其中，KI3是设定的系统机体角速度环PID控制器的积分调节系数，t0是惯性轮自平衡系统开始工作时的时刻，t1是当前时刻，τ是代表时刻的变量，e3(τ)是在τ时刻求出的摆杆倾斜角速度的误差；
摆杆倾斜角速度误差的积分值e3i作为比例微分环节的输入，并由比例微分环节算出飞轮转速的控制值
飞轮转速的控制值
由下式算出：
其中，KP3是设定的系统机体角速度环PID控制器的比例调节系数，KD3是设定的系统机体角速度环PID控制器的微分调节系数，e3(t2)是上一次测量时得到的摆杆倾斜角速度的误差；
第三积分环节算出摆杆倾斜角速度误差的积分值e3i后，对摆杆倾斜角速度误差的积分值e3i进行限幅滤波；当摆杆倾斜角速度误差的积分值e3i的绝对值大于100时，将摆杆倾斜角速度误差的积分值e3i的绝对值更新为100，使摆杆倾斜角速度误差的积分值e3i保持在正负100之间。
2.根据权利要求1所述的一种惯性轮自平衡方法，其特征在于，系统机体角度环PID控制器包括第二积分环节和第二比例环节；步骤(IV)中，系统机体角度环PID控制器算出摆杆倾斜角度的误差e2，摆杆倾斜角度的误差e2按照下式求出：
e2＝θe+θc-θr；
摆杆倾斜角度的误差e2作为第二积分环节的输入，使第二积分环节输出摆杆倾斜角度误差的积分值e2i，摆杆倾斜角度误差的积分值e2i由下式求出：
其中，KI2是设定的系统机体角度环PID控制器的积分调节系数，t0是惯性轮自平衡系统开始工作时的时刻，t1是当前时刻，τ是代表时刻的变量，e2(τ)是在τ时刻求出的摆杆倾斜角度的误差；
摆杆倾斜角度误差的积分值e2i作为第二比例环节的输入，并由第二比例环节算出摆杆倾斜角速度的补偿值
摆杆倾斜角速度的补偿值
由下式算出：
其中，KP2是设定的系统机体角度环PID控制器的比例调节系数。
3.根据权利要求1所述的一种惯性轮自平衡方法，其特征在于，步骤(Ⅵ)中，通过脉冲调制的方式来控制电机的转速，即通过将飞轮转速的控制值
设置为脉宽调制信号的脉宽，并将脉宽调制信号送至电机，改变电机的占空比，来控制电机的转速。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811590519.4A CN109709796B (zh) | 2018-12-20 | 2018-12-20 | 一种惯性轮自平衡方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811590519.4A CN109709796B (zh) | 2018-12-20 | 2018-12-20 | 一种惯性轮自平衡方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109709796A CN109709796A (zh) | 2019-05-03 |
| CN109709796B true CN109709796B (zh) | 2021-11-12 |
Family
ID=66257537
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811590519.4A Active CN109709796B (zh) | 2018-12-20 | 2018-12-20 | 一种惯性轮自平衡方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109709796B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111273542B (zh) * | 2020-01-20 | 2023-03-10 | 武汉科技大学 | 立方体机器人及控制系统和方法以及设计方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20150048589A (ko) * | 2013-10-28 | 2015-05-07 | 가천대학교 산학협력단 | 센서 융합을 이용한 자세 안정화 전동 외발 스쿠터 |
| CN105404296A (zh) * | 2015-10-28 | 2016-03-16 | 南京师范大学 | 一种两轮自平衡智能车姿态控制方法 |
| CN105730586A (zh) * | 2016-03-11 | 2016-07-06 | 厦门大学 | 一种双控制方式的两轮自平衡车控制系统 |
| CN108594637A (zh) * | 2018-05-30 | 2018-09-28 | 大连理工大学 | 两轮自平衡车平面点对点自主运动控制方法 |
| CN108897213A (zh) * | 2018-05-28 | 2018-11-27 | 宝鸡文理学院 | 一种基于面内调控的褶皱薄膜主动振动控制方法 |
-
2018
- 2018-12-20 CN CN201811590519.4A patent/CN109709796B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20150048589A (ko) * | 2013-10-28 | 2015-05-07 | 가천대학교 산학협력단 | 센서 융합을 이용한 자세 안정화 전동 외발 스쿠터 |
| CN105404296A (zh) * | 2015-10-28 | 2016-03-16 | 南京师范大学 | 一种两轮自平衡智能车姿态控制方法 |
| CN105730586A (zh) * | 2016-03-11 | 2016-07-06 | 厦门大学 | 一种双控制方式的两轮自平衡车控制系统 |
| CN108897213A (zh) * | 2018-05-28 | 2018-11-27 | 宝鸡文理学院 | 一种基于面内调控的褶皱薄膜主动振动控制方法 |
| CN108594637A (zh) * | 2018-05-30 | 2018-09-28 | 大连理工大学 | 两轮自平衡车平面点对点自主运动控制方法 |
Non-Patent Citations (2)
| Title |
|---|
| Design of Three-ring Nesting Cascade PID Controller of Full-auto Two-wheel Balance Vehicle;Yi Kang etc.;《2018 IEEE International Conference on Mechatronics and Automation (ICMA)》;20181008;第364-369页 * |
| 单球驱动机器人Ballbot的建模与控制研究;周爱国等;《机电一体化》;20121120;第18卷(第11期);第13-20页和第83页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109709796A (zh) | 2019-05-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109240305B (zh) | 基于互补滤波的两轮机器人运动控制系统及方法 | |
| CN105116729B (zh) | 一种两轮自平衡机器人自适应滑模变结构控制方法 | |
| Juang et al. | Design and control of a two-wheel self-balancing robot using the arduino microcontroller board | |
| Ha et al. | Trajectory tracking control for navigation of self-contained mobile inverse pendulum | |
| JP2024512235A (ja) | 脚車輪型ロボットの制御方法、装置、脚車輪型ロボット及び機器 | |
| CN101872198B (zh) | 车载摄像稳定平台 | |
| WO2021098238A1 (zh) | 一种车载摄像头云台伺服系统及控制方法 | |
| CN202267481U (zh) | 一种小型水平稳定平台 | |
| CN110637801B (zh) | 大型喷杆滚转运动控制用的电液主动悬架系统与控制方法 | |
| CN110109354B (zh) | 一种反作用轮平衡自行车机器人自适应滑模控制方法 | |
| CN109709796B (zh) | 一种惯性轮自平衡方法 | |
| CN107509443A (zh) | 一种智能割草机的坡地行驶控制方法及系统 | |
| CN109947099B (zh) | 一种基于事件触发机制的机器人控制方法及装置 | |
| CN208384428U (zh) | 基于反向位移补偿的稳定转台系统 | |
| CN110162068A (zh) | 一种自平衡机器人的控制方法 | |
| Engin | Embedded LQR controller design for self-balancing robot | |
| CN110109353B (zh) | 一种反作用轮平衡自行车机器人模糊自适应滑模控制系统 | |
| CN111273542A (zh) | 立方体机器人及控制系统和方法以及设计方法 | |
| Majczak et al. | Comparison of two efficient control strategies for two-wheeled balancing robot | |
| Susnea et al. | Path following, real-time, embedded fuzzy control of a mobile platform wheeled mobile robot | |
| Aphiratsakun et al. | Implementation of AU balancing ballbot (AUB 3 | |
| Gao et al. | Two types of coaxial self-balancing robots | |
| Vries | Redesign & implementation of a moment exchange unicycle robot | |
| CN110308727A (zh) | 一种消除双足机器人上身姿态晃动的控制方法 | |
| Ning et al. | Trajectory tracking control of a wheeled inverted pendulum vehicle: from theory to experiment |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |