控制设备平衡的方法和装置转让专利
申请号 : CN202010604061.4
文献号 : CN111669096B
文献日 : 2021-11-02
发明人 : 张洋 , 张虎
申请人 : 张洋 , 张虎
摘要 :
权利要求 :
1.一种控制设备平衡的方法,其特征在于,所述方法包括:获取姿态传感器测量的设备的加速度、角速度;
利用所述加速度、角速度,进行姿态解算,计算所述设备的倾斜角;
获取飞轮的当前转速;
对所述角速度进行微分D控制,对所述倾斜角进行比例P控制,对所述飞轮的当前转速进行比例积分PI控制,用上述角速度的微分D控制、倾斜角的比例P控制、飞轮转速的比例积分PI控制的和,调制脉冲宽度调制PWM信号,具体的:对所述设备的角速度进行微分,然后乘以微分增益,得到角速度的微分D控制;设置所述设备的角度设定值,用所述倾斜角减去所述角度设定值,得到角度偏差,第一比例增益乘以所述角度偏差,得到倾斜角的比例P控制;
设置所述飞轮的转速设定值,用所述飞轮的当前转速减去所述转速设定值,得到转速偏差,第二比例增益乘以所述转速偏差,得到飞轮转速的比例P控制,对所述转速偏差进行积分,然后乘以积分增益,得到飞轮转速的积分I控制;计算所述角速度的微分D控制、倾斜角的比例P控制、飞轮转速的比例P控制、积分I控制的和,用于调制脉冲宽度调制PWM信号;
采用所述PWM信号驱动电机带动飞轮转动,产生力矩,维持所述设备的平衡。
2.根据权利要求1所述的方法,其特征在于,所述方法还包括:限制所述飞轮转速的积分I控制的幅度;
限制所述角速度的微分D控制、倾斜角的比例P控制、飞轮转速的比例P控制、限幅后积分I控制的和的幅度。
3.根据权利要求1‑2任一所述的方法,其特征在于,所述飞轮的转速设定值为零。
4.根据权利要求1‑2任一所述的方法,其特征在于,所述方法还包括:与所述设备的其它模块和/或上位机组成蓝牙mesh网络,进行蓝牙通信。
5.根据权利要求4所述的方法,其特征在于,所述方法还包括:通过所述蓝牙mesh网络,接收并保存所述上位机发送的调整后的PID参数。
6.根据权利要求1‑2任一所述的方法,其特征在于,所述方法还包括:计算当所述设备的力矩等于飞轮的力矩时,所述飞轮的角加速度;
根据所述飞轮的角加速度、所述设备的平衡情况,调整PID参数。
7.根据权利要求6所述的方法,其特征在于,所述方法还包括:通过如下公式计算所述设备的力矩和飞轮的力矩:T1=M×g×cosθ×L
其中,T1为所述设备的力矩,M为所述设备的质量,g为重力加速度,L为所述设备的重心到设备与地面接触点的距离,θ为所述设备的重心与设备与地面接触点的连线与地面的夹角;
其中,T2为所述飞轮的力矩,m为所述飞轮的质量,R为所述飞轮的外半径,r为所述飞轮的内半径,α为所述飞轮的角加速度;
当所述设备的力矩等于飞轮的力矩时,所述飞轮的角加速度α为:
8.一种控制设备平衡的装置,其特征在于,所述装置包括:获取姿态参数单元,配置用于获取姿态传感器测量的设备的加速度、角速度;
姿态解算单元,配置用于利用所述加速度、角速度,进行姿态解算,计算所述设备的倾斜角;
获取飞轮参数单元,配置用于获取飞轮的当前转速;
PID控制单元,配置用于对所述角速度进行微分D控制,对所述倾斜角进行比例P控制,对所述飞轮的当前转速进行比例积分PI控制,用上述角速度的微分D控制、倾斜角的比例P控制、飞轮转速的比例积分PI控制的和,调制脉冲宽度调制PWM信号,具体的:对所述设备的角速度进行微分,然后乘以微分增益,得到角速度的微分D控制;设置所述设备的角度设定值,用所述倾斜角减去所述角度设定值,得到角度偏差,第一比例增益乘以所述角度偏差,得到倾斜角的比例P控制;设置所述飞轮的转速设定值,用所述飞轮的当前转速减去所述转速设定值,得到转速偏差,第二比例增益乘以所述转速偏差,得到飞轮转速的比例P控制,对所述转速偏差进行积分,然后乘以积分增益,得到飞轮转速的积分I控制;计算所述角速度的微分D控制、倾斜角的比例P控制、飞轮转速的比例P控制、积分I控制的和,用于调制脉冲宽度调制PWM信号;
驱动单元,配置用于采用所述PWM信号驱动电机带动飞轮转动,产生力矩,维持所述设备的平衡。
9.根据权利要求8所述的装置,其特征在于,所述PID控制单元包括:角速度控制子单元,配置用于对所述设备的角速度进行微分,然后乘以微分增益,得到角速度的微分D控制;
角度控制子单元,设置所述设备的角度设定值,用所述倾斜角减去所述角度设定值,得到角度偏差,第一比例增益乘以所述角度偏差,得到倾斜角的比例P控制;
飞轮转速控制子单元,配置用于设置所述飞轮的转速设定值,用所述飞轮的当前转速减去所述转速设定值,得到转速偏差,第二比例增益乘以所述转速偏差,得到飞轮转速的比例P控制,对所述转速偏差进行积分,然后乘以积分增益,得到飞轮转速的积分I控制;
调制子单元,配置用于计算所述角速度的微分D控制、倾斜角的比例P控制、飞轮转速的比例P控制、积分I控制的和,用于调制脉冲宽度调制PWM信号。
10.根据权利要求8所述的装置,其特征在于,所述装置还包括:蓝牙通信单元,配置用于与所述设备的其它模块和/或上位机组成蓝牙mesh网络,进行蓝牙通信。
11.根据权利要求8‑10任一所述的装置,其特征在于,所述装置还包括:角加速度计算单元,配置用于计算当所述设备的力矩等于飞轮的力矩时,所述飞轮的角加速度;
调整PID参数单元,配置用于根据所述飞轮的角加速度、所述设备的平衡情况,调整PID参数。
说明书 :
控制设备平衡的方法和装置
技术领域
背景技术
发明内容
倾斜角;获取飞轮的当前转速;对所述角速度进行微分D控制,对所述倾斜角进行比例P控
制,对所述飞轮的当前转速进行比例积分PI控制,用上述角速度的微分D控制、倾斜角的比
例P控制、飞轮转速的比例积分PI控制的和,调制脉冲宽度调制PWM信号;采用所述PWM信号
驱动电机带动飞轮转动,产生力矩,维持所述设备的平衡。
例P控制、飞轮转速的比例积分PI控制的和,调制脉冲宽度调制PWM信号,包括:对所述设备
的角速度进行微分,然后乘以微分增益,得到角速度的微分D控制;设置所述设备的角度设
定值,用所述倾斜角减去所述角度设定值,得到角度偏差,第一比例增益乘以所述角度偏
差,得到倾斜角的比例P控制;设置所述飞轮的转速设定值,用所述飞轮的当前转速减去所
述转速设定值,得到转速偏差,第二比例增益乘以所述转速偏差,得到飞轮转速的比例P控
制,对所述转速偏差进行积分,然后乘以积分增益,得到飞轮转速的积分I控制;计算所述角
速度的微分D控制、倾斜角的比例P控制、飞轮转速的比例P控制、积分I控制的和,用于调制
脉冲宽度调制PWM信号。
和的幅度。
的夹角;
用所述加速度、角速度,进行姿态解算,计算所述设备的倾斜角;获取飞轮参数单元,配置用
于获取飞轮的当前转速;PID控制单元,配置用于对所述角速度进行微分D控制,对所述倾斜
角进行比例P控制,对所述飞轮的当前转速进行比例积分PI控制,用上述角速度的微分D控
制、倾斜角的比例P控制、飞轮转速的比例积分PI控制的和,调制脉冲宽度调制PWM信号;驱
动单元,配置用于采用所述PWM信号驱动电机带动飞轮转动,产生力矩,维持所述设备的平
衡。
所述设备的角度设定值,用所述倾斜角减去所述角度设定值,得到角度偏差,第一比例增益
乘以所述角度偏差,得到倾斜角的比例P控制;飞轮转速控制子单元,配置用于设置所述飞
轮的转速设定值,用所述飞轮的当前转速减去所述转速设定值,得到转速偏差,第二比例增
益乘以所述转速偏差,得到飞轮转速的比例P控制,对所述转速偏差进行积分,然后乘以积
分增益,得到飞轮转速的积分I控制;调制子单元,配置用于计算所述角速度的微分D控制、
倾斜角的比例P控制、飞轮转速的比例P控制、积分I控制的和,用于调制脉冲宽度调制PWM信
号。
速度计算单元,配置用于计算当所述设备的力矩等于飞轮的力矩时,所述飞轮的角加速度;
调整PID参数单元,配置用于根据所述飞轮的角加速度、所述设备的平衡情况,调整PID参
数。
况。对设备的角速度进行微分D控制,在产生倾斜角之前,一旦发现有产生倾斜角的趋势就
开始调节,是提前控制,更及时。当角速度有变大或变小趋势时,输出控制,提前防治倾斜角
变大或超调。通过上述倾斜角比例P控制、角速度微分D控制,便能让设备在平衡点附近震
荡,但飞轮会越转越快,所以对飞轮的当前转速进行比例积分PI控制,快速降低飞轮的转
速,同时消除飞轮惯性旋转累积的转速。即在维持设备平衡的同时,提前防治设备倾斜,不
断减小倾斜的角度,回归自然平衡状态,同时控制降低飞轮的转速。
附图说明
具体实施方式
便于描述,附图中仅示出了与有关发明相关的部分。
个结构示意图。如图所示,飞轮电机包括微控制单元MCU101、电机驱动电路102、电机103、飞
轮104、转速采集电路105、姿态传感器106。其中,转速采集电路105作为反馈单元,采集飞轮
104的当前转速,发送给微控制单元MCU101。姿态传感器106用于采集设备的加速度、角速
度,反馈给微控制单元MCU101。本申请的控制设备平衡的方法由微控制单元MCU101执行。微
控制单元MCU101根据姿态传感器、转速采集电路反馈的数据,控制输出PWM信号至电机驱动
电路102,通过电机驱动电路102驱动电机103转动,电机103转动带动飞轮104转动,飞轮104
转动,产生力矩,维持静止或行进中设备的平衡。本申请的控制设备平衡的方法用于控制纵
向两轮、横向两轮、独轮设备的平衡。
的安装轴线为沿车身行驶方向。例如,飞轮安装在电动摩托车座椅的下面,飞轮转动的轴也
是纵向前后方向。
用于分别测量设备的三轴角速度、三轴加速度。其中,安装时,调整姿态传感器的方向,使姿
态传感器的一个轴为设备的纵方向,一个轴为设备的左右横方向。使设备左右倾斜角为设
备的俯仰角或横滚角。
中,转速采集电路主要由编码器组成。
控制单元。
分PI控制的和,调制脉冲宽度调制PWM信号。
两轮设备自然平衡的状况下,左右倾斜的角度。用上一步计算的设备当前实际左右倾斜的
倾斜角减去角度设定值,作为角度偏差,将该角度偏差乘以第一比例增益,得到倾斜角的比
例P控制。通过倾斜角的比例P控制,可快速调整改善设备的倾斜状况,在控制设备平衡的情
况下,控制设备不断从倾斜状态回归到自然平衡状态。
势,在产生较大倾斜角度之前,通过角速度微分控制,提前防治设备的倾斜,及时性更好,如
果角速度变小,则角加速度为负值,微分D控制会输出一个负值,产生反向作用,调节平衡。
另外,设置合理的微分增益、微分时间。
控且消耗功率,所以本申请对飞轮的转速进行比例积分控制。参考图3,设置转速设定值的
值,在本实施例中,将该值设置为零,在其它实现方式中,设置转速设定值为每秒2转或每秒
3转等。将采集的飞轮的当前转速减去转速设定值,得到转速偏差,该转速偏差乘以第二比
例增益,得到飞轮转速的比例控制。计算转速偏差的积分,用于积分控制,通常设置积分时
间,计算该积分时间内所有转速偏差的和,然后将该和乘以积分增益,得到飞轮转速的积分
I控制。为防止积分超调和震荡,使整个PID控制更稳定、准确,在本实施例中,限制积分I控
制的幅度,即小于等于某一预设幅度阈值。
号的占空比。
指设备的重心沿设备在地面上的投影。例如,如果设备是前后对称,设备垂直地面放置,则
该接触点为重心在地面上的投影,后面如果设备原地左右倾斜,接触点的位置是不发生变
化的,OD的距离为L。如图所示,θ为所述设备的重心与设备与地面接触点的连线与地面的夹
角。OA表示设备的重力,AB表示重力的一个分力,该分力被设备与地面的摩擦力抵消。OB表
示重力在垂直于设备方向上的分力,因为该分力的存在导致设备不断左倾斜或右倾斜。该
分力产生的力矩T1为:
例增益、微分增益、微分时间、积分增益、积分时间。
备的倾斜角度进行比例P控制,快速调整改善设备的倾斜状况,对飞轮的当前转速进行比例
积分PI控制,快速降低飞轮的转速,同时消除飞轮惯性旋转累积的转速。在维持设备平衡的
情况下,控制设备的倾斜角回归角度设定值,回到自然平衡状态;控制飞轮的转速降低到转
速设定值,回到飞轮停止转动静止的状态。
中,上位机包括但不限于:智能手机、平板电脑、遥控手柄。
控制摩托车前进或后退的速度。通过转向舵机模块驱动前轮转向,控制摩托车的行驶方向。
飞轮电机、动力电机模块、转向舵机模块及上位机都具有蓝牙mesh芯片,作为一个节点,支
持组成一个蓝牙mesh网络。蓝牙mesh网络中的各个节点可相互通信。支持用户通过上位机
向飞轮电机、动力电机模块、转向舵机模块发送控制指令,此外,接收飞轮电机、动力电机模
块、转向舵机模块发送的参数信息。例如,智能手机通过蓝牙mesh网络向动力电机模块发送
运动指令,其中,该指令包含电机的转动方向、转速和舵机转动角度。动力电机模块接收上
述运动指令,解析出与自身对应的指令,根据该指令,控制电机转动,使遥控摩托车前进或
者后退。然后,动力电机模块将上述收到的指令通过mesh adv的方式完整地转发给转向舵
机模块,转向舵机模块从指令中解析与自身对应的指令,完成转动角度的动作,使遥控摩托
车左转或者右转。上述过程中,遥控摩托车按照指令执行前进后退、左转右转的运动,并在
此过程中通过飞轮电机维持遥控摩托车的平衡。
值,得到角度偏差,然后乘以比例增益,得到倾斜角的比例P控制。其中,设置角度设定值为
零。对设备前后轴向上的角速度做微分D控制,对飞轮转速做比例积分PI控制。即用PID算法
控制设备的平衡。
元504、驱动单元505。其中,获取姿态参数单元501配置用于获取姿态传感器测量的设备的
加速度、角速度;姿态解算单元502配置用于利用上述加速度、角速度,进行姿态解算,计算
设备的倾斜角;获取飞轮参数单元503配置用于获取飞轮的当前转速;PID控制单元504配置
用于对角速度进行微分D控制,对倾斜角进行比例P控制,对飞轮的当前转速进行比例积分
PI控制,用上述角速度的微分D控制、倾斜角的比例P控制、飞轮转速的比例积分PI控制的
和,调制脉冲宽度调制PWM信号的占空比;驱动单元505配置用于采用上述PWM信号驱动电机
转动,电机带动飞轮转动,产生力矩,维持所述设备的平衡。
算,然后乘以微分增益,得到角速度的微分D控制;角度控制子单元,设置设备的角度设定
值,用姿态解算单元中计算倾斜角减去上述角度设定值,得到角度偏差,第一比例增益乘以
上述角度偏差,得到倾斜角的比例P控制;飞轮转速控制子单元,配置用于设置飞轮的转速
设定值,用飞轮的当前转速减去转速设定值,得到转速偏差,第二比例增益乘以上述转速偏
差,得到飞轮转速的比例P控制,对转速偏差进行积分,然后乘以积分增益,得到飞轮转速的
积分I控制;调制子单元,配置用于计算角速度的微分D控制、倾斜角的比例P控制、飞轮转速
的比例P控制、积分I控制的和,用于调制脉冲宽度调制PWM信号的占空比。
调整后的PID参数。
参数单元配置用于根据飞轮的角加速度、设备的平衡情况,调整PID参数。以适用不同的设
备,来保持设备的平衡。
方案,同时也应涵盖在不脱离所述发明构思的情况下,由上述技术特征或其等同特征进行
任意组合而形成的其它技术方案。例如上述特征与本申请中公开的(但不限于)具有类似功
能的技术特征进行互相替换而形成的技术方案。