[发明专利]一种自平衡机器人抗持续性外力作用的姿态控制方法

权利要求书

1.一种自平衡机器人抗持续性外力作用的姿态控制方法，其特征在于，所述自平衡机器人为两轮自平衡机器人且左右轮轴上分别安装有编码器，所述自平衡机器人抗持续性外力作用的姿态控制方法，包括：

步骤S1、定时读取左右轮轴分别对应的编码器的数值，将两编码器的数值相加作为状态参数，所述状态参数具有正负；

步骤S2、根据所述状态参数判断当前自平衡机器人的运行状态，包括：

若所述状态参数的绝对值小于第一临界值，则当前自平衡机器人的运行状态为正常工作状态；

若所述状态参数的绝对值大于第二临界值，则当前自平衡机器人的运行状态为失控偏移状态；

若所述状态参数的绝对值位于第一临界值和第二临界值之间，则当前自平衡机器人的运行状态为可调偏移状态；

步骤S3、根据所述运行状态，采用PID控制器调节自平衡机器人的姿态，包括：

若运行状态为正常工作状态，则不需要重心调节；

若运行状态为可调偏移状态，则通过速度环PI控制器抵消自平衡机器人受的持续性水平外力维持原地平衡；

若运行状态为失控偏移状态，则通过角度补偿的角度环PD控制器改变自平衡机器人的重心，抵消受到的持续性水平外力维持原地平衡；

其中，所述通过角度补偿的角度环PD控制器改变自平衡机器人的重心，抵消受到的持续性水平外力维持原地平衡，包括：

建立角度环PD控制器的输出公式为：

Balance_PWM＝Balance_Kp*Angle+Balance_Kd*Gyro

式中，Balance_PWM为角度环PD控制器最终输出的PWM波大小；Balance_Kp为PD环的比例参数；Angle是陀螺仪采集到的自平衡机器人当前倾角；Balance_Kd为PD环的微分参数；Gyro是陀螺仪采集到的自平衡机器人当前倾斜方向的角速度；

则角度补偿的角度环PD控制器的输出公式为：

Balance_PWM＝Balance_Kp*(Angle+Central_Point)+Balance_Kd*Gyro

式中，Central_Point为设置机器人重心点的值，当Central_Point的值取正数时，则调整机器人的重心点向后改变，当Central_Point的值取负数时，则调整机器人的重心点向前改变，并且Central_Point的绝对值越大，机器人的重心点离机器人的物理重心零点越远，即机器人的倾斜角度越大；

其中，设置自平衡机器人重心点的值Central_Point的调整公式如下：

Central_Point＝Mechanical_Zero+Angle_Bias

式中，Mechanical_Zero为机器人机体的物理重心零点，且取值为0，Angle_Bias为每隔预设间隔改变自平衡机器人的角度值，如果需要自平衡机器人重心点往后偏移，则Angle_Bias值取正数；如果需要自平衡机器人重心点往前偏移，则Angle_Bias值取负数；

因此，更新角度补偿的角度环PD控制器的输出公式为：

Balance_PWM＝Balance_Kp*(Angle+Mechanical_Zero+Angle_Bias)+Balance_Kd*Gyro

根据角度补偿的角度环PD控制器的输出Balance_PWM，改变自平衡机器人的重心，抵消受到的持续性水平外力维持原地平衡。

2.如权利要求1所述的自平衡机器人抗持续性外力作用的姿态控制方法，其特征在于，若所述自平衡机器人为向前运动，则状态参数为正数；若所述自平衡机器人为向后运动，则状态参数为负数。

3.如权利要求1所述的自平衡机器人抗持续性外力作用的姿态控制方法，其特征在于，所述每隔预设间隔改变自平衡机器人的角度值Angle_Bias的取值范围为[-0.05，-0.01]∪[0.01，0.05]。