PD控制的本质及其应用(二)

PD控制的本质及其应用(二)

关于这篇文章

阅读这篇文章之前请先阅读前一篇文章PD控制器的本质与应用(一) .

发布了上一篇文章之后, 被硕哥和王兴兴等dalao点赞了233, 还被李泽湘教授发朋友圈, 高二知乎某个问题推荐了李教授的" A Mathematical Introduction to Robotic Manipulation " 作为机器人学的入门书之一, 屁颠屁颠地去找来看, 发现一行公式都看不懂, 才深感自己之naive, 知识水平之低. 有了这么多大佬的肯定, 马上就把第二篇放出来.

这篇文章将介绍

  • 控制规律分解(Control-law partitioning)
  • 基于模型的非线性控制(二) (Model-based nonlinear control)
  • 轨迹生成(Trajectory generation)
  • 轨迹跟踪控制(Trajectory following control)

控制规律分解

为了方便对复杂和非线性系统的控制, 我们引入控制规律分解, 核心思想就是把整个系统分成完全独立的伺服部分(servo porion)和模型部分(model-based portion), 对于伺服部分来说, 模型部分的参数和是否非线性已经不重要, 将模型部分和伺服部分连接起来的过程其实就是线性化.

二阶线性系统例子(弹簧阻尼系统)

根据牛顿第二定律建立微分方程: m\ddot x +b \dot x + kx = F ,其中 F 是控制器的输出, 不同于之前我们先别急着找 F=? 使得整个系统满足  m\ddot {e} +b \dot {e} + ke =0 , 先将这个系统化简为一个单位质量, 这样对于伺服部分来说, 需要控制的对象就是一个除了控制器控制的力不受任何外力的质量为1的物块了(与上一篇的线性控制中的最简单质点类似), 无论整个系统多么复杂, 伺服部分用的都是同一个控制方法 具体应该怎么做呢?

对于伺服部分来说(m = 1)

F = \alpha F' + \beta , 我们需要找到 \alpha = ? \beta =? 使得整个系统等价于 F' = \ddot x , 答案是 \alpha = m, \beta = b\dot x + kx (如果无法理解请将这两个式子代入到原来到微分方程). 这样伺服部分就是: F' = m\ddot x_d + b(\dot x_d-\dot x) + k(x_d - x)\dot x_d = 0, \ddot x_d = 0 时等价于 F' = k_p{e}+k_d\dot {e} , 似乎和之前没什么区别? 关键在于, 无论模型部分多么复杂, 伺服部分可以永远是: F' = k_p{e}+k_d\dot {e} , 然后再用 F = \alpha F'+\beta 求出实际的输出, \alpha,\beta 起到连接伺服部分和模型部分的作用, 在这个例子中不能提现控制规律分解的优越性, 让我们看非线性例子.

平面上的四轴飞行器

这是上一篇局部线性化中的同款飞行器, u_1 是推力, u_2 是旋转的扭矩.

平面内的四轴飞行器, 图源自公开课Robotics: Aerial Robotics的作业文档

根据动力学我们可以建立三个有关的微分方程.

\ddot y = -u_1\frac{1}{m}sin(\phi)

\ddot z = -g + u_1\frac{1}{m} cos(\phi)

\ddot \phi = u_2\frac{1}{I}

其中, y 轴是非线性的, 我们只看 y 轴. 设 u_1 = \alpha u_1' + \beta , 我们需要找到 \alpha = ?,\beta=?使得整个系统等价于 u_1' = \ddot y , 答案是 \alpha = -\frac{m}{sin(\phi)}, \beta = 0 , 而伺服部分我们只需要和上面那个线性模型一样, 也使用最简单的PD控制器: u _1' = k_p{e}+k_d\dot {e} 即可, 通过 u_1 = \alpha u_1' + \beta 便可以求出真正的输出 u_1 .

在这个例子中, 使用局部线性化和控制规律分解的控制效果可能不大, 因为四轴飞行器的工作点都是接近 \phi = 0 的(也就是平飞状态), 这样是局部线性化为什么能奏效的原因, 下面的例子是无固定工作点的非线性情况.

倒立摆或单轴操作臂

注意, 这个倒立摆并非我们常说的倒立摆, 我们的目的不是保持其倒立, 而是控制它停在某个角度, 因此关节上有点电机可以产生扭矩.

图中的积木夹取装置就可以简单看作一个单轴操作臂, 来源见水印

一阶倒立摆或单轴操作臂

根据动力学建立系统的微分方程: ml^2\ddot \theta +mlgcos(\theta) = \tau , 控制规律分解: \tau = \alpha \tau'+\beta , 当 \alpha = ml^2, \beta = mlgcos(\theta) 时, 整个系统可以等价于 \tau' = \ddot \theta , 可以把它当作一个单位质量, 或者单位转动惯量(在数学上没有区别), 伺服部分独立于模型部分是: \tau_1' = k_p{e}+k_d\dot {e} , 模型部分通过 \tau = \alpha \tau'+\beta 可以算出实际输出值.

相信你已经感受到了控制规律分解的威力, 我们可以将整个系统分割, 使得对于线性系统来说, 要控制的对象只是一个单位质量, 或单位模型, 可以使用最简单的控制方式控制, 使用 \tau = \alpha \tau' + \beta 或者 u_1 = \alpha u_1' +\beta (在数学上是完全等价的, 只是换了个字母而已), 相当于得到一个被控系统的逆模型, 被控系统中的非线性部分被逆模型中的非线性部分互相抵消, 是得对于伺服部分来说, 系统是线性的. 这种方法比起局部线性化有着一定优势, 对于上文所说的操作臂, 它的工作点是不定的, 不像四轴飞行器, 工作时倾角都会在0附近( \phi \approx 0 ), 这样的情况非常难用局部线性化的思路来做.

轨迹生成

有了控制器, 我们可以给控制器一些控制目标, 比如让控制对象的值到达5( 或者让电机的角度旋转到5度 ), 不过如果我们要控制整个到达过程的速度, 加速度, 应该如何做呢? 首先我们需要一个关于时间的函数 \theta(t) , 它描述了每个时刻控制量的值(如每个时刻电机的角度), 而且它是光滑可求导的, \dot \theta(t), \ddot \theta(t) 就是控制量的导数和控制量的二阶导数与时间的关系.

我们将讨论两种轨迹生成器, 他们都可以制定开始: \theta(0) = \theta_{strat} 与目标值 \theta(\theta_{end}) = \theta_{end} , 在开始和目标的速度都为0: \theta(\theta_{strat}) =\theta(\theta_{end}) = 0 , 其他生成器如: 具有中间路径的 可以阅读书: 《机器人学导论》.

三次多项式

前文说到, 我们有四个约束条件: \theta(0) = \theta_{strat} ,\theta(\theta_{end}) = \theta_{end}, \theta(\theta_{strat}) =\theta(\theta_{end}) = 0, 自然而然的我们可以用一个三次多项式 ( 共有四个项 ) 来描述轨迹: \theta(t) = a_0 +a_1t+a_2t^2+a_3t^3 , 与四个约束条件连立, 可以解得: a_0 = \theta_{strat}, a_1 = 0, a_2 = \frac{3}{t_{end}^2}(\theta_{strat}-\theta_{end}), a_3 = -\frac{2}{t_{end}^3} , 其中 t_{end} 是可以自定义的, 可以通过设定平均速度 v 来求出: t_{end} = \frac{\theta_{end}-\theta_{strat}}{v} . 图像及Matlab代码(可以改一下参数试试):

开始值为0, 目标值为300, 平均速度为200的轨迹

%set the parameter
posStart = 0;
posEnd = 100;
speed = 200;

timeTotal = abs(posEnd - posStart)/speed;

t = 0:0.01:timeTotal;
a0 = posStart;
a1 = 0;
a2 = 3/(timeTotal)^2*(posEnd - posStart);
a3 = -2/(timeTotal)^3*(posEnd - posStart);
traj = posStart + a1*t +a2*t.^2 + a3*t.^3;
speed = 2*a2*t + 3*a3*t.^2;
subplot(121);
plot(t,traj);
ylabel('angle(°)')
xlabel('time(t)')
subplot(122);
plot(t,speed);
ylabel('speed(°/s)')
xlabel('time(t)')

可以看到, 这个轨迹的速度一直在变化, 而且最大速度大大超过了我们的平均速度, 我们在介绍一种生成器.

二次函数过渡的一次函数

顾名思义, 这个函数将是分段函数, 加速阶段和减速阶段都用一个二次函数, 而匀速阶段使用一个一次函数, 将这三个函数拼起来作为一个轨迹函数.

除了前面提到的四个约束条件: \theta(0) = \theta_{strat} ,\theta(\theta_{end}) = \theta_{end}, \theta(\theta_{strat}) =\theta(\theta_{end}) = 0 及平均速度(等同于总时间), 我们还多了:加速阶段和减速时加速度(通常一样): \ddot \theta , 同时加速阶段与匀速阶段衔接处的速度应该一致, 匀速阶段与加速阶段的衔接处的速度也应该一致, 设加速(减速)时间为: t_{acc} , 加速结束时角度为 \theta_{acc} , 我们能有第六个约束: \ddot \theta t_{acc}=\frac{\frac{\theta_{end}-\theta_{start}}{2}-\theta_{acc}}{\frac{t_{end}}{2}-t_{acc}} , 其中 \theta_{acc} = \theta_0 + \frac{1}{2}\ddot \theta t_{acc}^2 ,设平均速度为: v 可得: t_{acc} = \frac{|\theta_{end}-\theta_{strat}|}{2v} - \frac{\sqrt{a^2 \frac{|\theta_{end}-\theta{strat}|}{v}^2-4a (\theta_{end}-\theta_{strat})}}{2|a|}

\ddot \theta \geq\frac{4(\theta_{end}-\theta_{strat})}{t_{end}^2} 时,可以使用这个轨迹生成器(如果加速度较小, 而路程过短, 会无法加速到匀速便开始减速), 分为三段:

  1. 加速阶段: \theta_a(t) = \theta_{start} + \frac{\ddot \theta}{2} t^2
  2. 匀速阶段: \theta_u(t) = \ddot \theta t_{acc}(t-t_{acc})+\theta_{acc}+ \frac{\ddot \theta ^3}{2}
  3. 减速阶段: \theta_d(t) = \theta_{end}-{\frac{{\ddot \theta(\theta_{end}-\theta_{strat})}^2}{v}} +\frac{{\ddot \theta^2(\theta_{end}-\theta_{strat})}^2}{v}t+-\frac{t^2}{2}

具体的推导计算过程我就不写出来了, 表面原因是太繁琐而且就是无脑解方程, 实际原因就是我已经忘记怎么算了, 逃~, 夏令营算了半天一直算错(就是字面上的半天), 附上图片和Matlab代码(注意加速度方向)和可以直接在嵌入式上面跑的c代码(这个代码就是我们在比赛时使用的代码, 已经经过大量的测试了, 应该是没问题的, 应该嗯):

开始为0, 结束为300, 速度为180, 加速度为600的轨迹

%set the parameter
posStart = 0;
posEnd = 300;
acc = 600;
speed = 180;

timeTotal = abs(posEnd - posStart)/speed;
if (abs(acc)>=(abs(4*(posEnd - posStart))/(timeTotal^2)))
	timeAcc =  timeTotal/2 - sqrt(acc^2*timeTotal^2-4*acc*(posEnd-posStart))/abs(2*acc);
	posAcc = abs(posEnd  - posStart)/2 - acc*timeAcc*(timeTotal/2-timeAcc);
	a1 = posStart;
	b1 = 0;
	c1 = 0.5*acc;
	a2 = posEnd - (acc*timeTotal^2)/2;
	b2 = acc*timeTotal;
	c2 = -0.5*acc;
    speedActually = 2*c1*timeAcc;
    time1 = 0:0.01:timeAcc;
    traj1 = ones(1,size(time1,2))*a1 + c1*(time1.^2);
    time2 = timeAcc:0.01:(timeTotal-timeAcc);
    traj2 = ones(1,size(timeAcc,2))*a1 + c1*(timeAcc.^2) + speedActually*(time2 - ones(1,size(time2,2))*timeAcc);
    time3 = timeTotal-timeAcc:0.01:timeTotal;
    traj3 = ones(1,size(time3,2))*a2 +b2*time3 + c2*(time3.^2);
    time = [time1,time2,time3];
    traj = [traj1,traj2,traj3];
    plot(time,traj);
    title('trajectory');
    ylabel('angle(°)')
    xlabel('time(t)')
end

注释什么的...结合这个文章应该可以不用注释看吧

trajectory.h :

///****************************************************************************
 *  Copyright (C) 2018 RoboMaster.
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program. If not, see <http://www.gnu.org/licenses/>.
 ***************************************************************************/

/**
  *********************** (C) COPYRIGHT 2018 DJI **********************
  * @update
  * @history
  * Version     Date              Author           Modification
  * V1.0.0      July-18-2018	Qiayuan_Liao
  * @verbatim
  *********************** (C) COPYRIGHT 2018 DJI **********************
  */

  /**
  * @brief    struct of trajector
  */

	typedef struct
{
	float pos_last;
	float speed_actually;
	float time_total;
	float time_acc;					
	float pos_acc;			
	float a1,b1,c1;		
	float a2,b2,c2;		

} trajectory_t;

/**
  * @brief     trajectory init funciton, work out a1, b1, c1
  * @param[in] start:
  * @param[in] end:
  * @param[in] acc:
  * @param[in] speed:
  */
int	trajectory_calc(trajectory_t *traj,float traget, float acc, float speed);

/**
  * @brief   get the pos, speed, acc of trajector 
  * @param[in] *traj: traj you want to get the traget vlue
  * @param[in] output: pos, speed, acc
  * @param[in] time: 
  */
void trajectory_get(trajectory_t *traj,float output[3], float time);

trajectory.c:

/****************************************************************************
 *  Copyright (C) 2018 RoboMaster.
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program. If not, see <http://www.gnu.org/licenses/>.
 ***************************************************************************/

/**
  *********************** (C) COPYRIGHT 2018 DJI **********************
  * @update
  * @history
  * Version     Date              Author           Modification
  * V1.0.0      July-18-2018	Qiayuan_Liao
  * @verbatim
  *********************** (C) COPYRIGHT 2018 DJI **********************
  */

#include "trajectory.h"
#include <math.h>
#include <stdlib.h>
/**
  * @brief     trajectory init funciton, work out a1, b1, c1
  * @param[in] start:
  * @param[in] end:
  * @param[in] acc:
  * @param[in] speed:
  */
int	trajectory_calc(trajectory_t *traj, float traget, float acc, float speed){
	if((traget - traj->pos_last)<0){
		acc = -acc;
	}
	traj->time_total = fabs(traget - traj->pos_last)/speed;
	while(fabs(acc) < fabs(4.0*(traget - traj->pos_last))/(traj->time_total*traj->time_total)){//if distant is too small
		speed *= 0.5;//half hte speed 
		traj->time_total = fabs(traget - traj->pos_last)/speed;
	}
	traj->time_acc = traj->time_total/2.0 - sqrtf(acc*acc*(traj->time_total)*(traj->time_total)-4.0*acc*(traget-traj->pos_last))/fabs(2.0*acc);
	traj->pos_acc = fabs(traget  - traj->pos_last)/2.0 - (traj->time_total/2.0 - traj->time_acc)*acc*traj->time_acc;
	traj->a1 = traj->pos_last;
	traj->b1 = 0;
	traj->c1 = 0.5*acc;
	traj->a2 = traget - (acc*traj->time_total*traj->time_total)/2.0;
	traj->b2 = acc*traj->time_total;
	traj->c2 = -0.5*acc;
	traj->speed_actually = 2.0*traj->c1*traj->time_acc;
	traj->pos_last =traget;
	return 1;
}

/**
  * @brief   get the pos, speed, acc of trajector 
  * @param[in] *traj: traj you want to get the traget vlue
  * @param[in] output: pos, speed, acc
  * @param[in] time: 
  */
void trajectory_get(trajectory_t *traj,float output[3], float time){
	if(time <= traj->time_total){
		if(time < traj->time_acc){
			output[0]  = traj->a1 + traj->c1*time*time ;
			output[1]  = 2*traj->c1*time;
			output[2]  = 2*traj->c1;
		}
		else if(time < traj->time_total - traj->time_acc){
			output[0] = traj->speed_actually*(time - traj->time_acc) + traj->a1 +traj->c1*(traj->time_acc*traj->time_acc);
			output[1] = traj->speed_actually;
			output[2] = 0;
		}
		else {
			output[0]  = traj->a2 + traj->b2*time + traj->c2*time*time ;
			output[1]  = traj->b2 + 2*traj->c2*time;
			output[2]  = 2*traj->c2;
		}
	}
}

轨迹跟踪控制

当我们有轨迹生成器时, 当然要让控制器可以根据轨迹生成器给定的数值计算输出, 我们之前讨论的都是位置式的PD控制器, 还记得倒立摆或单轴操作臂吗?
倒立摆或单轴操作臂的轨迹跟踪

一阶倒立摆或单轴操作臂

像上文所说的:根据动力学建立系统的微分方程: ml^2\ddot \theta +mlgcos(\theta) = \tau , 控制规律分解: \tau = \alpha \tau'+\beta , 当 \alpha = ml^2, \beta = mlgcos(\theta) 时, 整个系统可以等价于 \tau' = \ddot \theta , 伺服部分独立于模型部分是其实是: \tau_1' = \ddot \theta_d + k_v(\dot \theta_d-\dot \theta) + k_p(\theta_d - \theta) , 当\ddot \theta = 0,\dot \theta=0, 才能与: \tau_1' = k_p{e}+k_d\dot {e} 等价, 现在每一个时刻的 \ddot \theta_d ,\dot \theta_d,\theta_d 都可从轨迹生成器获得, 而且 \ddot \theta_d ,\dot \theta_d 不一定等于 0 , 因此我们不能将其等价为 \tau_1' = k_p{e}+k_d\dot {e} , 对于轨迹跟踪来说, 我们要同时用到某一时刻的预期加速度, 预期速度, 预期位置, 因此控制器的输出应该为 \tau_1' = \ddot \theta_d + k_v(\dot \theta_d-\dot \theta) + k_p(\theta_d - \theta) , 控制框图如下:

如果有了解过控制框图的话, 可以参考.


最后一些话

这两篇文章, 大概是2018年robomaster高中生夏令营12组香港记者队所用到的所有控制理论了, 根据这两篇文章做出自动小车的底盘是没什么问题啦, 当然视觉识别黑线补偿这里没有提及, 最后附上几个框架图以供参考:

总框架图

妙算框架图

嵌入式框架图

参考文献

中英文版的机器人学导论:

Craig, John J. 机器人学导论. China Machine Press, 2005.

Craig, J. J. (2005).Introduction to robotics: mechanics and control(Vol. 3, pp. 48-70). Upper Saddle River, NJ, USA:: Pearson/Prentice Hal

Coursera Robotics: Aerial Robotics 的部分课程

编辑于 2019-06-28

文章被以下专栏收录