• 面向阿克曼移动机器人（自行车模型）的LQR（最优二次型调节器）路径跟踪方法

            线性二次调节器（Linear Quadratic Regulator，LQR）是针对线性系统的最优控制方法。LQR 方法标准的求解体系是在考虑到损耗尽可能小的情况下, 以尽量小的代价平衡其他状态分量。一般情况下，线性系统在LQR 控制方法中用状态空间方程描述，性能能指标函数由二次型函数描述。

    LQR 方法存在以下优点：

    1. 最小能量消耗和最高路径跟踪精度。
    2. 求解时能够考虑多状态情况。
    3. 鲁棒性较强。

    缺点：

    1. 控制效果和模型精确程度有很大相关性。
    2. 实时计算状态反馈矩阵和控制增益。

    一、系统模型

    1.1 车辆模型

            一般来说阿克曼移动机器人可以简化为自行车模型，是一个非线性时变系统，工程上一般通过在平衡点附近差分线性化转化为线性系统来分析和控制，具体就不推导了，我直接给出模型。

    1.2 线性系统状态反馈控制示意图

    状态反馈是线性能控线性系统镇定的一个有效方法，主要是通过极点配置方法寻找一组非正的闭环极点使得闭环系统大范围渐进稳定。

     A，B，C分别代表系统矩阵、输入矩阵和输出矩阵，K是待设计的状态反馈增益。

    二、控制器设计

    2.1 代价函数泛函设计

    最优控制里，代价函数一般设计为控制性能和控制代价的范数加权和，形式如下

    其中，期望和实际的误差系统定义为

    2.2 最优状态反馈控制律推导

    当想要状态与期望状态之间的误差越差越小，同时控制消耗更少的能量。求解极小值点时，新定义拉格朗日函数如下 

    在拉格朗日函数基础上对各个优化变量的一阶导为零 ，得

     当\Delta_{x(t)}=0时候，

    推导LQR控制律时候，设\lambda(t)=P_tx(t) ,求偏导后可得

    由 得

     由于状态量初始不为零，只能是

    又由于当上述方程成立时候，P收敛到了期望的范围 ，\dot{P}为零，因此得到立卡提方程形式的矩阵微分方程

    综上，通过迭代或者近似方法求解上述立卡提方程后，最优的控制律为

     2.3 连续立卡提方程求解流程

    For~~ iteration=1: iteration_{max}\\ ~~~~~~~\bar{P}=Q+A^{\top}PB(R+B^TPB)^{-1}B^{\top}PA\\ ~~~~~~~criteria=\overline\lambda(\vert\bar{P}-P\vert)\\ ~~~~~~~IF~~criteria<\epsilon\\ ~~~~~~~~~~Break\\ ~~~~~~P=\bar{P}

    三、具体实现代码

    3.1 main函数

    1. close all
    2. clear;
    3. clc; 
    4. cx = [];
    5. cy= [];
    6. y0 = @(t_step) 10*sin(2 * t_step + 1);
    7. x0_dot= @(t_step) 5 * 2 * cos(2 * t_step + 1);
    8. x0 = @(t_step) -40*cos(t_step + 0.5);
    9. for theta=0:pi/200:2*pi
    10.     cx(end + 1) = x0(theta);
    11.     cy(end + 1) = y0(theta);
    12. end
    13. refer_path_primary= [cx', cy'];
    14. x = refer_path_primary(:, 1)';
    15. y = refer_path_primary(:, 2)';
    16. points = [x; y]';
    17. ds = 0.1 ;%等距插值处理的间隔
    18. distance = [0, cumsum(hypot(diff(x, 1), diff(y, 1)))]';
    19. distance_specific = 0:ds:distance(end);
    20. hypot(diff(x, 1), diff(y, 1));
    21. diff(x, 1);
    22. diff(y, 1);
    23. s = 0:ds:distance(end);
    24. refer_path= interp1(distance, points, distance_specific, 'spline');
    25. figure(1)
    26. % 绘制拟合曲线
    27. plot(refer_path(:, 1), refer_path(:, 2),'b-',x, y,'r.');
    28. hold on
    29. refer_path_x = refer_path(:,1);  % x
    30. refer_path_y = refer_path(:,2); % y
    31. for i=1:length(refer_path)
    32.     if i==1
    33.          dx = refer_path(i + 1, 1) - refer_path(i, 1);
    34.          dy = refer_path(i + 1, 2) - refer_path(i, 2);
    35.          ddx = refer_path(3, 1) + refer_path(1, 1) - 2 * refer_path(2, 1);
    36.          ddy = refer_path(3, 2) + refer_path(1, 2) - 2 * refer_path(2, 2);
    37.     elseif  i==length(refer_path)
    38.          dx = refer_path(i, 1) - refer_path(i - 1, 1);
    39.          dy = refer_path(i, 2) - refer_path(i - 1, 2);
    40.          ddx = refer_path(i, 1) + refer_path(i - 2, 1) - 2 * refer_path(i - 1, 1);
    41.          ddy = refer_path(i, 2) + refer_path(i - 2, 2) - 2 * refer_path(i - 1, 2);
    42.     else
    43.           dx = refer_path(i + 1, 1) - refer_path(i, 1);
    44.           dy = refer_path(i + 1, 2) - refer_path(i, 2);
    45.           ddx = refer_path(i + 1, 1) + refer_path(i - 1, 1) - 2 * refer_path(i, 1);
    46.           ddy = refer_path(i + 1, 2) + refer_path(i - 1, 2) - 2 * refer_path(i, 2);
    47.     end
    48.     refer_path(i,3)=atan2(dy, dx);%yaw
    49.     refer_path(i,4)=(ddy * dx - ddx * dy) / ((dx ^ 2 + dy ^ 2) ^ (3 / 2));
    50. end
    51. figure(2)
    52. plot(refer_path(:, 3),'b-');
    53. figure(3)
    54. plot(refer_path(:, 4),'b-')
    55. % 
    56. %%目标及初始状态
    57. L=2;%车辆轴距
    58. v=2;%初始速度
    59. dt=0.05;%时间间隔
    60. goal=refer_path(end,1:2);
    61. x_0=refer_path_x(1);
    62. y_0=refer_path_y(1);
    63. psi_0 = refer_path(1, 3);
    64. % %运动学模型
    65. ugv = KinematicModel(x_0, y_0, psi_0, v, dt, L);
    66. Q = eye(3) * 3.0;
    67. R = eye(2) * 2.0;
    68. robot_state = zeros(4, 1);
    69. step_points = length(refer_path(:, 1));
    70. for i=1:1:step_points
    71.     robot_state(1)=ugv.x;
    72.     robot_state(2)=ugv.y;
    73.     robot_state(3)=ugv.psi;
    74.     robot_state(4)=ugv.v;
    75.     [e, k, ref_yaw, min_idx] = calc_track_error(robot_state(1), robot_state(2), refer_path);
    76.     ref_delta = atan2(L * k, 1);
    77.     [A, B] = state_space( robot_state(4), ref_delta, ref_yaw, dt, L);
    78.     delta = lqr(robot_state, refer_path, min_idx, A, B, Q, R);
    79.     delta = delta + ref_delta;
    80.     [ugv.x, ugv.y, ugv.psi, ugv.v] = update(0, delta, dt, L, robot_state(1), robot_state(2),robot_state(3), robot_state(4));
    81.     ugv.record_x(end + 1 ) = ugv.x;
    82.     ugv.record_y(end + 1 ) = ugv.y;
    83.     ugv.record_psi(end + 1 ) = ugv.psi;
    84.      ugv.record_phy(end + 1 ) = ref_delta;
    85. end
    86. figure(4)
    87. % 绘制拟合曲线
    88. %  scr_size = get(0,'screensize');
    89. % set(gcf,'outerposition', [1 1 scr_size(4) scr_size(4)]);
    90. plot(ugv.record_x , ugv.record_y, Color='m',LineStyle='--',LineWidth=2);
    91. axis([-40,40,-40,40])
    92. grid on
    93. hold on
    94. % 绘制车辆曲线
    95. axis equal
    96. for ii = 1:1:length(ugv.record_x)
    97.        
    98.         h =  PlotCarbox(ugv.record_x(ii), ugv.record_y(ii), ugv.record_psi(ii), 'Color', 'r',LineWidth=2);
    99.         h1 = plot(ugv.record_x(1:ii), ugv.record_y(1:ii),'Color', 'b');
    100.         th1 = text(ugv.record_x(ii), ugv.record_y(ii)+10, ['#car', num2str(1)], 'Color', 'm');
    101.         set(th1, {'HorizontalAlignment'},{'center'});
    102.         h2 = PlotCarWheels(ugv.record_x(ii), ugv.record_y(ii), ugv.record_psi(ii),ugv.record_phy(ii),'k',LineWidth=2);
    103.         h3 = plot(ugv.record_x(1:ii) , ugv.record_y(1:ii), Color='b',LineStyle='-',LineWidth=4);
    104.         drawnow
    105.         delete(h); delete(h1);delete(th1);delete(h3);
    106.         for jj = 1:1:size(h2)
    107.             delete(h2{jj});
    108.         end
    109. end
    110. % 
    111. function [P] =  cal_Ricatti(A, B, Q, R)
    112.  Qf = Q;
    113.  P = Qf;
    114.   iter_max = 100;
    115.   Eps = 1e-4;
    116.  for step = 1:1:iter_max
    117.      P_bar = Q + A' * P * A - A' * P * B * pinv(R + B' * P *B) * B' * P * A;
    118.      criteria = max(abs(P_bar - P));
    119.      if  criteria < Eps
    120.          break;
    121.      end
    122.      P = P_bar;
    123.  end
    124.  
    125. end
    126. %%LQR控制器
    127. function[u_star]=lqr(robot_state, refer_path, s0, A, B, Q, R)
    128.     x = robot_state(1:3) - refer_path(s0,1:3)';
    129.     P =  cal_Ricatti(A, B, Q, R);
    130.     K= -pinv(R + B' * P * B) * B' * P * A;
    131.     u = K * x;%状态反馈
    132.     u_star = u(2);
    133. end
    134.  
    135. function [e, k, yaw, min_idx]=calc_track_error(x, y, refer_path)
    136.     p_num = length(refer_path);
    137.     d_x = zeros(p_num, 1);
    138.     d_y = zeros(p_num, 1);
    139.     d = zeros(p_num, 1);
    140.     for i=1:1:p_num 
    141.         d_x(i) = refer_path(i, 1) - x; 
    142.         d_y(i) = refer_path(i, 2) - y;
    143.     end
    144.     for i=1:1:p_num 
    145.         d(i) = sqrt(d_x(i) ^2 + d_y(i) ^ 2) ;
    146.     end
    147.     [~, min_idx] = min(d); 
    148.     yaw = refer_path(min_idx, 3);
    149.     k = refer_path(min_idx, 4);
    150.     angle= normalize_angle(yaw - atan2(d_y(min_idx), d_x(min_idx)));
    151.     e = d(min_idx);
    152.     if angle < 0
    153.         e = e*(-1);
    154.     end
    155. end
    156. %%将角度取值范围限定为[-pi,pi]
    157. function [angle]=normalize_angle(angle)
    158.     while angle > pi
    159.         angle = angle - 2*pi;
    160.     end
    161.     while angle < pi
    162.         angle = angle + 2*pi;
    163.     end
    164. end
    165. function [x_next, y_next, psi_next, v_next] = update(a, delta_f, dt, L, x, y, psi, v)
    166.      x_next = x + v * cos(psi) * dt;
    167.      y_next = y + v * sin(psi) * dt;
    168.      psi_next = psi + v / L * tan(delta_f) * dt;
    169.      v_next = v + a * dt;
    170. end
    171. function [A, B]=state_space(v, ref_delta, ref_yaw, dt, L)
    172.     A=[ 1.0, 0.0,  -v * dt * sin(ref_yaw);
    173.            0.0, 1.0,   v *  dt * cos(ref_yaw);
    174.            0.0, 0.0,   1.0  ];
    175.     B =[ dt * cos(ref_yaw), 0;
    176.            dt * sin(ref_yaw), 0;
    177.            dt * tan(ref_delta) / L, v * dt / (L * cos(ref_delta) * cos(ref_delta))];
    178. end
    179. function h = PlotCarbox(x, y, theta, varargin)
    180. Params = GetVehicleParams();
    181.  
    182. carbox = [-Params.Lr -Params.Lb/2; Params.Lw+Params.Lf -Params.Lb/2; Params.Lw+Params.Lf Params.Lb/2; -Params.Lr Params.Lb/2];
    183. carbox = [carbox; carbox(1, :)];
    184.  
    185. transformed_carbox = [carbox ones(5, 1)] * GetTransformMatrix(x, y, theta)';
    186. h = plot(transformed_carbox(:, 1), transformed_carbox(:, 2), varargin{:});
    187.  
    188. end
    189. function hs = PlotCarWheels(x, y, theta, phy, varargin)
    190. Params = GetVehicleParams();
    191.  
    192. wheel_box = [-Params.wheel_radius -Params.wheel_width / 2;
    193.     +Params.wheel_radius -Params.wheel_width / 2;
    194.     +Params.wheel_radius +Params.wheel_width / 2;
    195.     -Params.wheel_radius +Params.wheel_width / 2];
    196.  
    197. front_x = x + Params.Lw * cos(theta);
    198. front_y = y + Params.Lw * sin(theta);
    199. track_width_2 = (Params.Lb - Params.wheel_width / 2) / 2;
    200.  
    201. boxes = {
    202.     TransformBox(wheel_box, x - track_width_2 * sin(theta), y + track_width_2 * cos(theta), theta);
    203.     TransformBox(wheel_box, x + track_width_2 * sin(theta), y - track_width_2 * cos(theta), theta);
    204.     TransformBox(wheel_box, front_x - track_width_2 * sin(theta), front_y + track_width_2 * cos(theta), theta+phy);
    205.     TransformBox(wheel_box, front_x + track_width_2 * sin(theta), front_y - track_width_2 * cos(theta), theta+phy);
    206. };
    207.  
    208. hs = cell(4, 1);
    209. for ii = 1:4
    210.     hs{ii} = fill(boxes{ii}(:, 1), boxes{ii}(:, 2), varargin{:});
    211. end
    212.  
    213. end
    214.  
    215. function transformed = TransformBox(box, x, y, theta)
    216. transformed = [box; box(1, :)];
    217. transformed = [transformed ones(5, 1)] * GetTransformMatrix(x, y, theta)';
    218. transformed = transformed(:, 1:2);
    219. end
    220. function mat = GetTransformMatrix(x, y, theta)
    221. mat = [ ...
    222.     cos(theta) -sin(theta), x; ...
    223.     sin(theta) cos(theta), y; ...
    224.     0 0 1];
    225. end
    226.

    3.2 运动学结构体： 

    1. classdef  KinematicModel
    2.     properties
    3.         x;
    4.         y;
    5.         psi;
    6.         v;
    7.         dt;
    8.         L;
    9.         record_x;
    10.         record_y;
    11.         record_psi;
    12.         record_phy;
    13.     end
    14.     methods
    15.         function self=KinematicModel(x, y, psi, v, dt, L)
    16.             self.x=x;
    17.             self.y=y;
    18.             self.psi=psi;
    19.             self.v = v;
    20.             self.L = L;
    21.             % 实现是离散的模型
    22.             self.dt = dt;
    23.             self.record_x = [];
    24.             self.record_y= [];
    25.             self.record_psi= [];
    26.             self.record_phy= [];
    27.         end
    28. end
    29. end

    四、仿真参数和效果

     4.1 参数配置

    1. %期望轨迹
    2. y0 = @(t_step) 10*sin(2 * t_step + 1);
    3. x0_dot= @(t_step) 5 * 2 * cos(2 * t_step + 1);
    4. L=2;%车辆轴距
    5. v=2;%初始速度
    6. dt=0.05;%时间间隔
    7. Q = eye(3) * 3.0;
    8. R = eye(2) * 2.0;
    9. robot_state = zeros(4, 1);
    10. VehicleParams.Lw = 2.8 * 2; % wheel base
    11. VehicleParams.Lf = 0.96 * 2; % front hang
    12. VehicleParams.Lr = 0.929 * 2; % rear hang
    13. VehicleParams.Lb = 1.942 * 2; % width
    14. VehicleParams.Ll = VehicleParams.Lw + VehicleParams.Lf + VehicleParams.Lr; % length
    15. VehicleParams.f2x = 1/4 * (3*VehicleParams.Lw + 3*VehicleParams.Lf - VehicleParams.Lr);
    16. VehicleParams.r2x = 1/4 * (VehicleParams.Lw + VehicleParams.Lf - 3*VehicleParams.Lr);
    17. VehicleParams.radius = 1/2 * sqrt((VehicleParams.Lw + VehicleParams.Lf + VehicleParams.Lr) ^ 2 / 4 + VehicleParams.Lb ^ 2);
    18. VehicleParams.a_max = 0.5;
    19. VehicleParams.v_max = 2.5;
    20. VehicleParams.phi_max = 0.7;
    21. VehicleParams.omega_max = 0.5;
    22. % for wheel visualization
    23. VehicleParams.wheel_radius = 0.32*2;
    24. VehicleParams.wheel_width = 0.22*2;
    25. iter_max = 100;
    26. Eps = 1e-4;

      4.1 仿真效果

  • 相关阅读:
    WZOI-584矩阵快速幂
    react源码分析：深度理解React.Context
    测试自动化的边缘：DevTestOps 和 DevSecOps
    Dreamweaver教程从入门到精通 html篮球网站制作 学生静态网页作业源码模板
    linux拨号上网
    强缓存和协商缓存原理，及前端如何和服务端一同控制影响浏览器缓存，以及代码实战
    微软语音扩展全球语言支持，发布160个新声音
    C语言之extern关键字实例总结(八十二)
    centos7安装mongodb
    Node.js 中的进程和线程
  • 原文地址：https://blog.csdn.net/Ezekiel_Mok/article/details/140105628