CN110244735B - 移动机器人跟踪预定轨迹的启发式动态规划控制方法 - Google Patents

移动机器人跟踪预定轨迹的启发式动态规划控制方法 Download PDF

Info

Publication number
CN110244735B
CN110244735B CN201910551319.6A CN201910551319A CN110244735B CN 110244735 B CN110244735 B CN 110244735B CN 201910551319 A CN201910551319 A CN 201910551319A CN 110244735 B CN110244735 B CN 110244735B
Authority
CN
China
Prior art keywords
mobile robot
current time
formula
robot
pose information
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910551319.6A
Other languages
English (en)
Other versions
CN110244735A (zh
Inventor
宋彦
张羊阳
姚琦
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Anhui Agricultural University AHAU
Original Assignee
Anhui Agricultural University AHAU
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Anhui Agricultural University AHAU filed Critical Anhui Agricultural University AHAU
Priority to CN201910551319.6A priority Critical patent/CN110244735B/zh
Publication of CN110244735A publication Critical patent/CN110244735A/zh
Application granted granted Critical
Publication of CN110244735B publication Critical patent/CN110244735B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G05D1/0278—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using satellite positioning signals, e.g. GPS

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Feedback Control In General (AREA)
  • Manipulator (AREA)

Abstract

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

Description

移动机器人跟踪预定轨迹的启发式动态规划控制方法
技术领域
本发明属于移动机器人技术领域，具体涉及一种移动机器人跟踪预定轨迹的控制方法。
背景技术
随着移动机器人的广泛应用，复杂的地形和多变的环境对移动机器人的运动控制能力提出了更高的要求，如何提高移动机器对于不同线形道路的路径跟踪性能成为了研究的热点。
移动机器人对于预定轨迹的跟踪多采用“预瞄—跟随”原理，传统的控制方法有纯追踪和PID控制，但是上述方法均忽略了系统的数学模型，难以取得高精度的控制效果。一些学者提出了基于车辆动力学模型或运动学模型的控制方法，上述方法在模型较为精确的条件下，提高了控制精度，但是需要调节和优化的控制参数较多，尤其是当移动机器人的工作环境发生变化时，需要重新选择控制参数，优化控制性能。
为了提高移动机器人的环境适应性和自主行为能力，一些学者提出了基于智能控制的方法，如模糊控制和神经网络控制等。但是模糊控制仍然需要将专家经验抽象为模糊控制规则，并调校模糊控制参数。神经网络控制方法需要大量的试验数据训练网络，并且这些数据影响最终的控制效果。
发明内容
本发明为现有技术存在的不足之处，提出一种移动机器人跟踪预定轨迹的启发式动态规划控制方法，以期能够在跟踪不同的预定轨迹时不需要对控制参数进行反复调试就能够获得较好的控制效果，从而能提高移动机器人的环境适应性和自主优化能力。
本发明为解决技术问题，采用如下技术方案：
本发明一种移动机器人跟踪预定轨迹的启发式动态规划控制方法的特点是按如下步骤进行：
第一步、以移动机器人安装的GPS传感器主天线处为坐标原点，以移动机器人前进方向为x轴，以移动机器人的前进方向逆时针旋转90度方向为y轴，建立机器人坐标系xoy；以大地质心为原点，正东方向为X轴，以正北方向为Y轴，建立大地坐标系XOY；
第二步、控制移动机器人运动，并将大地坐标系下移动机器人的位姿信息记录存储作为预定轨迹；所述位姿信息包括位置坐标和航向角；
第三步、确定当前时刻ti下移动机器人的位姿信息和预瞄点位姿信息，从而建立机器人坐标系下的轨迹跟踪误差状态方程；
步骤3.1、定义(Xc(ti),Yc(ti))为移动机器人在大地坐标系XOY中当前时刻ti下的位置坐标，定义航向角φc(ti)为当前时刻ti下移动机器人纵轴线与大地坐标系X轴的夹角，定义(Xc(ti),Yc(ti),φc(ti))为移动机器人的当前时刻ti下的位姿信息；
步骤3.2、设定预瞄距离为L，在预定轨迹中搜索大于预瞄距离L且距离移动机器人最近的位姿信息，并将符合要求的位姿信息定义为预瞄点Op记为(Xp(ti),Yp(ti),φp(ti))；
步骤3.3、定义(xe(ti),ye(ti),φe(ti))为当前时刻ti下移动机器人在机器人坐标系xoy中的位姿误差，其中xe(ti)为纵向偏差，ye(ti)为横向偏差，φe(ti)为航向角偏差，则利用式(1)建立轨迹跟踪误差状态方程：
Figure GDA0002524746400000021
步骤3.4、利用式(2)得到当前时刻ti下移动机器人与预瞄点Op的横向偏差的变化率ye(ti)和航向角偏差的变化率
Figure GDA0002524746400000022
Figure GDA0002524746400000023
式(2)中，vx(ti)为移动机器人当前时刻ti下的纵向速度，vy(ti)为移动机器人当前时刻ti下的侧向速度，ω(ti)为移动机器人当前时刻ti下的角速度，ρ(ti)为当前时刻ti下预瞄点Op处的道路曲率；
第四步、计算当前时刻ti下预瞄点Op处位置的道路曲率ρ(ti)；
步骤4.1、从所述预定轨迹中选择三个位姿信息，包括以预瞄点Op作为第1个位姿信息、以与移动机器人的位置坐标(Xc(ti),Yc(ti))距离相同的两个位姿信息A和B作为第2个和第3个位姿信息，其中位姿信息A与位置坐标(Xc(ti),Yc(ti))的距离为L-δ；位姿信息B与位置坐标(Xc(ti),Yc(ti))的距离为L+δ，δ为所设定的间隔距离；
步骤4.2、利用式(3)将选取的三个位姿信息进行坐标转换，从而得到当前时刻t1下在机器人坐标系xoy中任意第j个位姿信息与移动机器人的位置坐标(Xc(ti),Yc(ti))的距离分别在x轴的投影xd,j(ti)和y轴上的投影yd,j(ti)以及当前时刻ti下第j个位姿信息在机器人坐标系中的横坐标xj(ti)和纵坐标yj(ti)：
Figure GDA0002524746400000031
式(3)中，Xj(ti),Yj(ti)为选取的预定轨迹上的经纬度坐标，Rave为地球平均半径，j＝1,2,3；
步骤4.3、使用三次多项式对坐标转换后的三个位姿信息的位置坐标进行曲线拟合，从而得到当前时刻ti下预瞄点Op处的道路曲率ρ(ti)；
第五步、建立移动机器人的马尔可夫决策过程模型：
步骤5.1、定义移动机器人在当前时刻ti下的状态向量为
Figure GDA0002524746400000032
定义移动机器人在当前时刻ti下的控制动作为角速度u(ti)；
步骤5.2、利用式(4)计算当前时刻ti下状态向量s(ti)的回报r(s(ti))：
r(s(ti))＝s(ti)TQs(ti)+(u(ti)-ωp(ti))TR(u(ti)-ωp(ti)) (4)
式(4)中，Q为半正定对角矩阵，R为一正常数，ωp(ti)为当前时刻ti下预瞄点Op处期望的角速度，且ωp(ti)＝vx(ti)ρ(ti)；
步骤5.3、利用式(5)定义当前时刻ti下状态向量s(ti)的性能指标函数值J(s(ti))：
J(s(ti))＝r(s(ti),u(s(ti)))+γJ(s(ti+1)) (5)
式(5)中，r(s(ti),u(s(ti)))为当前时刻ti下的立即回报值，其中s(ti)为当前时刻ti下的状态向量，u(s(ti))为当前时刻ti下移动机器人的动作，γ为折扣因子，J(s(ti+1))为下一时刻ti+1下的性能指标函数，s(ti+1)为下一时刻ti+1下的移动机器人状态向量，并由式(6)得到：
Figure GDA0002524746400000041
式(6)中，Ts为控制周期，且Ts＝ti+1-ti；
步骤5.4、利用式(7)建立当前时刻ti下最优的控制动作u*(s(ti))：
Figure GDA0002524746400000042
第六步、利用式(8)计算当前时刻ti下的控制动作u(s(ti))并作为HDP算法中执行器的输出：
u(s(ti))＝Φa(Wa(ti)×σa(Va(ti)×s(ti)+ba1(ti))+ba2(ti)) (8)
式(8)中，Va(ti)为当前时刻ti下神经网络从输入层到隐含层的第一权值，ba1(ti)为当前时刻ti下神经网络从输入层到隐含层的第一偏置，Wa(ti)为当前时刻ti下隐含层到输出层的第一权值，ba2(ti)为当前时刻ti下隐含层到输出层的第一偏置；
利用式(9)计算当前时刻ti下状态向量s(ti)的性能指标函数值J(s(ti))并作为HDP算法中评价器的输出：
J(s(ti))＝Φc(Wc(ti)×σc(Vc(ti)×s(ti)+bc1(ti))+bc2(ti))(9)
式(9)中，Vc(ti)为当前时刻ti下神经网络从输入层到隐含层的第二权值，bc1(ti)为当前时刻ti下神经网络从输入层到隐含层的第二偏置，Wc(ti)为当前时刻ti下隐含层到输出层的第二权值，bc2(ti)为当前时刻ti下隐含层到输出层的第二偏置；
第七步、建立HDP算法中执行器网络和评价器网络的在线学习规则；
步骤7.1、执行器网络在线学习规则：
利用式(10)建立执行器误差函数Ea(s(ti))，并将最小化执行器误差函数Ea(s(ti))作为执行器网络的学习目标：
Figure GDA0002524746400000043
定义中间变量ζd(ti)＝u(s(ti))-u*(s(ti))，并利用式(11)-式(14)更新当前时刻ti下执行器网络的权值Wa(ti),Va(ti)和偏置ba1(ti),ba2(ti)，从而得到下一时刻ti+1下的执行器网络的权值Wa(ti+1),Va(ti+1)和偏置ba1(ti+1),ba2(ti+1)：
Figure GDA0002524746400000051
Figure GDA0002524746400000052
Figure GDA0002524746400000053
Figure GDA0002524746400000054
式(11)-式(14)中，αa为执行器网络的学习率；
步骤7.2、评价器网络在线学习规则：
定义中间变量为δd(s(ti))＝J(s(ti))-r(s(ti),u(s(ti)))-γJ(s(ti+1))，并利用式(15)建立评价器误差函数Ec(s(ti))，并以最小化误差函数Ec(s(ti))作为评价器的学习目标：
Figure GDA0002524746400000059
利用式(16)-式(19)更新当前时刻ti下评价器网络的权值Wc(ti),Vc(ti)和偏置bc1(ti),bc2(ti)，从而得到下一时刻ti+1下的评价器网络的权值Wc(ti+1),Vc(ti+1)和偏置bc1(ti+1),bc2(ti+1)：
Figure GDA0002524746400000055
Figure GDA0002524746400000056
Figure GDA0002524746400000057
Figure GDA0002524746400000058
式(16)-式(19)中，αc为评价器网络的学习率；
第八步、由执行器网络得到的控制动作u(s(ti))，以及当前移动机器人的前进速度，若所述移动机器人为履带式机器人，则利用式(20)得到移动机器人两侧电机的转速nL(ti),nR(ti)；若所述移动机器人为轮式机器人，则利用式(21)得到移动机器人前轮转向角：
Figure GDA0002524746400000061
式(20)中，rd为移动机器人的驱动轮半径，B为履带中心距，vc为移动机器人前进速度；
Figure GDA0002524746400000062
式(21)中，δ(ti)为前轮转向角，Lz为移动机器人轴距；
第九步、判断当前时刻ti下预瞄点Op是否为预定轨迹中的最后一位置坐标，若是，则令角速度u(s(ti))＝0，控制移动机器人减速并在达到预瞄点Op时停止运动；否则，将i+1赋值给i，并返回第三步顺序执行。
与现有技术相比，本发明的有益效果在于：
1、本发明首先建立了移动机器人跟踪预定轨迹控制问题的马尔科夫决策过程(MDP)模型，选取了移动机器人的横向偏差ye、航向角偏差φe、当前角速度ωc和预瞄点处的道路曲率ρ作为状态向量，综合误差性能指标和跟随稳定性指标设计了回报函数，然后提出了一种基于HDP算法的跟踪预定轨迹的控制方法，采用了三层前馈神经网络设计评价器和执行器，并推导了网络参数的在线学习规则，最后通过系统试验验证了本发明跟踪预定轨迹的性能，解决了传统的移动机器人跟踪预定轨迹的控制方法需要人工调校参数，缺乏自主优化能力的问题，实现了不需要对控制参数进行反复调试就能够获得较好的控制效果，提高了移动机器人路径跟随控制方法的环境适应性和自主优化能力；
2、采用本发明移动机器人跟踪预定轨迹的控制方法，能够利用目标点相对机器人的位置，利用HDP算法确定移动机器人的角速度使得机器人跟踪预定轨迹，功能通用性强，且具有对道路环境自适应性；
3、移动机器人对预定轨迹的跟踪能力，是评价移动机器人性能的重要指标之一，也是移动机器人能够安全、有效的完成预定任务的重要保障。采用本发明移动机器人跟踪预定轨迹的控制方法，综合考虑了误差性能指标和跟随稳定性指标，能够在实现移动机器人对预期轨迹的跟踪的同时也保证了移动机器人的安全性和稳定性；
4、由于采用本发明方法在控制过程中仅需要目标点相对机器人的坐标，而并不局限于某一种固定的传感器和定位手段，获取目标点相对机器人的坐标，既可以使用GPS系统，也可以采用雷达、光学等传感器，因此本发明方法具有较好的普适性、通用性，能够适用于安装不同传感器的移动机器人；
5、本发明基于启发式动态规划算法框架，通过对当前控制动作的评价性反馈信号优化控制动作，在跟随路径线形变化时，该方法不需要对控制参数进行调试就能够获得高精度的控制效果，具有较好的环境适应性和自主优化能力；
6、由于本发明采用经典的三层前馈神经网络构建HDP算法中的执行器网络和评价器网络移动机器人跟踪预定轨迹的智能控制方法中的控制算法采用经典的三层前馈神经网络构建HDP算法框架，实现简单，鲁棒性和可靠性较高。
附图说明
图1为本发明移动机器人跟踪预定轨迹的控制系统结构图；
图2为本发明移动机器人跟踪预定轨迹的示意图；
图3为本发明基于HDP算法的跟踪预定轨迹的控制方法结构图；
图4为采用本发明移动机器人跟踪预定轨迹的智能控制方法对预定轨迹为直线的效果图；
图5为采用本发明移动机器人跟踪预定轨迹的智能控制方法对预定轨迹为钝角转向曲线的效果图；
图6为采用本发明移动机器人跟踪预定轨迹的智能控制方法对预定轨迹为锐角转向曲线的效果图。
具体实施方式
本实施例以实验室的履带式机器人平台为例，具体说明如何采用本发明方法实现对预定轨迹的跟踪及其功能效果。
本实施例中所采用的履带式机器人平台上，其长度为0.8m，宽度为0.57m，重量为44kg，履带中心距B为0.485m，主动轮半径r为0.2m。采用48V20Ah的锂电池供电，两侧的电机功率为400w。装有北斗星通公司提供的产品名称为BDM680的GPS传感器，能够获取机器人所在位置的经纬度、机器人速度信号，可以通过双天线定位提供移动平台的航向角信息，在使用差分定位的情况下，定位精度可以达到0.02m。该机器人的计算平台采用控汇智能公司提供的产品名称为i5-4500U的工控机和正点原子型号为STM32ZET6开发板组成，工控机实现计算控制动作；单片机通过机器人数学模型将动作分解为两侧电机的转速，并给两侧的电机发送指令。
本实施例中，采用如图1所示的移动机器人跟踪预定轨迹的控制系统结构，一种移动机器人跟踪预定轨迹的启发式动态规划控制方法，采用等采样周期的计算机控制方式来实现对预定轨迹的跟踪，每个控制周期是按如下步骤进行：
第一步、建立机器人坐标系：以机器人安装的GPS传感器主天线处为坐标原点，以机器人前进方向为x轴，以机器人的前进方向逆时针旋转90度方向为y轴，建立机器人坐标系xoy；运动过程中的坐标系如图2中所示，以大地质心为原点，以正东方向为X轴，以正北方向为Y轴，建立大地坐标系XOY；
第二步、遥控控制移动机器人运动，将大地坐标系下移动机器人的位姿信息记录存储作为预定轨迹，位姿信息包括位置坐标和航向角；预定轨迹是由n个目标点组成的序列，目标点序列采用一n×3维的数组描述，每一行的三个元素分别代表轨迹中一点的经纬度和航向角信息。
第三步、确定当前时刻ti下移动机器人的位姿信息和预瞄点位姿信息，从而建立机器人坐标系下的轨迹跟踪误差状态方程；
步骤3.1、如图2中所示，定义(Xc(ti),Yc(ti))为移动机器人在大地坐标系XOY中当前时刻ti下的位置坐标，定义航向角φc(ti)为当前时刻ti下移动机器人纵轴线与大地坐标系X轴的夹角，定义(Xc(ti),Yc(ti),φc(ti))为移动机器人的当前时刻ti下的位姿信息；
步骤3.2、设定预瞄距离为L，在预定轨迹中搜索大于预瞄距离L且距离移动机器人最近的位姿信息，并将符合要求的位姿信息定义为预瞄点Op记为(Xp(ti),Yp(ti),φp(ti))；
步骤3.3、定义(xe(ti),ye(ti),φe(ti))为当前时刻ti下移动机器人在机器人坐标系xoy中的位姿误差，其中xe(ti)为纵向偏差，ye(ti)为横向偏差，φe(ti)为航向角偏差，则利用式(1)建立轨迹跟踪误差状态方程：
Figure GDA0002524746400000081
步骤3.4、利用式(2)得到当前时刻ti下移动机器人与预瞄点Op的横向偏差的变化率
Figure GDA0002524746400000082
和航向角偏差的变化率
Figure GDA0002524746400000083
Figure GDA0002524746400000091
式(2)中，vx(ti)为移动机器人当前时刻ti下的纵向速度，vy(ti)为移动机器人当前时刻ti下的侧向速度，ω(ti)为移动机器人当前时刻ti下的角速度，ρ(ti)为当前时刻ti下预瞄点Op处的道路曲率；
第四步、计算当前时刻ti下预瞄点Op处位置的道路曲率ρ(ti)；
步骤4.1、从预定轨迹中选择三个位姿信息，包括以预瞄点Op作为第1个位姿信息、以与移动机器人的位置坐标(Xc(ti),Yc(ti))距离相同的两个位姿信息A和B作为第2个和第3个位姿信息，其中位姿信息A与位置坐标(Xc(ti),Yc(ti))的距离为L-δ；位姿信息B与位置坐标(Xc(ti),Yc(ti))的距离为L+δ，δ为所设定的间隔距离；
步骤4.2、利用式(3)将选取的三个位姿信息进行坐标转换，从而得到当前时刻t1下在机器人坐标系xoy中任意第j个位姿信息与移动机器人的位置坐标(Xc(ti),Yc(ti))的距离分别在x轴的投影xd,j(ti)和y轴上的投影yd,j(ti)以及当前时刻ti下第j个位姿信息在机器人坐标系中的横坐标xj(ti)和纵坐标yj(ti)：
Figure GDA0002524746400000092
式(3)中，Xj(ti),Yj(ti)为选取的预定轨迹上的经纬度坐标，Rave为地球平均半径，j＝1,2,3；
步骤4.3、使用三次多项式对坐标转换后的三个位姿信息的位置坐标进行曲线拟合，得到如式(4)所示的曲线方程：
y＝a3x3+a2x2+a1x (4)
将转换后的轨迹点分别代入上式计算出a3,a2,a1，则预瞄点Op位置的道路曲率ρ(ti)为：
Figure GDA0002524746400000093
第五步、建立移动机器人的马尔可夫决策过程模型：
步骤5.1、针对本实施例履带机器人跟踪预定轨迹的问题，定义履带机器人在当前时刻ti下的状态向量为
Figure GDA0002524746400000103
定义履带机器人在当前时刻ti下的控制动作为角速度u(ti)；
步骤5.2、跟踪预定轨迹的过程中，不但期望减小横向偏差和航向角偏差，也期望尽量减少控制动作波动，因此利用式(5)计算当前时刻ti下状态向量s(ti)的回报r(s(ti))：
r(s(ti))＝s(ti)TQs(ti)+(u(ti)-ωp(ti))TR(u(ti)-ωp(ti)) (5)
式(5)中，Q为半正定对角矩阵，本实施例中Q＝diag{2,1,0,0}，表征了对横向偏差和航向角偏差指标的权重；R为一正常数，本实施例中R＝5，表征了对当前控制动作补偿的权重；ωp(ti)为当前时刻ti下预瞄点Op处期望的角速度，且ωp(ti)＝vx(ti)ρ(ti)；
步骤5.3、利用式(6)定义当前时刻ti下状态向量s(ti)的性能指标函数J(s(ti))：
J(s(ti))＝r(s(ti),u(s(ti)))+γJ(s(ti+1)) (6)
式(6)中，r(s(ti),u(s(ti)))为当前时刻ti下的立即回报值，其中s(ti)为当前时刻ti下的状态向量，u(s(ti))为当前时刻ti下移动机器人的动作，γ为折扣因子，本实施例中γ＝0.9，J(s(ti+1))为下一时刻ti+1下的性能指标函数，s(ti+1)为下一时刻ti+1下的移动机器人状态向量，并由式(7)得到：
Figure GDA0002524746400000101
式(7)中，Ts为控制周期，且Ts＝ti+1-ti；
步骤5.4、利用式(8)建立当前时刻ti下最优的控制动作u*(s(ti))：
Figure GDA0002524746400000102
第六步、采用HDP算法来求取上式(8)中的最优控制动作，构建如图3所示的控制框架。利用式(9)计算当前时刻ti下的控制动作u(s(ti))并作为HDP算法中执行器的输出：
u(s(ti))＝Φa(Wa(ti)×σa(Va(ti)×s(ti)+ba1(ti))+ba2(ti)) (9)
式(9)中，Va(ti)为当前时刻ti下神经网络从输入层到隐含层的第一权值，ba1(ti)为当前时刻ti下神经网络从输入层到隐含层的第一偏置，Wa(ti)为当前时刻ti下隐含层到输出层的第一权值，ba2(ti)为当前时刻ti下隐含层到输出层的第一偏置；
图(3)中评价器1和评价器2的结构及参数均相同，但是分别用于估计当前状态的性能指标函数值以及预测状态的性能指标函数值。
利用式(10)计算当前时刻ti下状态向量s(ti)的性能指标函数J(s(ti))并作为HDP算法中评价器的输出：
J(s(ti))＝Φc(Wc(ti)×σc(Vc(ti)×s(ti)+bc1(ti))+bc2(ti)) (10)
式(10)中，Vc(ti)为当前时刻ti下神经网络从输入层到隐含层的第二权值，bc1(ti)为当前时刻ti下神经网络从输入层到隐含层的第二偏置，Wc(ti)为当前时刻ti下隐含层到输出层的第二权值，bc2(ti)为当前时刻ti下隐含层到输出层的第二偏置；
第七步、建立HDP算法中执行器网络和评价器网络的在线学习规则，实质上是各网络权值的在线调整过程，这一过程是通过迭代运算逐步实现的。其中执行器网络和评价器网络的初始权值既可以使用其他的路径跟随控制方法在实际跟踪预定轨迹时使用本步骤推导的学习规则得到，也可以使用其他路径跟随控制方法的离线数据通过本步骤推导的学习规则得到。
步骤7.1、执行器网络在线学习规则：
执行器网络优化的目标是找到最优控制动作u*(s(ti))使得目标函数值J(s(ti))最小，利用式(11)建立执行器误差函数Ea(s(ti))，并将最小化执行器误差函数Ea(s(ti))作为执行器网络的学习目标：
Figure GDA0002524746400000111
定义中间变量ζd(ti)＝u(s(ti))-u*(s(ti))，并利用式(12)-式(15)更新当前时刻ti下执行器网络的权值Wa(ti),Va(ti)和偏置ba1(ti),ba2(ti)，从而得到下一时刻ti+1下的执行器网络的权值Wa(ti+1),Va(ti+1)和偏置ba1(ti+1),ba2(ti+1)：
Figure GDA0002524746400000121
Figure GDA0002524746400000122
Figure GDA0002524746400000123
Figure GDA0002524746400000124
式(12)-式(15)中，αa为执行器网络的学习率；
步骤7.2、评价器网络在线学习规则：
评价器网络的优化目标是对最优性能指标函数值J*(s(ti))的近似，定义中间变量为δd(s(ti))＝J(s(ti))-r(s(ti),u(s(ti)))-γJ(s(ti+1))，并利用式(16)建立评价器误差函数Ec(s(ti))，并以最小化误差函数Ec(s(ti))作为评价器的学习目标：
Figure GDA0002524746400000125
利用式(17)-式(20)更新当前时刻ti下评价器网络的权值Wc(ti),Vc(ti)和偏置bc1(ti),bc2(ti)，从而得到下一时刻ti+1下的评价器网络的权值Wc(ti+1),Vc(ti+1)和偏置bc1(ti+1),bc2(ti+1)：
Figure GDA0002524746400000126
Figure GDA0002524746400000127
Figure GDA0002524746400000128
Figure GDA0002524746400000129
式(17)-式(20)中，αc为评价器网络的学习率；
第八步、由执行器网络得到的控制动作u(s(ti))，以及当前移动机器人的前进速度，若移动机器人为履带式机器人，则采用差速转向的方式，从而利用式(20)得到移动机器人两侧电机的转速nL(ti),nR(ti)；若移动机器人为轮式机器人，则采用前轮转向的方式，从而利用式(21)得到移动机器人前轮转向角：
Figure GDA0002524746400000131
式(20)中，rd为移动机器人的驱动轮半径，B为履带中心距，vc为移动机器人前进速度；
Figure GDA0002524746400000132
式(21)中，δ(ti)为前轮转向角，Lz为移动机器人轴距；
本实施例中使用的是履带机器人，其前进速度为vc＝0.3m/s；则利用式(21)得到移动机器人两侧电机的转速
Figure GDA0002524746400000133
第九步、判断当前时刻ti下预瞄点Op是否为预定轨迹中的最后一位置坐标，若是，则令角速度u(s(ti))＝0，控制移动机器人减速并在达到预瞄点Op时停止运动；否则，将i+1赋值给i，并返回第三步顺序执行。
从图4、图5和图6可以看出实际行驶轨迹虚点曲线1和预定轨迹实曲线2几乎重合，说明采用本发明方法有较高的跟踪精度，并且该方法仅通过历史跟随数据对评价器和执行器网络进行了离线训练，变换跟随路径时控制参数未做人工调试，仅通过HDP算法的迭代优化机制对控制网络进行在线优化，说明该方法具有较好的自主优化能力。

Claims (1)

1.一种移动机器人跟踪预定轨迹的启发式动态规划控制方法，其特征是按如下步骤进行：
第一步、以移动机器人安装的GPS传感器主天线处为坐标原点，以移动机器人前进方向为x轴，以移动机器人的前进方向逆时针旋转90度方向为y轴，建立机器人坐标系xoy；以大地质心为原点，正东方向为X轴，以正北方向为Y轴，建立大地坐标系XOY；
第二步、控制移动机器人运动，并将大地坐标系下移动机器人的位姿信息记录存储作为预定轨迹；所述位姿信息包括位置坐标和航向角；
第三步、确定当前时刻ti下移动机器人的位姿信息和预瞄点位姿信息，从而建立机器人坐标系下的轨迹跟踪误差状态方程；
步骤3.1、定义(Xc(ti),Yc(ti))为移动机器人在大地坐标系XOY中当前时刻ti下的位置坐标，定义航向角φc(ti)为当前时刻ti下移动机器人纵轴线与大地坐标系X轴的夹角，定义(Xc(ti),Yc(ti),φc(ti))为移动机器人的当前时刻ti下的位姿信息；
步骤3.2、设定预瞄距离为L，在预定轨迹中搜索大于预瞄距离L且距离移动机器人最近的位姿信息，并将符合要求的位姿信息定义为预瞄点Op记为(Xp(ti),Yp(ti),φp(ti))；
步骤3.3、定义(xe(ti),ye(ti),φe(ti))为当前时刻ti下移动机器人在机器人坐标系xoy中的位姿误差，其中xe(ti)为纵向偏差，ye(ti)为横向偏差，φe(ti)为航向角偏差，则利用式(1)建立轨迹跟踪误差状态方程：
Figure FDA0002524746390000011
步骤3.4、利用式(2)得到当前时刻ti下移动机器人与预瞄点Op的横向偏差的变化率
Figure FDA0002524746390000012
和航向角偏差的变化率
Figure FDA0002524746390000013
Figure FDA0002524746390000014
式(2)中，vx(ti)为移动机器人当前时刻ti下的纵向速度，vy(ti)为移动机器人当前时刻ti下的侧向速度，ω(ti)为移动机器人当前时刻ti下的角速度，ρ(ti)为当前时刻ti下预瞄点Op处的道路曲率；
第四步、计算当前时刻ti下预瞄点Op处位置的道路曲率ρ(ti)；
步骤4.1、从所述预定轨迹中选择三个位姿信息，包括以预瞄点Op作为第1个位姿信息、以与移动机器人的位置坐标(Xc(ti),Yc(ti))距离相同的两个位姿信息A和B作为第2个和第3个位姿信息，其中位姿信息A与位置坐标(Xc(ti),Yc(ti))的距离为L-δ；位姿信息B与位置坐标(Xc(ti),Yc(ti))的距离为L+δ，δ为所设定的间隔距离；
步骤4.2、利用式(3)将选取的三个位姿信息进行坐标转换，从而得到当前时刻t1下在机器人坐标系xoy中任意第j个位姿信息与移动机器人的位置坐标(Xc(ti),Yc(ti))的距离分别在x轴的投影xd,j(ti)和y轴上的投影yd,j(ti)以及当前时刻ti下第j个位姿信息在机器人坐标系中的横坐标xj(ti)和纵坐标yj(ti)：
Figure FDA0002524746390000021
式(3)中，Xj(ti),Yj(ti)为选取的预定轨迹上的经纬度坐标，Rave为地球平均半径，j＝1,2,3；
步骤4.3、使用三次多项式对坐标转换后的三个位姿信息的位置坐标进行曲线拟合，从而得到当前时刻ti下预瞄点Op处的道路曲率ρ(ti)；
第五步、建立移动机器人的马尔可夫决策过程模型：
步骤5.1、定义移动机器人在当前时刻ti下的状态向量为s(ti)＝(ye(ti),
Figure FDA0002524746390000022
ω(ti),ρ(ti))T，定义移动机器人在当前时刻ti下的控制动作为角速度u(ti)；
步骤5.2、利用式(4)计算当前时刻ti下状态向量s(ti)的回报r(s(ti))：
r(s(ti))＝s(ti)TQs(ti)+(u(ti)-ωp(ti))TR(u(ti)-ωp(ti)) (4)
式(4)中，Q为半正定对角矩阵，R为一正常数，ωp(ti)为当前时刻ti下预瞄点Op处期望的角速度，且ωp(ti)＝vx(ti)ρ(ti)；
步骤5.3、利用式(5)定义当前时刻ti下状态向量s(ti)的性能指标函数值J(s(ti))：
J(s(ti))＝r(s(ti),u(s(ti)))+γJ(s(ti+1)) (5)
式(5)中，r(s(ti),u(s(ti)))为当前时刻ti下的立即回报值，其中s(ti)为当前时刻ti下的状态向量，u(s(ti))为当前时刻ti下移动机器人的动作，γ为折扣因子，J(s(ti+1))为下一时刻ti+1下的性能指标函数，s(ti+1)为下一时刻ti+1下的移动机器人状态向量，并由式(6)得到：
Figure FDA0002524746390000031
式(6)中，Ts为控制周期，且Ts＝ti+1-ti；
步骤5.4、利用式(7)建立当前时刻ti下最优的控制动作u*(s(ti))：
Figure FDA0002524746390000032
第六步、利用式(8)计算当前时刻ti下的控制动作u(s(ti))并作为HDP算法中执行器的输出：
u(s(ti))＝Φa(Wa(ti)×σa(Va(ti)×s(ti)+ba1(ti))+ba2(ti)) (8)
式(8)中，Va(ti)为当前时刻ti下神经网络从输入层到隐含层的第一权值，ba1(ti)为当前时刻ti下神经网络从输入层到隐含层的第一偏置，Wa(ti)为当前时刻ti下隐含层到输出层的第一权值，ba2(ti)为当前时刻ti下隐含层到输出层的第一偏置；Φa(·)表示用于逼近执行器的神经网络输出层激活函数；σa(·)表示用于逼近执行器的神经网络隐含层激活函数；
利用式(9)计算当前时刻ti下状态向量s(ti)的性能指标函数值J(s(ti))并作为HDP算法中评价器的输出：
J(s(ti))＝Φc(Wc(ti)×σc(Vc(ti)×s(ti)+bc1(ti))+bc2(ti)) (9)
式(9)中，Vc(ti)为当前时刻ti下神经网络从输入层到隐含层的第二权值，bc1(ti)为当前时刻ti下神经网络从输入层到隐含层的第二偏置，Wc(ti)为当前时刻ti下隐含层到输出层的第二权值，bc2(ti)为当前时刻ti下隐含层到输出层的第二偏置；Φc(·)表示用于逼近评价器的神经网络输出层激活函数；σc(·)表示用于逼近评价器的神经网络隐含层激活函数；
第七步、建立HDP算法中执行器网络和评价器网络的在线学习规则；
步骤7.1、执行器网络在线学习规则：
利用式(10)建立执行器误差函数Ea(s(ti))，并将最小化执行器误差函数Ea(s(ti))作为执行器网络的学习目标：
Figure FDA0002524746390000041
定义中间变量ζd(ti)＝u(s(ti))-u*(s(ti))，并利用式(11)-式(14)更新当前时刻ti下执行器网络的权值Wa(ti),Va(ti)和偏置ba1(ti),ba2(ti)，从而得到下一时刻ti+1下的执行器网络的权值Wa(ti+1),Va(ti+1)和偏置ba1(ti+1),ba2(ti+1)：
Figure FDA0002524746390000042
Figure FDA0002524746390000043
Figure FDA0002524746390000044
Figure FDA0002524746390000045
式(11)-式(14)中，αa为执行器网络的学习率；
步骤7.2、评价器网络在线学习规则：
定义中间变量为δd(s(ti))＝J(s(ti))-r(s(ti),u(s(ti)))-γJ(s(ti+1))，并利用式(15)建立评价器误差函数Ec(s(ti))，并以最小化误差函数Ec(s(ti))作为评价器的学习目标：
Figure FDA0002524746390000046
利用式(16)-式(19)更新当前时刻ti下评价器网络的权值Wc(ti),Vc(ti)和偏置bc1(ti),bc2(ti)，从而得到下一时刻ti+1下的评价器网络的权值Wc(ti+1),Vc(ti+1)和偏置bc1(ti+1),bc2(ti+1)：
Figure FDA0002524746390000047
Figure FDA0002524746390000048
Figure FDA0002524746390000051
Figure FDA0002524746390000052
式(16)-式(19)中，αc为评价器网络的学习率；
第八步、由执行器网络得到的控制动作u(s(ti))，以及当前移动机器人的前进速度，若所述移动机器人为履带式机器人，则利用式(20)得到移动机器人两侧电机的转速nL(ti),nR(ti)；若所述移动机器人为轮式机器人，则利用式(21)得到移动机器人前轮转向角：
Figure FDA0002524746390000053
式(20)中，rd为移动机器人的驱动轮半径，B为履带中心距，vc为移动机器人前进速度；
Figure FDA0002524746390000054
式(21)中，δ(ti)为前轮转向角，Lz为移动机器人轴距；
第九步、判断当前时刻ti下预瞄点Op是否为预定轨迹中的最后一位置坐标，若是，则令角速度u(s(ti))＝0，控制移动机器人减速并在达到预瞄点Op时停止运动；否则，将i+1赋值给i，并返回第三步顺序执行。
CN201910551319.6A 2019-06-24 2019-06-24 移动机器人跟踪预定轨迹的启发式动态规划控制方法 Active CN110244735B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910551319.6A CN110244735B (zh) 2019-06-24 2019-06-24 移动机器人跟踪预定轨迹的启发式动态规划控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910551319.6A CN110244735B (zh) 2019-06-24 2019-06-24 移动机器人跟踪预定轨迹的启发式动态规划控制方法

Publications (2)

Publication Number Publication Date
CN110244735A CN110244735A (zh) 2019-09-17
CN110244735B true CN110244735B (zh) 2020-08-21

Family

ID=67889155

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910551319.6A Active CN110244735B (zh) 2019-06-24 2019-06-24 移动机器人跟踪预定轨迹的启发式动态规划控制方法

Country Status (1)

Country Link
CN (1) CN110244735B (zh)

Families Citing this family (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110426959B (zh) * 2019-08-09 2022-08-26 太原科技大学 一种履带机器人控制系统
CN110471428B (zh) * 2019-09-18 2021-05-07 吉林大学 一种基于模型的变预瞄距离和速度约束的路径跟踪方法
CN112633543A (zh) * 2019-10-08 2021-04-09 北京京东乾石科技有限公司 无人配送车在非结构化场景中的泊车路径规划方法及装置
CN110789530B (zh) * 2019-11-19 2021-04-09 中国科学院深圳先进技术研究院 一种四轮独立转向-独立驱动车辆轨迹跟踪方法和系统
CN112937580B (zh) * 2019-11-25 2023-04-14 宇通客车股份有限公司 一种目标路径跟踪方法与装置
CN110955245B (zh) * 2019-12-09 2021-06-08 深圳市银星智能科技股份有限公司 路径追踪方法、装置、移动机器人及非暂态计算机可读存储介质
CN112650217B (zh) * 2020-12-03 2022-06-28 南京理工大学 一种基于评估函数的机器人轨迹跟踪策略动态优化方法
CN114690754B (zh) * 2020-12-25 2024-02-09 天津大学 重复作业式无人车迭代学习自趋优的循迹控制算法
CN112947433B (zh) * 2021-02-03 2023-05-02 中国农业大学 一种果园移动机器人及其自主导航方法
CN113515044B (zh) * 2021-07-02 2022-08-26 北京理工大学 一种轮式移动机器人的基于学习预测跟踪控制方法及装置
CN113608529B (zh) * 2021-07-21 2024-08-23 天津农学院 一种轮式移动平台运动轨迹预瞄跟踪控制系统、方法
CN114139795B (zh) * 2021-11-26 2024-09-13 苏州安智汽车零部件有限公司 一种基于智能算法的汽车轨迹预测方法
CN114509936B (zh) * 2022-04-19 2022-09-02 之江实验室 一种运动能力可在线学习的运动规划方法、装置和介质

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105549598A (zh) * 2016-02-16 2016-05-04 江南大学 一种二维运动移动机器人的迭代学习轨迹跟踪控制及其鲁棒优化方法
CN107272677A (zh) * 2017-06-07 2017-10-20 东南大学 一种移动机器人的变结构自适应轨迹跟踪控制方法
CN109164810A (zh) * 2018-09-28 2019-01-08 昆明理工大学 一种基于蚁群-聚类算法的机器人自适应动态路径规划方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105549598A (zh) * 2016-02-16 2016-05-04 江南大学 一种二维运动移动机器人的迭代学习轨迹跟踪控制及其鲁棒优化方法
CN107272677A (zh) * 2017-06-07 2017-10-20 东南大学 一种移动机器人的变结构自适应轨迹跟踪控制方法
CN109164810A (zh) * 2018-09-28 2019-01-08 昆明理工大学 一种基于蚁群-聚类算法的机器人自适应动态路径规划方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
A robust adaptive fuzzy variable structure tracking control for the wheeled mobile robot:simulation and experimental results;BEGNINI M等;《Control Engineering Practice》;20170428;第27-43页 *
Adaptive critic motion control design of autonomous wheeled mobile robot by dual heuristic programming;LIN Weisong等;《Automatica》;20081010;第2716-2723页 *
基于GNSS的农机自动导航路径搜索及转向控制;魏爽等;《农业工程学报》;20170228;第33卷;第70-77页 *
视觉导引AGV鲁棒特征识别与精确路径跟踪研究;武星等;《农业机械学报》;20160731;第47卷(第7期);第48-56页 *

Also Published As

Publication number Publication date
CN110244735A (zh) 2019-09-17

Similar Documents

Publication Publication Date Title
CN110244735B (zh) 移动机器人跟踪预定轨迹的启发式动态规划控制方法
Guo et al. Design of automatic steering controller for trajectory tracking of unmanned vehicles using genetic algorithms
CN108227491B (zh) 一种基于滑模神经网络的智能车轨迹跟踪控制方法
Ollero et al. Predictive path tracking of mobile robots. Application to the CMU Navlab
CN105320129A (zh) 一种无人驾驶自行车轨迹跟踪控制的方法
CN109799814A (zh) 基于运动学模型的履带无人车辆轨迹跟踪控制器设计方法
Heredia et al. Stability of autonomous vehicle path tracking with pure delays in the control loop
CN110687908B (zh) 基于ros的园区无人车轨迹生成系统及其控制方法
Li et al. Multiple vehicle formation control based on robust adaptive control algorithm
CN108958246A (zh) 无人车在U-Turn场景的轨迹跟踪控制方法
CN113848905B (zh) 基于神经网络和自适应控制的移动机器人轨迹跟踪方法
CN106354147A (zh) 一种基于发动机预摆的大质心横移下的姿轨控耦合控制系统与方法
CN114442491A (zh) 室内机器人的局部路径规划和跟踪控制方法、装置及介质
CN114879698B (zh) 基于改进人工势场和mpc的机器人驾驶车辆避障方法
CN114200926A (zh) 一种无人驾驶车辆的局部路径规划方法及系统
CN114940164A (zh) 一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法及系统
CN115265532A (zh) 一种用于船用组合导航中的辅助滤波方法
Pshikhopov et al. Position-path control of a vehicle
Shin et al. Explicit path tracking by autonomous vehicles
Tsai et al. Modeling and hierarchical tracking control of tri-wheeled mobile robots
CN110209190B (zh) 一种卫星标称轨道无偏飞行控制的方法
CN116700317A (zh) 基于视觉伺服的四旋翼无人机rbf滑模轨迹跟踪控制方法
Vilca et al. An overall control strategy based on target reaching for the navigation of an urban electric vehicle
Worrall et al. Autonomous planetary rover control using inverse simulation
Kanjanawanishkul Coordinated path following for mobile robots using a virtual structure strategy with model predictive control

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant