当前位置:   article > 正文

移动机器人MPC控制仿真实现_mpc模型是怎么控制移动机器人呢

mpc模型是怎么控制移动机器人呢

0 运动学建模

在这里插入图片描述

首先建立移动机器人运动学方程:
[ x ˙ y ˙ θ ˙ ] = [ c o s θ 0 s i n θ 0 0 1 ] [ v ω ]

[x˙y˙θ˙]
=
[cosθ0sinθ001]
[vω]
x˙y˙θ˙=cosθsinθ0001[vω]
其中, P = [ x y θ ] P=
[xyθ]
P=xyθ
为机器人全局坐标系下的位姿, u = [ v ω ] u=
[vω]
u=[vω]
为机器人的线速度和角速,是系统的控制输入,暂时不把左右轮转速引入。

移动机器人跟踪问题中输入为参考轨迹 P r P_r Pr和参考速度 u r u_r ur

这里MPC模型的状态量有多种选择:1)全局坐标系位姿;2)全局坐标系误差;3)局部坐标系误差(即全局误差投影到局部坐标系)。下面来一一建模分析。

1 局部误差为状态量

1.1 MPC建模

首先以局部误差为状态量进行分析,对于移动机器人的非完整性,存在以下假设:机器人不发生侧向滑动,即在全局坐标系下 y ˙ L = 0 \dot{y}_L=0 y˙L=0,即有:
x ˙ s i n θ − y ˙ c o s θ = 0 x ˙ c o s θ + y ˙ s i n θ = v

x˙sinθy˙cosθ=0x˙cosθ+y˙sinθ=v
x˙sinθy˙cosθx˙cosθ+y˙sinθ=0=v

此公式对于参考位姿和参考速度依然成立。
在这里插入图片描述

定义机器人局部坐标系下位姿误差为:
[ x e y e θ e ] = [ c o s θ s i n θ 0 − s i n θ c o s θ 0 0 0 1 ] [ x r − x y r − y θ r − θ ]

[xeyeθe]
=
[cosθsinθ0sinθcosθ0001]
[xrxyryθrθ]
xeyeθe=cosθsinθ0sinθcosθ0001xrxyryθrθ
则有:
x ˙ e = − ω s i n θ ( x r − x ) + ω c o s θ ( y r − y ) + c o s θ ( x ˙ r − x ˙ ) + s i n θ ( y ˙ r − y ˙ ) = ω y e − v + x ˙ r c o s θ + y ˙ r s i n θ   ( 由 于 θ ≠ θ r , 后 两 项 不 能 合 并 为 v r ) = ω y e − v + x ˙ r c o s ( θ r − θ e ) + y ˙ r s i n ( θ r − θ e ) = ω y e − v + x ˙ r ( c o s θ r c o s θ e + s i n θ r s i n θ e ) + y ˙ r ( s i n θ r c o s θ e − c o s θ r s i n θ e ) = ω y e − v + ( x ˙ r c o s θ r + y ˙ r s i n θ r ) c o s θ e + ( x ˙ r s i n θ r − y ˙ r c o s θ r ) s i n θ e = ω y e − v + v r c o s θ e y ˙ e = − ω c o s θ ( x r − x ) − ω s i n θ ( y r − y ) − s i n θ ( x ˙ r − x ˙ ) + c o s θ ( y ˙ r − y ˙ ) = − ω x e − x ˙ r s i n θ + y ˙ r c o s θ = − ω x e − x ˙ r s i n ( θ r − θ e ) + y ˙ r c o s ( θ r − θ e ) = − ω x e − x ˙ r ( s i n θ r c o s θ e − c o s θ r s i n θ e ) + y ˙ r ( c o s θ r c o s θ e + s i n θ r s i n θ e ) = − ω x e + ( − x ˙ r s i n θ r + y ˙ r c o s θ r ) c o s θ e + ( x ˙ r c o s θ r + y ˙ r s i n θ r ) s i n θ e = − ω x e + v r s i n θ e θ ˙ e = θ ˙ r − θ ˙ = ω r − ω
x˙e=ωsinθ(xrx)+ωcosθ(yry)+cosθ(x˙rx˙)+sinθ(y˙ry˙)=ωyev+x˙rcosθ+y˙rsinθ (θθrvr)=ωyev+x˙rcos(θrθe)+y˙rsin(θrθe)=ωyev+x˙r(cosθrcosθe+sinθrsinθe)+y˙r(sinθrcosθecosθrsinθe)=ωyev+(x˙rcosθr+y˙rsinθr)cosθe+(x˙rsinθry˙rcosθr)sinθe=ωyev+vrcosθey˙e=ωcosθ(xrx)ωsinθ(yry)sinθ(x˙rx˙)+cosθ(y˙ry˙)=ωxex˙rsinθ+y˙rcosθ=ωxex˙rsin(θrθe)+y˙rcos(θrθe)=ωxex˙r(sinθrcosθecosθrsinθe)+y˙r(cosθrcosθe+sinθrsinθe)=ωxe+(x˙rsinθr+y˙rcosθr)cosθe+(x˙rcosθr+y˙rsinθr)sinθe=ωxe+vrsinθeθ˙e=θ˙rθ˙=ωrω
x˙ey˙eθ˙e=ωsinθ(xrx)+ωcosθ(yry)+cosθ(x˙rx˙)+sinθ(y˙ry˙)=ωyev+x˙rcosθ+y˙rsinθ (θ=θrvr)=ωyev+x˙rcos(θrθe)+y˙rsin(θrθe)=ωyev+x˙r(cosθrcosθe+sinθrsinθe)+y˙r(sinθrcosθecosθrsinθe)=ωyev+(x˙rcosθr+y˙rsinθr)cosθe+(x˙rsinθry˙rcosθr)sinθe=ωyev+vrcosθe=ωcosθ(xrx)ωsinθ(yry)sinθ(x˙rx˙)+cosθ(y˙ry˙)=ωxex˙rsinθ+y˙rcosθ=ωxex˙rsin(θrθe)+y˙rcos(θrθe)=ωxex˙r(sinθrcosθecosθrsinθe)+y˙r(cosθrcosθe+sinθrsinθe)=ωxe+(x˙rsinθr+y˙rcosθr)cosθe+(x˙rcosθr+y˙rsinθr)sinθe=ωxe+vrsinθe=θ˙rθ˙=ωrω

写成矩阵形式为:
[ x ˙ e y ˙ e θ ˙ e ] = [ ω y e − v + v r c o s θ e − ω x e + v r s i n θ e ω r − ω ] (1)

[x˙ey˙eθ˙e]=[ωyev+vrcosθeωxe+vrsinθeωrω]
\tag{1} x˙ey˙eθ˙e=ωyev+vrcosθeωxe+vrsinθeωrω(1)

很明显上式是一个非线性系统,需要进行线性化:
[ x ˙ e y ˙ e θ ˙ e ] = [ 0 ω 0 − ω 0 0 0 0 0 ] [ x e y e θ e ] + [ − v + v r c o s θ e v r s i n θ e ω r − ω ]

[x˙ey˙eθ˙e]=[0ω0ω00000][xeyeθe]+[v+vrcosθevrsinθeωrω]
x˙ey˙eθ˙e=0ω0ω00000xeyeθe+v+vrcosθevrsinθeωrω
由于 lim ⁡ θ e → 0 c o s θ e = 1 ,   lim ⁡ θ e → 0 s i n θ e = θ e \displaystyle\lim_{\theta_e \to 0}cos\theta_e=1,\ \lim_{\theta_e \to 0}sin\theta_e=\theta_e θe0limcosθe=1, θe0limsinθe=θe,上式可写成:
[ x ˙ e y ˙ e θ ˙ e ] = [ 0 ω 0 − ω 0 v r 0 0 0 ] [ x e y e θ e ] + [ v r − v 0 ω r − ω ] = [ 0 ω 0 − ω 0 v r 0 0 0 ] [ x e y e θ e ] + [ 1 0 0 0 0 1 ] [ v r − v ω r − ω ]
[x˙ey˙eθ˙e]=[0ω0ω0vr000][xeyeθe]+[vrv0ωrω]=[0ω0ω0vr000][xeyeθe]+[100001][vrvωrω]
x˙ey˙eθ˙e=0ω0ω000vr0xeyeθe+vrv0ωrω=0ω0ω000vr0xeyeθe+100001[vrvωrω]

此时定义MPC系统状态量为 X = [ x e y e θ e ] X=

[xeyeθe]
X=xeyeθe,控制量为 u ~ = [ v r − v ω r − ω ] \widetilde u=
[vrvωrω]
u =[vrvωrω]
u ~ = u r − u \widetilde u=u_r-u u =uru

A = [ 0 ω 0 − ω 0 v r 0 0 0 ] ,   B = [ 1 0 0 0 0 1 ] A=

[0ω0ω0vr000]
,\ B=
[100001]
A=0ω0ω000vr0, B=100001,则有:
X ˙ = A X + B u ~ \dot X=AX+B\widetilde u X˙=AX+Bu

离散化:
X ( k + 1 ) − X ( k ) Δ T = A X ( k ) + B u ~ ( k )

X(k+1)X(k)ΔT=AX(k)+Bu~(k)
ΔTX(k+1)X(k)=AX(k)+Bu (k)
其中 Δ T \Delta T ΔT为离散时间间隔,即:
X ( k + 1 ) = ( I + Δ T ) A X ( k ) + Δ T B u ~ ( k )
X(k+1)=(I+ΔT)AX(k)+ΔTBu~(k)
X(k+1)=(I+ΔT)AX(k)+ΔTBu (k)

A ~ = ( I + Δ T ) A ,   B ~ = Δ T B \widetilde A=(I+\Delta T)A,\ \widetilde B=\Delta TB A =(I+ΔT)A, B =ΔTB,同时设定预测时域为 N N N,则有:
X ( k + 1 ) = A ~ X ( k ) + B ~ u ~ ( k ) X ( k + 2 ) = A ~ X ( k + 1 ) + B ~ u ~ ( k + 1 ) = A ~ 2 X ( k ) + A ~ B ~ u ~ ( k ) + B ~ u ~ ( k + 1 ) X ( k + 3 ) = A ~ 3 X ( k ) + A ~ 2 B ~ u ~ ( k ) + A ~ B ~ u ~ ( k + 1 ) + B ~ u ~ ( k + 2 ) . . . X ( k + N ) = A ~ N X ( k ) + ∑ i = 0 N − 1 A ~ N − i − 1 B ~ u ~ ( k + i )
X(k+1)=A~X(k)+B~u~(k)X(k+2)=A~X(k+1)+B~u~(k+1)=A~2X(k)+A~B~u~(k)+B~u~(k+1)X(k+3)=A~3X(k)+A~2B~u~(k)+A~B~u~(k+1)+B~u~(k+2)...X(k+N)=A~NX(k)+i=0N1A~Ni1B~u~(k+i)
X(k+1)X(k+2)X(k+3)...X(k+N)=A X(k)+B u (k)=A X(k+1)+B u (k+1)=A 2X(k)+A B u (k)+B u (k+1)=A 3X(k)+A 2B u (k)+A B u (k+1)+B u (k+2)=A NX(k)+i=0N1A Ni1B u (k+i)

这里其实做了简化,根据 A A A的表达式, A ~ ( k ) \widetilde A(k) A (k) A ~ ( k + 1 ) \widetilde A(k+1) A (k+1)不完全等价。

令:
Y ( k ) = [ X ( k + 1 ) X ( k + 2 ) . . . X ( k + N ) ] , Ψ = [ A ~ A ~ 2 . . . A ~ N ] , Δ U = [ u ~ ( k ) u ~ ( k + 1 ) . . . u ~ ( k + N − 1 ) ] Θ = [ B ~ 0 . . . 0 A ~ B ~ B ~ . . . 0 . . . A ~ N − 1 B ~ A ~ N − 2 B ~ . . . B ~ ] Y(k)=

[X(k+1)X(k+2)...X(k+N)]
, \Psi=
[A~A~2...A~N]
, \Delta U=
[u~(k)u~(k+1)...u~(k+N1)]
\\ \Theta =
[B~0...0A~B~B~...0...A~N1B~A~N2B~...B~]
Y(k)=X(k+1)X(k+2)...X(k+N),Ψ=A A 2...A N,ΔU=u (k)u (k+1)...u (k+N1)Θ=B A B ...A N1B 0B A N2B .........00B
则有:
Y ( k ) = Ψ X ( k ) + Θ Δ U Y(k)=\Psi X(k) + \Theta \Delta U Y(k)=ΨX(k)+ΘΔU
其中 Y ( k ) Y(k) Y(k) k k k时刻预测时域内的误差,定义代价函数:
J = Y T ( k ) Q Y ( k ) + Δ U T R Δ U
J=YT(k)QY(k)+ΔUTRΔU
J=YT(k)QY(k)+ΔUTRΔU

其中 Q Q Q R R R为权重。该代价函数可以使预测时域内误差最小,且对控制量也有一定约束。对 J J J作展开:
J = ( Ψ X ( k ) + Θ Δ U ) T Q ( Ψ X ( k ) + Θ Δ U ) + Δ U T R Δ U = X T ( k ) Ψ T Q Ψ X ( k ) + 2 X T ( k ) Ψ T Q Θ Δ U + Δ U T Θ T Q Θ Δ U + Δ U T R Δ U = Δ U T ( Θ T Q Θ + R ) Δ U + 2 X T ( k ) Ψ T Q Θ Δ U + X T ( k ) Ψ T Q Ψ X ( k )
J=(ΨX(k)+ΘΔU)TQ(ΨX(k)+ΘΔU)+ΔUTRΔU=XT(k)ΨTQΨX(k)+2XT(k)ΨTQΘΔU+ΔUTΘTQΘΔU+ΔUTRΔU=ΔUT(ΘTQΘ+R)ΔU+2XT(k)ΨTQΘΔU+XT(k)ΨTQΨX(k)
J=(ΨX(k)+ΘΔU)TQ(ΨX(k)+ΘΔU)+ΔUTRΔU=XT(k)ΨTQΨX(k)+2XT(k)ΨTQΘΔU+ΔUTΘTQΘΔU+ΔUTRΔU=ΔUT(ΘTQΘ+R)ΔU+2XT(k)ΨTQΘΔU+XT(k)ΨTQΨX(k)

由于在 k k k时刻, X ( k ) ,   Ψ ,   Θ X(k),\ \Psi,\ \Theta X(k), Ψ, Θ均为确定值,所以上式即为关于 Δ U \Delta U ΔU的二次型。


H = 2 ( Θ T Q Θ + R ) ,   f T = 2 X T ( k ) Ψ T Q Θ H=2(\Theta ^TQ\Theta+R),\ f^T=2X^T(k)\Psi ^TQ\Theta H=2(ΘTQΘ+R), fT=2XT(k)ΨTQΘ
即可利用MATLAB中quadprog函数求解。

在得到 Δ U ∗ \Delta U^* ΔU后,取其中第一个元素 u ~ ∗ ( k ) \widetilde u^*(k) u (k)作为 k k k时刻的MPC系统控制量,则实际速度控制量为:
u ∗ ( k ) = u r ( k ) − u ~ ∗ ( k ) u^*(k)=u_r(k)-\widetilde u^*(k) u(k)=ur(k)u (k)

1.2 仿真实现参考代码

clear;clc;
close all;

%% 仿真参数设定
dt = 0.01;                  % 离散时间间隔
num_step = 2000;            % 仿真步数
t = (0:num_step-1)*dt;      % 生成仿真时间序列

%% 运动学参数设定(本实例给定参考速度)
u_r = [1;0.5]*ones(1,num_step);                 % 参考速度(圆轨迹)
% u_r = [3;0]*ones(1,num_step);                   % 参考速度(直线轨迹)

% 利用积分法生成参考轨迹
P_r = zeros(3,num_step);
for k = 2:num_step
    last_theta = P_r(3,k-1);
    P_r(:,k) = P_r(:,k-1)+[cos(last_theta),0;sin(last_theta),0;0,1]*u_r(:,k-1)*dt;
end

% 设定初始位置和初始速度
P = [1;-1;0];               % 初始位置
u = [0;0];                  % 初始速度
X = zeros(3,num_step);      % 存储误差向量 

%% MPC控制器参数设定
N = 10;                     % 预测时序长度
Q = 5*diag([4,10,0.1]);     % 状态量权重
R = 1*diag([1,0.5]);        % 控制量权重
Q_ = kron(eye(N),Q);        % 利用K积将权重矩阵扩充到与Y相同维度
R_ = kron(eye(N),R);
B = [1,0;0,0;0,1];          % B 矩阵
B_ = dt*B;                  % B~ 矩阵

%% 二次型求解器设置(使用 interior-point-convex 算法,不显示迭代过程)
options = optimoptions('quadprog',...
    'Algorithm','interior-point-convex','Display','off');

%% 滚动优化 (为了不处理最后N个时刻的特殊情况,循环只进行到 num_step-N )
tic;
figure(1);
hold on;
xlabel('x(m)');
ylabel('y(m)');
for k = 1:num_step-N
    % 刻画每个时刻的参考位置和实际位置
    plot(P(1,k),P(2,k),'b.');
    plot(P_r(1,k),P_r(2,k),'r.');
    drawnow;
    
    % 计算当前时刻的 X
    Tx = [cos(P(3,k)),sin(P(3,k)),0;
        -sin(P(3,k)),cos(P(3,k)),0;
        0,0,1];
    X(:,k) = Tx*(P_r(:,k)-P(:,k));
    
    % 计算当前时刻的 \Psi 和 \Theta
    A = [0,u(2,k),0;-u(2,k),0,u_r(1,k);0,0,0];      % A 矩阵
    A_ = eye(size(P,1))+dt*A;         % A_ 矩阵
    Psi = zeros(3*N,3);                 % 初始化 \Psi
    Theta = zeros(3*N,2*N);             % 初始化 \Theta
    for j = 1:N
        Psi(3*j-2:3*j,1:3) = A_^j;      % \Psi 的第j块
        for i = 1:j
            Theta(3*j-2:3*j,2*i-1:2*i) = A_^(j-i)*B_;   % \Theta 的第(j,i)块
        end
    end
    
    % 计算 H 和 f
    H = 2*(Theta'*Q_*Theta+R_);
    f = (2*X(:,k)'*Psi'*Q_*Theta)';
    
    % 调用 quadprog 求解器
    [U_star,fval,exitflag,output,lambda] = quadprog(H,f,[],[],[],[],[],[],[],options);
    
    % 输入控制量利用积分计算下一个位姿和误差
    u_star = U_star(1:2);           % 取出 U 的第一部分(前两项)   
    u =[u, u_r(:,k) - u_star];      % 计算实际线速度和角速度控制量
    last_theta = P(3,k);
    P =[P, P(:,k)+[cos(last_theta),0;sin(last_theta),0;0,1]*u(:,k+1)*dt];
end
toc;

%% 误差收敛速度
mse = sqrt(sum((X.^2),1));
figure;
plot(t(1:num_step-N),mse(1:num_step-N),'b-','LineWidth',1)
xlabel('t(s)');
ylabel('mse');

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89

直线跟踪效果:
直线轨迹跟踪效果

圆轨迹跟踪效果:
圆轨迹跟踪效果

1.3结果分析

可以看到收敛速度较慢,有可能是权重影响的,可以自行修改参数进行仿真。另外还可以在quadprog求解器中加入控制量的约束,使其不要产生过大的加速度。

2 全局坐标系为状态量

3 全局误差为状态量

本文内容由网友自发贡献,转载请注明出处:https://www.wpsshop.cn/w/寸_铁/article/detail/925613
推荐阅读
相关标签
  

闽ICP备14008679号