CN108501768A - 一种基于z轴陀螺仪及轮速差的两轮速度控制方法 - Google Patents

Abstract

本发明是一种基于Z轴陀螺仪及轮速差的两轮速度控制方法，主要作用于移动机器人两轮速度控制系统中。针对移动机器人运行过程中单轮滑动或者地形阻力变化导致的两轮速度控制误差的问题，本发明方法利用Z轴陀螺仪信息构建两轮独立控制回路之间的PID反馈通道，对两轮速度控制误差进行补偿和校正。本发明能有效提升两轮移动机器人的速度控制精度和复杂环境下运行时的航向角稳定性，确保了两轮移动机器人两轮速度控制的高可靠性。

Description

一种基于Z轴陀螺仪及轮速差的两轮速度控制方法

技术领域

本发明属于数字控制和组合导航技术领域，特别涉及一种基于Z轴陀螺仪及轮速差的两轮速度控制方法。

背景技术

近年来，随着微电子技术的进步，尤其是集成化的微机电系统(MEMS)中的惯性器件精度大幅度提升，使得具有自主姿态控制的移动机器人获得了较快的发展和广泛的应用。移动机器人在运行的过程中，由于地面摩擦系数减小导致车轮出现滑动，速度测量模块感知的车轮速度虽然与控制系统设定的参考量一致，但滑动车轮相对于地面参考系的实际运行速度却与参考速度有很大的偏差，此偏差造成移动机器人航向角发生变化而控制系统却无法感知和修正。另外，车轮运行在不同的地形环境下或受到障碍物阻力作用，轮速急剧变化同样会导致与参考值出现偏差进而影响航向角角速度。

目前移动机器人两轮速度控制方法主要是基于PID的移动机器人控制算法。复杂的地形环境造成移动机器人颠簸、失重、抖动，甚至出现单轮腾空与地面短时间脱离的现象。这种情况下由于电机负载突然减小，电机将会加速转动引起车轮速度增加，PID控制环路虽然最终能将车轮运行速度调控到与参考速度一致，但存在一个调节过程，且微分控制参数获取不到准确值，此过程会产生较大的两轮速度控制误差。为了解决上述问题，需要对算法进行改进。本发明专利是针对上述问题的一种有效的移动机器人两轮速度控制方法。

发明内容

本发明主要是针对上述背景技术的不足，提供了一种基于Z轴陀螺仪及轮速差的两轮速度控制方法。本发明为实现上述发明目的采用如下技术方案。

一种基于Z轴陀螺仪及轮速差的两轮速度控制方法，包括如下步骤：

步骤1：根据两轮移动机器人结构和运动特性，建立移动机器人旋转运动模型，获得两轮移动机器人航向角的目标角速度；

步骤2：通过速度传感器实时采集两轮速度量，通过Z轴陀螺仪采集姿态输出信息，对其解算得到移动机器人当前时刻的两轮轮速及航向角角速度；

步骤3：将两轮移动机器人航向角角速度与两轮移动机器人航向角的目标角速度做差，获取航向角角速度误差，计算得到两轮速度控制的陀螺仪修正控制量；

步骤4：根据两轮轮速差与两轮目标轮速差的误差，并采用两轮速度PI控制策略，获得两轮差速控制量；

步骤5：根据陀螺仪修正控制量和两轮差速控制量，计算修正后的两轮差速控制量，对两轮速度控制量进行修正。

进一步地，步骤1中，移动机器人航向角角速度为ω_s，左、右两轮相对于地面参考系的实际运行速度为v_L和v_R，右轮当前圆周运动的半径为r，两轮之间的有效距离为L。

对其运动进行分析，可以得到：

其中，π表示圆周率。

两轮移动机器人右轮当前圆周运动半径r为：

因此，两轮移动机器人航向角的目标角速度ω_s为：

移动机器人两轮相对于地面参考系的速度差能直接体现在移动机器人航向角角速度上，且航向角角速度大小不受摩擦系数以及电机负载变化导致的车轮速度变化的影响。

进一步地，步骤2中，由于车轮与电机输出转轴相连，电机输出转轴与电机转子转轴通过减速齿轮箱相连，因此可以通过对电机转子转速的测量得到车轮转速从而换算得到车轮运行速度。选用磁编码器作为速度传感器，通过磁编码器实时采集左轮轮速v_Lm和右轮轮速v_Rm。

两轮移动机器人运行过程中地面摩擦系数改变导致单个车轮出现滑动或者单轮电机负载变化导致的电机转速变化，最终导致航向角角速度发生改变，被控制系统的Z轴陀螺仪所感知。通过Z轴陀螺仪采集角速度信息，对其解算得到移动机器人当前时刻的航向角角速度。

进一步地，步骤3中，Z轴陀螺仪检测得到移动机器人航向角角速度，并将结果与航向角的目标角速度进行做差，获取航向角角速度误差。

移动机器人航向角角速度为ω_m，航向角角速度误差为e_rot＝ω_m-ω_s，陀螺仪修正控制的比例控制系数为k_pr，积分控制系数为k_ir，微分控制系数为k_dr，由此可得两轮速度控制的陀螺仪修正控制量Δu_rot：

进一步地，步骤4中，控制系统两轮目标轮速差为v_s，旋转磁编码器测量到的两轮轮速差为v_m＝v_Lm-v_Rm，两轮轮速差与两轮目标轮速差的误差为e_s＝v_m-v_s，PI控制策略的比例控制系数为k_p，积分控制系数为k_i，通过PI控制策略获取两轮差速控制量Δu_s为：

Δu_s(t)＝k_pe_s(t)+k_i∫e_s(t)dt

进一步地，步骤5中，对于摩擦系数不够造成的速度误差，需通过减小未滑动车轮的运行速度，从而实现两轮对地速度的严格一致。对于某一车轮受到地形阻力导致对地速度变化，则需要对受阻车轮控制量进行调控。为了兼顾这两种情况，两轮速度控制量分为左、右两独立调控量，u_cl左轮速度控制量，u_cr为右轮速度控制量，修正后的两轮差速控制量Δu_c。

根据陀螺仪修正控制量和两轮差速控制量，计算修正后的两轮差速控制量：

根据修正后的两轮差速控制量，对两轮速度控制量进行补偿：

其中，u_c为两轮速度目标控制量。

加入Z轴陀螺仪信息后，控制系统利用Z轴陀螺仪检测移动机器人航向角角速度，并将测量的结果与给定的航向角参考角速度进行对比获取航向角角速度误差，该误差经过PID控制器生成两轮速度控制的陀螺仪修正控制量，根据陀螺仪修正控制量和两轮差速控制量，计算修正后的两轮差速控制量，对两轮速度控制量进行修正，提高两轮速度控制精度。

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍。

附图说明

图1基于Z轴陀螺仪及轮速差的两轮速度控制框图

图2两轮移动机器人旋转运动模型

图3无Z轴陀螺仪修正时两轮对地运行速度

图4无Z轴陀螺仪修正时两轮速度控制误差

图5 Z轴陀螺仪修正下两轮对地运行速度

图6 Z轴陀螺仪修正下两轮速度控制误差

具体实施方式

下面详细描述本发明的实施例，本实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。参照说明书附图对本发明的一种基于Z轴陀螺仪及轮速差的两轮速度控制方法作以下详细地说明。

如图1所示，为基于Z轴陀螺仪及轮速差的两轮速度控制框图。

在该两轮速度控制结构中，微控制器通过ADC实时采集Z轴陀螺仪的输出信号并对其进行解算，得到移动机器人当前时刻的航向角角速度。根据系统设定的角速度参考量，求得移动机器人当前航向角角速度与参考量的误差后，经过PID控制策略，计算得到两轮速度控制的陀螺仪修正控制量。根据陀螺仪修正控制量和两轮差速控制量，计算修正后的两轮差速控制量，对两轮速度控制量进行修正。

1)根据两轮移动机器人结构和运动特性，建立移动机器人旋转运动模型，获得两轮移动机器人航向角的目标角速度；

移动机器人航向角角速度为ω_s，左、右两轮相对于地面参考系的实际运行速度为v_L和v_R，右轮当前圆周运动的半径为r，两轮之间的有效距离为L。

对其运动进行分析，可以得到：

其中，π表示圆周率。

两轮移动机器人右轮当前圆周运动半径r为：

因此，两轮移动机器人航向角的目标角速度ω_s为：

移动机器人两轮相对于地面参考系的速度差能直接体现在移动机器人航向角角速度上，且航向角角速度大小不受摩擦系数以及电机负载变化导致的车轮速度变化的影响。

2)通过速度传感器实时采集两轮速度量，通过Z轴陀螺仪采集姿态输出信息，对其解算得到移动机器人当前时刻的两轮轮速及航向角角速度；

由于车轮与电机输出转轴相连，电机输出转轴与电机转子转轴通过减速齿轮箱相连，因此可以通过对电机转子转速的测量得到车轮转速从而换算得到车轮运行速度。选用磁编码器作为速度传感器，通过磁编码器实时采集左轮轮速v_Lm和右轮轮速v_Rm。通过需求分析和性能对比，选择AMS公司的AS5040非接触式旋转磁编码器芯片作为速度传感器。

两轮移动机器人运行过程中地面摩擦系数改变导致单个车轮出现滑动或者单轮电机负载变化导致的电机转速变化，最终导致航向角角速度发生改变，被控制系统的Z轴陀螺仪所感知。通过Z轴陀螺仪采集角速度信息，对其解算得到移动机器人当前时刻的航向角角速度。选用高性能、超小封装的LY530AL单Z轴MEMS陀螺仪作为两轮移动机器人航向角角速度检测器件。该传感器的最大量程为±300°/s，具有3.3mV/°/s的灵敏度，能较好地满足移动机器人航向角角速度的测量并提升角速度测量的灵敏度。

3)将两轮移动机器人航向角角速度与两轮移动机器人航向角的目标角速度做差，获取航向角角速度误差，计算得到两轮速度控制的陀螺仪修正控制量；

Z轴陀螺仪检测得到移动机器人航向角角速度，并将结果与航向角的目标角速度进行做差，获取航向角角速度误差。

移动机器人航向角角速度为ω_m，航向角角速度误差为e_rot＝ω_m-ω_s，陀螺仪修正控制的比例控制系数为k_pr，积分控制系数为k_ir，微分控制系数为k_dr，由此可得两轮速度控制的陀螺仪修正控制量Δu_rot：

本例中，k_pr、k_ir和k_dr分别设置为1.2、0.05和4.1时可以取得较好的速度控制误差校正效果，不同控制方法其值存在差异。

4)根据两轮轮速差与两轮目标轮速差的误差，并采用两轮速度PI控制策略，获得两轮差速控制量；

控制系统两轮目标轮速差为v_s，旋转磁编码器测量到的两轮轮速差为v_m＝v_Lm-v_Rm，两轮轮速差与两轮目标轮速差的误差为e_s＝v_m-v_s，PI控制策略的比例控制系数为k_p，积分控制系数为k_i，通过PI控制策略获取两轮差速控制量Δu_s为：

Δu_s(t)＝k_pe_s(t)+k_i∫e_s(t)dt

本例中，比例控制系数k_p可以加快系统响应速度，提升系统控制精度和灵敏度。积分控制系数k_i可以消除控制系统的稳态误差，提高动态跟随性能。

5)根据陀螺仪修正控制量和两轮差速控制量，计算修正后的两轮差速控制量，对两轮速度控制量进行修正；

对于摩擦系数不够造成的速度误差，需通过减小未滑动车轮的运行速度，从而实现两轮对地速度的严格一致。对于某一车轮受到地形阻力导致对地速度变化，则需要对受阻车轮控制量进行调控。为了兼顾这两种情况，两轮速度控制量分为左、右两独立调控量，u_cl左轮速度控制量，u_cr为右轮速度控制量，修正后的两轮差速控制量Δu_c。

根据陀螺仪修正控制量和两轮差速控制量，计算修正后的两轮差速控制量：

根据修正后的两轮差速控制量，对两轮速度控制量进行补偿：

其中，u_c为两轮速度目标控制量。

加入Z轴陀螺仪信息后，控制系统利用Z轴陀螺仪检测移动机器人航向角角速度，并将测量的结果与给定的航向角参考角速度进行对比获取航向角角速度误差，该误差经过PID控制器生成两轮速度控制的陀螺仪修正控制量，根据陀螺仪修正控制量和两轮差速控制量，计算修正后的两轮差速控制量，对两轮速度控制量进行修正，提高两轮速度控制精度。

综上所述，对一种基于Z轴陀螺仪及轮速差的两轮速度控制方法进行仿真验证，图3为无Z轴陀螺仪修正时两轮对地运行速度，移动机器人两轮运行时由于受到不同的地形阻力作用，各自PID控制环路将车轮速度调节到参考速度，但两轮速度调节并不同步，导致出现较大的速度控制误差如图4。图5是在引入Z轴陀螺仪修正后，在相同地形环境下测得的结果，由于利用Z轴陀螺仪信息对两独立的PID控制回路进行调节，确保了两轮速度调节的同步性和一致性，使控制系统获得了较高的速度控制精度，速度控制误差结果如图6所示。实验中，相同地形环境下以3m/s运行的移动机器人，无Z轴陀螺仪修正时速度控制误差最大为0.41m/s，引入Z轴陀螺仪修正策略后，相同条件下的速度控制误差最大为0.074m/s，极大地提升了两轮对地参考系的速度控制精度，有利于确保移动机器人航向角稳定性。对于复杂环境下运行的两轮移动机器人单轮滑动或时变地形阻力导致的两轮速度控制误差，基于Z轴陀螺仪及轮速差的两轮速度控制能对该误差控制进行有效补偿与校正，确保两轮实际运行的速度一致。

Claims (3)

1.一种基于Z轴陀螺仪及轮速差的两轮速度控制方法，包括如下步骤：

步骤1：根据两轮移动机器人结构和运动特性，建立移动机器人旋转运动模型，获得两轮移动机器人航向角的目标角速度；

步骤2：通过速度传感器实时采集两轮速度量，通过Z轴陀螺仪采集姿态输出信息，对其解算得到移动机器人当前时刻的两轮轮速及航向角角速度；

步骤3：将两轮移动机器人航向角角速度与两轮移动机器人航向角的目标角速度做差，获取航向角角速度误差，计算得到两轮速度控制的陀螺仪修正控制量；

步骤4：根据两轮轮速差与两轮目标轮速差的误差，并采用两轮速度PI控制策略，获得两轮差速控制量；

步骤5：根据陀螺仪修正控制量和两轮差速控制量，计算修正后的两轮差速控制量，对两轮速度控制量进行修正。

2.根据权利要求1所述的一种基于Z轴陀螺仪及轮速差的两轮速度控制方法，其特征在于步骤3所述，计算得到两轮速度控制的陀螺仪修正控制量步骤如下：

由Z轴陀螺仪检测得到移动机器人航向角角速度，将两轮移动机器人航向角角速度与两轮移动机器人航向角的目标角速度做差，获取航向角角速度误差，计算得到两轮速度控制的陀螺仪修正控制量；

两轮速度控制的陀螺仪修正控制量Δu_rot计算公式为：

其中，陀螺仪修正控制的比例控制系数为k_pr，积分控制系数为k_ir，微分控制系数为k_dr；e_rot为移动机器人航向角角速度与目标角速度的误差。

3.根据权利要求1所述的一种基于Z轴陀螺仪及轮速差的两轮速度控制方法，其特征在于步骤5所述，对两轮速度控制量进行修正步骤如下：

根据陀螺仪修正控制量和两轮差速控制量，计算修正后的两轮差速控制量：

其中，两轮速度控制量分为左、右两独立调控量，u_cl左轮速度控制量，u_cr为右轮速度控制量，修正后的两轮差速控制量Δu_c，Δu_s为两轮差速控制量，e_s为两轮轮速差与两轮目标轮速差的误差，PI控制策略的比例控制系数为k_p，积分控制系数为k_i；

根据修正后的两轮差速控制量，对两轮速度控制量进行补偿：

其中，u_c为两轮速度目标控制量。