[发明专利]移动机器人跟踪预定轨迹的启发式动态规划控制方法

摘要

本发明公开了一种移动机器人跟踪预定轨迹的启发式动态规划控制方法，其步骤包括：1、建立大地坐标系和机器人坐标系；2、确定移动机器人的位姿信息和预瞄点位姿信息；3、计算预瞄点位置的道路曲率；4、建立移动机器人的马尔可夫决策过程模型；5、基于启发式动态规划算法设计跟踪预定轨迹的控制方法；6、推导启发式动态规划算法中执行器和评价器的在线学习规则。7、判断预瞄点是否为预定轨迹中的最后一位置坐标，若是，则控制机器人减速至预瞄点位置，若否则返回步骤2顺序进行。本发明能够在跟踪不同的预定轨迹时不需要对控制参数进行反复调试就能够获得较好的控制效果，从而能提高移动机器人的环境适应性和自主优化能力。

主权项

1.一种移动机器人跟踪预定轨迹的启发式动态规划控制方法，其特征是按如下步骤进行：第一步、以移动机器人安装的GPS传感器主天线处为坐标原点，以移动机器人前进方向为x轴，以移动机器人的前进方向逆时针旋转90度方向为y轴，建立机器人坐标系xoy；以大地质心为原点，正东方向为X轴，以正北方向为Y轴，建立大地坐标系XOY；第二步、控制移动机器人运动，并将大地坐标系下移动机器人的位姿记录存储作为预定轨迹；所述位姿信息包括位置坐标和航向角；第三步、确定当前时刻t_i下移动机器人的位姿信息和预瞄点位姿信息，从而建立机器人坐标系下的轨迹跟踪误差状态方程；步骤3.1、定义(X_c(t_i),Y_c(t_i))为移动机器人在大地坐标系XOY中当前时刻t_i下的位置坐标，定义航向角φ_c(t_i)为当前时刻t_i下移动机器人纵轴线与大地坐标系X轴的夹角，定义(X_c(t_i),Y_c(t_i),φ_c(t_i))为移动机器人的当前时刻t_i下的位姿信息；步骤3.2、设定预瞄距离为L，在预定轨迹中搜索大于预瞄距离L且距离移动机器人最近的位姿信息，并将符合要求的位姿信息定义为预瞄点O_p记为(X_p(t_i),Y_p(t_i),φ_p(t_i))；步骤3.3、定义(x_e(t_i),y_e(t_i),φ_e(t_i))为当前时刻t_i下移动机器人在机器人坐标系xoy中的位姿误差，其中x_e(t_i)为纵向偏差，y_e(t_i)为横向偏差，φ_e(t_i)为航向角偏差，则利用式(1)建立轨迹跟踪误差状态方程：步骤3.4、利用式(2)得到当前时刻t_i下移动机器人与预瞄点O_p的横向偏差的变化率和航向角偏差的变化率式(2)中，v_x(t_i)为移动机器人当前时刻t_i下的纵向速度，v_y(t_i)为移动机器人当前时刻t_i下的侧向速度，ω(t_i)为移动机器人当前时刻t_i下的角速度，ρ(t_i)为当前时刻t_i下预瞄点O_p处的道路曲率；第四步、计算当前时刻t_i下预瞄点O_p处位置的道路曲率ρ(t_i)；步骤4.1、从所述预定轨迹中选择三个位姿信息，包括以预瞄点O_p作为第1个位姿信息、以与移动机器人的位置坐标(X_c(t_i),Y_c(t_i))距离相同的两个位姿信息A和B作为第2个和第3个位姿信息，其中位姿信息A与位置坐标(X_c(t_i),Y_c(t_i))的距离为L‑δ；位姿信息B与所位置坐标(X_c(t_i),Y_c(t_i))的距离为L+δ，δ为所设定的间隔距离；步骤4.2、利用式(3)将选取的三个位姿信息进行坐标转换，从而得到当前时刻t₁下在机器人坐标系xoy中任意第j个位姿信息与移动机器人的位置坐标(X_c(t_i),Y_c(t_i))的距离分别在x轴的投影x_d,j(t_i)和y轴上的投影y_d,j(t_i)以及当前时刻t_i下第j个位姿信息在机器人坐标系中的横坐标x_j(t_i)和纵坐标y_j(t_i)：式(3)中，X_j(t_i),Y_j(t_i)为选取的预定轨迹上的经纬度坐标，R_ave为地球平均半径，j＝1,2,3；步骤4.3、使用三次多项式对坐标转换后的三个位姿信息的位置坐标进行曲线拟合，从而得到当前时刻t_i下预瞄点O_p处的道路曲率ρ(t_i)；第五步、建立移动机器人的马尔可夫决策过程模型：步骤5.1、定义移动机器人在当前时刻t_i下的状态向量为s(t_i)＝(y_e(t_i),φ_e(t_i),ω_c(t_i),ρ(t_i))^T，定义移动机器人在当前时刻t_i下的控制动作为角速度u(t_i)；步骤5.2、利用式(4)计算当前时刻t_i下状态向量s(t₁)的回报r(s(t_i))：r(s(t_i))＝s(t_i)^TQs(t_i)+(u(t_i)‑ω_p(t_i))^TR(u(t_i)‑ω_p(t_i))     (4)式(4)中，Q为半正定对角矩阵，R为一正常数，ω_p(t_i)为当前时刻t_i下预瞄点O_p处期望的角速度，且ω_p(t_i)＝v_x(t_i)ρ(t_i)；步骤5.3、利用式(5)定义当前时刻t_i下状态向量s(t_i)的性能指标函数值J(s(t_i))：J(s(t_i))＝r(s(t_i),u(s(t_i)))+γJ(s(t_i+1))           (5)式(5)中，r(s(t_i),u(s(t_i)))为当前时刻t_i下的立即回报值，其中s(t_i)为当前时刻t_i下的状态向量，u(s(t_i))为当前时刻t_i下移动机器人的动作，γ为折扣因子，J(s(t_i+1))为下一时刻t_i+1下的性能指标函数，s(t_i+1)为下一时刻t_i+1下的移动机器人状态向量，并由式(6)得到：式(6)中，T_s为控制周期，且T_s＝t_i+1‑t_i；步骤5.4、利用式(7)建立当前时刻t_i下最优的控制动作u^*(s(t_i))：第六步、利用式(8)计算当前时刻t_i下的控制动作u(s(t_i))并作为HDP算法中执行器的输出：u(s(t_i))＝Φ_a(W_a(t_i)×σ_a(V_a(t_i)×s(t_i)+b_a1(t_i))+b_a2(t_i))       (8)式(8)中，V_a(t_i)为当前时刻t_i下神经网络从输入层到隐含层的第一权值，b_a1(t_i)为当前时刻t_i下神经网络从输入层到隐含层的第一偏置，W_a(t_i)为当前时刻t_i下隐含层到输出层的第一权值，b_a2(t_i)为当前时刻t_i下隐含层到输出层的第一偏置；利用式(9)计算当前时刻t_i下状态向量s(t_i)的性能指标函数值J(s(t_i))并作为HDP算法中评价器的输出：J(s(t_i))＝Φ_c(W_c(t_i)×σ_c(V_c(t_i)×s(t_i)+b_c1(t_i))+b_c2(t_i))       (9)式(9)中，V_c(t_i)为当前时刻t_i下神经网络从输入层到隐含层的第二权值，b_c1(t_i)为当前时刻t_i下神经网络从输入层到隐含层的第二偏置，W_c(t_i)为当前时刻t_i下隐含层到输出层的第二权值，b_c2(t_i)为当前时刻t_i下隐含层到输出层的第二偏置；第七步、建立HDP算法中执行器网络和评价器网络的在线学习规则；步骤7.1、执行器网络在线学习规则：利用式(10)建立执行器误差函数E_a(s(t_i))，并将最小化执行器误差函数E_a(s(t_i))作为执行器网络的学习目标：定义中间变量ζ_d(t_i)＝u(s(t_i))‑u^*(s(t_i))，并利用式(11)‑式(14)更新当前时刻t_i下执行器网络的权值W_a(t_i),V_a(t_i)和偏置b_a1(t_i),b_a2(t_i)，从而得到下一时刻t_i+1下的执行器网络的权值W_a(t_i+1),V_a(t_i+1)和偏置b_a1(t_i+1),b_a2(t_i+1)：式(11)‑式(14)中，α_a为执行器网络的学习率；步骤7.2、评价器网络在线学习规则：定义中间变量为δ_d(s(t_i))＝J(s(t_i))‑r(s(t_i),u(s(t_i)))‑γJ(s(t_i+1))，并利用式(15)建立评价器误差函数E_c(s(t_i))，并以最小化误差函数E_c(s(t_i))作为评价器的学习目标：利用式(16)‑式(19)更新当前时刻t_i下评价器网络的权值W_c(t_i),V_c(t_i)和偏置b_c1(t_i),b_c2(t_i)，从而得到下一时刻t_i+1下的评价器网络的权值W_c(t_i+1),V_c(t_i+1)和偏置b_c1(t_i+1),b_c2(t_i+1)：式(16)‑式(19)中，α_c为评价器网络的学习率；第八步、由执行器网络得到的控制动作u(s(t_i))，以及当前移动机器人的前进速度，若所述移动机器人为履带式机器人，则利用式(20)得到移动机器人两侧电机的转速n_L(t_i),n_R(t_i)；若所述移动机器人为轮式机器人，则利用式(21)得到移动机器人前轮转向角：式(20)中，r_d为移动机器人的驱动轮半径，B为履带中心距，v_c为移动机器人前进速度；式(21)中，δ(t_i)为前轮转向角，L_z为移动机器人轴距；第九步、判断当前时刻t_i下预瞄点O_p是否为预定轨迹中的最后一位置坐标，若是，则令角速度u(s(t_i))＝0，控制移动机器人减速并在达到预瞄点O_p时停止运动；否则，将i+1赋值给i，并返回第三步顺序执行。