浅谈机器人基础概论--运动控制算法

一、PID控制

提到智能车或运动控制,最经典,最常用的就是PID控制,PID算法是一种闭环控制算法,其中PID是比例 (Proportion) 积分 ,(Integral) 微分 ,(Differential coefficient) 的缩写,分别代表了三种控制算法。
这里,使用一个过阻尼的二阶系统作为被控对象,对PID控制进行仿真测试:

浅谈机器人基础概论--运动控制算法_第1张图片
由于控制器一般为数字控制器,这里采用离散形式的PID。
浅谈机器人基础概论--运动控制算法_第2张图片

单位阶跃相应的时域信号可以利用单片机,DSP,FPGA等主控芯片强大的计算能力,对传统散式PID进行改良。常见的方法有,积分分离,微分先行,模糊PID等。
理工智控路威机器人便使用该算法作为底盘运动控制核心算法。部分代码样例如下:

#include "pid.h"

#define PID_SCALE  0.01f  //PID缩放系数
#define PID_INTEGRAL_UP 1000  //积分上限

int16_t ax_motor_kp=200;  //电机转速PID-P	比例
int16_t ax_motor_ki=0;    //电机转速PID-I	积分
int16_t ax_motor_kd=400;  //电机转速PID-D	微分

/**
  * @简  述  电机A PID控制函数
  * @参  数  spd_target:编码器速度目标值 ,范围(±250)
  *          spd_current: 编码器速度当前值
  * @返回值  电机PWM速度
  */
int16_t AX_PID_MotorVelocityCtlA(int16_t spd_target, int16_t spd_current)
{
	static int16_t motor_pwm_out;
	static int32_t bias,bias_last,bias_integral = 0;

	//获得偏差值
	bias = spd_target - spd_current;
	
	//计算偏差累加值
	bias_integral += bias;
	
	//抗积分饱和
	if(bias_integral>PID_INTEGRAL_UP)bias_integral = PID_INTEGRAL_UP;
	if(bias_integral<-PID_INTEGRAL_UP)bias_integral = -PID_INTEGRAL_UP;
	
	//PID计算电机输出PWM值
	motor_pwm_out += ax_motor_kp*bias*PID_SCALE + ax_motor_kd*(bias-bias_last)*PID_SCALE + ax_motor_ki*bias_integral*PID_SCALE;
	
	//记录上次偏差
	bias_last = bias;
	
	//限制最大输出
	if(motor_pwm_out > 1950)
		motor_pwm_out = 1950;
	if(motor_pwm_out < -1950)
		motor_pwm_out = -1950;
  
	//返回PWM控制值
	return motor_pwm_out;
}	

/**
  * @简  述  电机B PID控制函数
  * @参  数  spd_target:编码器速度目标值 
  *          spd_target: 编码器速度当前值
  * @返回值  电机PWM速度
  */
int16_t AX_PID_MotorVelocityCtlB(int16_t spd_target, int16_t spd_current)
{
	static int16_t motor_pwm_out;
	static int32_t bias,bias_last,bias_integral = 0;
	
	//获得偏差值
	bias = spd_target - spd_current;
	
	//计算偏差累加值
	bias_integral += bias;
	
  //抗积分饱和
	if(bias_integral>PID_INTEGRAL_UP)bias_integral = PID_INTEGRAL_UP;
	if(bias_integral<-PID_INTEGRAL_UP)bias_integral = -PID_INTEGRAL_UP;
	
	//PID计算电机输出PWM值
	motor_pwm_out += ax_motor_kp*bias*PID_SCALE + ax_motor_kd*(bias-bias_last)*PID_SCALE + ax_motor_ki*bias_integral*PID_SCALE;
	
	//记录上次偏差
	bias_last = bias;
	
	//限制最大输出
	if(motor_pwm_out > 1950)
		motor_pwm_out = 1950;
	if(motor_pwm_out < -1950)
		motor_pwm_out = -1950;

	//返回PWM控制值
	return motor_pwm_out;
}

/**
  * @简  述  电机C PID控制函数
  * @参  数  spd_target:编码器速度目标值 
  *          spd_target: 编码器速度当前值
  * @返回值  电机PWM速度
  */
int16_t AX_PID_MotorVelocityCtlC(int16_t spd_target, int16_t spd_current)
{
	static int16_t motor_pwm_out;
	static int32_t bias,bias_last,bias_integral = 0;
	
	//获得偏差值
	bias = spd_target - spd_current;
	
	//计算偏差累加值
	bias_integral += bias;
	
  //抗积分饱和
	if(bias_integral>PID_INTEGRAL_UP)bias_integral = PID_INTEGRAL_UP;
	if(bias_integral<-PID_INTEGRAL_UP)bias_integral = -PID_INTEGRAL_UP;
	
	//PID计算电机输出PWM值
	motor_pwm_out += ax_motor_kp*bias*PID_SCALE + ax_motor_kd*(bias-bias_last)*PID_SCALE + ax_motor_ki*bias_integral*PID_SCALE;
	
	//记录上次偏差
	bias_last = bias;
	
	//限制最大输出
	if(motor_pwm_out > 1950)
		motor_pwm_out = 1950;
	if(motor_pwm_out < -1950)
		motor_pwm_out = -1950;

	//返回PWM控制值
	return motor_pwm_out;
}

/**
  * @简  述  电机D PID控制函数
  * @参  数  spd_target:编码器速度目标值 
  *          spd_target: 编码器速度当前值
  * @返回值  电机PWM速度
  */
int16_t AX_PID_MotorVelocityCtlD(int16_t spd_target, int16_t spd_current)
{
	static int16_t motor_pwm_out;
	static int32_t bias,bias_last,bias_integral = 0;
	
	//获得偏差值
	bias = spd_target - spd_current;
	
	//计算偏差累加值
	bias_integral += bias;
	
  //抗积分饱和
	if(bias_integral>PID_INTEGRAL_UP)bias_integral = PID_INTEGRAL_UP;
	if(bias_integral<-PID_INTEGRAL_UP)bias_integral = -PID_INTEGRAL_UP;
	
	//PID计算电机输出PWM值
	motor_pwm_out += ax_motor_kp*bias*PID_SCALE + ax_motor_kd*(bias-bias_last)*PID_SCALE + ax_motor_ki*bias_integral*PID_SCALE;
	
	//记录上次偏差
	bias_last = bias;
	
	//限制最大输出
	if(motor_pwm_out > 1950)
		motor_pwm_out = 1950;
	if(motor_pwm_out < -1950)
		motor_pwm_out = -1950;

	//返回PWM控制值
	return motor_pwm_out;
}

二、滑模变结构控制

不同于PID控制,滑模变结构控制(Variable Structure Control, VSC)是一种非连续的控制算法,这种系统的输出值只有开关两种状态,系统通过对切换函数进行计算来做出决策,确定系统应该处于那种状态。它其实是一种非线性控制,主要特点是控制的不连续性和控制结构的可变性,这种控制是依据被控对象的状态动态调整,使得系统状态轨迹按照设计好的滑动模态运动,所以得名滑模控制。(所谓“变结构”可以初步理解为在开关两种状态之间切换)

滑模态是预先设计好的,与系统的状态和外界的扰动等都不相关,因此具有参数扰动不灵敏性和实现简单的优点,是一种鲁棒控制方法。

为方便大家理解,这里以路威机器人的电机速度环控制为例,将PID与滑模变结构控制两种控制方式进行类比:

PID算法大家应该都比较熟悉,首先当前计算误差
e[t] = r[t] - c[t]
通过当前误差于上一次误差,计算出输出量为
c[t] = Kp*e[t] + Kd*(e[t]-e[t-1])+Ki*(e[t]+e[t-1]+... +e[1])
最后,再根据M,对c[t]进行限幅即可

首先要计算切换函数,理论上来说,切换函数可以使用:
s[t]= a*e[t]+b*e[t-1]
当s[t]>0时,c[t]=M 当s[t]<0时,c[t]=-M

符号说明:
r[t]:当前系统设定值(给定量)
c[t]:系统输出值(控制量)
e[t]:当前系统设定值与系统输出值的差
e[t-1]:上一个控制周期系统的设定值与系统输出值的差值
Kp:常数,比例系数
Ki:常数,积分系数(积分时间)
Kd:常数,微分系数(微分时间)
s[t]:当前开关函数的计算结果
M:常数,由于智能车一般不会满电压(占空比)跑,M是一个限幅值,输出的电压(或占空比)应该被限制在[-M,M]的范围内。

三、模糊控制

扎德(L.A.Zadeh)教授在1965年提出模糊逻辑,并于1973年提出把模糊逻辑应用于控制理论。模糊控制属于早期智能控制的一种。

模糊控制器(也叫做“模糊逻辑控制器”)的基本结构:
浅谈机器人基础概论--运动控制算法_第3张图片
什么是“模糊”?:
我们以电机转速为例,简单理解一下“模糊”的概念:
对于电机的转速度r,我们可以使用r=3600转,r=4800转,这类精确的数值来进行描述。
此外还可以使用“转得快”,“转得慢”,没有转”,“转得非常快“等模糊语言进行描述。
比如,当我们定义4000转为“转得快”,2000,转为“转得慢”,那么,对于r=3800转,时,有a4000+b2000 = 3800( b=a-1)解方程得a=0.9 , b=0.1
我们可以认为,r=3800转九成属于“转得快”,一成属于“转得慢”。
这个过程就是“模糊化”。其中a=0.9称为r关于“转得快”的隶属度,b=0.1称为r关于“转得慢”的隶属度。

模糊化
对于一个变量x,我们可以将其划分为七个等级:

NB
NM 中等
NS
ZO
PS
PM 中等
PB

分别计算x关于NB,NM,NS,ZO,SP,PM,PB的隶属度,得到x的模糊集
X = (a(x),b(x),c(x),d(x),e(x),f(x),g(x))
其中,a(x),b(x),c(x),d(x),e(x),f(x),g(x)分别为NB,NM,NS,ZO,PS,PM,PB的隶属度函数:
一般情况下,隶属度函数满足
a(x)+b(x)+c(x)+d(x)+e(x)+g(x)=1
浅谈机器人基础概论--运动控制算法_第4张图片
2.模糊规则表
模糊规则表中包含了人类关于某种控制过程的知识和经验,它是模糊控制的重要组成部分。
而推理机的作用是:将输入变量对应的模糊集通过查表的方式,转换为输出变量的模糊集。
比如一个输入变量的模糊集:
x = (0.1, 0.8, 0.1, 0, 0, 0)
浅谈机器人基础概论--运动控制算法_第5张图片
通过查表得到输出变量的模糊集:
y = (0, 0, 0, 0, 0.1, 0.9)
(其中0.9 = 0.8+0.1)。

解模糊
解模糊是模糊化的逆过程:
对于输出变量y,我们也将其划分为七个等级:
NB ,NM,NS,ZO,PS,PM,PB
这些等级分别对应的数值为
y1,y2,y3,y4,y5,y6,y7
从而,对于已知的模糊集
y = (s1,s2,s3,s4,s5,s6,s7)
我们可以计算出y的精确数值:
y = s1*y1+ s2*y2+s3*y3+s4*y4+ s5*y5+ s6*y6+s7*y7

四、自适应控制

前面介绍的几种控制方法,都需要具体的参数,比如PID控制中的kp,ki,kd。只有这些参数调整为合适的数值,才能使控制器工作在最佳状态。而自适应控制器可以自动计算出参数,一方面,减少了调节参数的麻烦;另一方面,一定程度上可以根据环境的变化,灵活地对控制器做出调整。
自适应过程是一个不断逼近目标的过程。它所遵循的途径以数学模型表示,称为自适应算法。通常采用基于梯度的算法,其中最小均方误差算法(即LMS算法)尤为常用。自适应算法可以用硬件(处理电路)或软件(程序控制)两种办法实现。前者依据算法的数学模型设计电路,后者则将算法的数学模型编制成程序并用计算机实现。

自适应控制器的种类和结构有很多种。
浅谈机器人基础概论--运动控制算法_第6张图片
浅谈机器人基础概论--运动控制算法_第7张图片

五、小结

运动控制算法多种多样,但是最常用的还是PID算法,实际在使用过程中还是需要根据需求和场景去选择算法。

你可能感兴趣的:(机器人,机器人,算法,制造,人工智能,c++)