一种惯性轮自平衡方法与流程

本发明涉及自平衡技术领域，具体为一种惯性轮自平衡方法。

背景技术：

移动机器人系统集环境感知、动态决策与规划、行为控制与执行等功能于一体，目前已广泛应于工业、农业、医疗、航天、娱乐等各种领域。根据机器人移动方式的不同，可分为：轮式机器人、腿式机器人、履带机器人、爬行机器人、蠕动机器人和游泳式机器人等类型。与多轮机器人相比，惯性轮自平衡系统是类似单轮自行车的轮式机器人系统，它具有成本低、车体结构简单、能量利用率高、占地面积较小，能最大限度地减少系统与地面之间的接触点，绕本体中心旋转和以任意半径转向，具有广阔的发展前景。惯性轮自平衡机器人通过惯性轮的旋转产生反力矩实现其自身的平衡控制，它是一类动态稳定的系统，其动力学模型具有多变量、高阶、强耦合、时变等非线性特点。因此，针对惯性轮自平衡机器人，研究并实现其平衡控制的方法和技术，是实现各种环境下惯性轮自平衡机器人的控制任务要求的首要要求，是其实际应用的有效保障，如轨迹跟踪、路径规划、自主行走避障等，具有重要的理论价值和现实意义。

技术实现要素：

本发明的目的在于提供一种惯性轮自平衡方法，以解决上述背景技术中提出的问题。

为实现上述目的，本发明提供如下技术方案：一种惯性轮自平衡方法，其特征在于，采用三串级pid(proportionintegraldifferential、比例积分微分)控制模块实现对摆杆倾斜角度的控制，所述三串级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βru-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控制器，由此提高了系统抗扰能力，加快了系统的自稳调节速度，实现系统的稳、准、快地控制目标。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。