CN111506081B - 一种机器人轨迹跟踪方法、系统及存储介质 - Google Patents

Abstract

本发明公开了一种机器人轨迹跟踪方法、系统及存储介质，将轨迹优化和控制融合到一起，更好的保证机器人进行轨迹跟踪的可行性。机器人穿越稠密障碍物时，本发明采用自适应的权重，根据差动机器人当前位置同障碍物间的最短距离相应调整目标函数中的权值，从而使得机器人在进行轨迹跟踪时能兼顾安全性、运行速度，改善障碍物区域的轨迹跟踪性能。本发明通过将避障约束放到目标函数中，将其转化为软约束，通过在容许控制空间中采样的方式寻找到最优控制输入，解决了非凸的优化问题，同时，通过动态调整权重的方式提高了移动机器人避障性能。

Description

一种机器人轨迹跟踪方法、系统及存储介质

技术领域

本发明涉及双轮差动式机器人轨迹跟踪控制领域，特别是一种机器人轨迹跟踪方法、系统及存储介质。

背景技术

进入二十一世纪第二个十年以来,国内外对机器人技术的发展越来越重视，机器人技术被认为是对新兴产业发展具有重要意义的高技术之一。在计算机技术、网络技术、MEMS技术等新技术发展的推动下,机器人技术正从传统的工业制造领域向医疗服务、教育娱乐等领域迅速扩展，大量的用于室内的服务型机器人前景广阔。

由于双轮差动型移动方式成本低廉而被室内服务型机器人广泛采用。当移动机器人运行在室内，经常会遇到需要穿越障碍物区域的情况，而目前，大多数双轮差动移动机器人的在穿越障碍物区域时，运行速度、安全性能不能很好地兼顾。运动规划问题通常包括两个步骤，首先进行路径规划，之后在路径规划的基础上完成轨迹跟踪。这类方法大多是基于凸优化来实现的，而空间中障碍物的存在往往使问题变为非凸的，无法解决非凸的优化问题。

发明内容

本发明所要解决的技术问题是，针对现有技术不足，提供一种机器人轨迹跟踪方法、系统及存储介质，解决现有双轮驱动机器人穿越障碍物区域时，运行速度、安全性能不能兼顾的问题。

为解决上述技术问题，本发明所采用的技术方案是：一种机器人轨迹跟踪方法，其特征在于，包括以下步骤：

1)获取全局路径序列

其中，

表示全局规划路径在全局坐标系下的第i个路径点对应的横坐标，

表示全局规划路径在全局坐标系下的第i个路径点对应的纵坐标，

表示全局规划序列在全局坐标系下的第i个路径点对应的航向角，N对应全局规划的路径中路径点的数量；

2)将机器人轨迹跟踪问题构建成最优化规划问题

采用动态窗口法求解所述最优化规划问题，得到局部最优轨迹序列X^*:＝{x_i,y_i,θ_i}_i＝1,...n和控制量序列 U^*:＝{v_i,ω_i}_i＝1,...n-1；其中，γ_k代表各个优化目标的权重，f_k(B)代表相应的各部分子代价函数；B:＝{X,U}代表可行解， X:＝{x_i}_i＝1,...,n,x_i＝(x_i,y_i,θ_i)代表轨迹序列，U:＝{u_i}_{i＝1,...,n-1},u_i＝(v_i,ω_i)代表控制量序列，v_i,ω_i分别为机器人从路径点i-1到路径点i的速度和角速度，n对应求解出的轨迹点数量；

3)将求解得到的速度、角速度序列的前m个值U_m ^*:＝{v_i,ω_i}_i＝1,...m，按照控制周期下发给机器人执行器；

4)若机器人到达终点，则停止；否则跳转至步骤2)。

本发明的上述步骤融合了轨迹优化和控制，可以更好的保证机器人进行轨迹跟踪的可行性，可以很好地解决非凸的优化问题，兼顾了机器人运行速度和安全性能。步骤2)中，构建最优化规划问题的具体实现过程包括：

A)构建机器人全局坐标系和机器人运动坐标系，并获取当前机器人在全局坐标系下的位姿x_i＝(x_i,y_i,θ_i)，即运动坐标系相对于全局坐标系的位姿；其中，机器人运动坐标系以机器人质心为原点，机器人速度方向为x轴正向，垂直x轴逆时针方向为y轴正向；全局坐标系与大地固连，定义为初始时刻机器人的运动坐标系，即机器人将要开始运动时，以机器人质心为原点，机器人将要行进的速度方向为x轴正向，垂直x轴逆时针方向为y轴正向构建的坐标系；

B)在全局坐标系下，将机器人轨迹跟踪构建成最优化问题，即代价函数

γ_k包括γ_path1、γ_path2、γ_obstacle1、γ_obstacle2、γ_goal、γ_velocity，分别对应为各个子代价函数为f_path1、f_path2、f_obstacle1、f_obstacle2、f_goal、f_velocity的权重，其中，γ_obstacle1、γ_goal、γ_velocity为设定的固定权重，γ_path1、γ_path2、γ_obstacle2为自适应变动的权重：

d_omin表示当前位置距离各个障碍物的最小距离，γ_path1,min和γ_path1,max分别表示权重γ_path1的最小值和最大值，γ_path2,max和γ_path1,min分别表示权重γ_path2的最大值和最小值，γ_obstacle2min表示权重γ_obstacle2的最小值，D_s为设定的阈值，K_c为设定的参数，a 为设定的参数；

f_k(B)表示各个子代价函数，各子代价函数具体形式如下：

f_path1＝e_Γ(d_path,min,r_pmax,ε,S,n)；

f_path2＝d_path,min；

f_obstacle1＝e_Γ(-d^p _omin,-r_omin,ε,S,n)；

f_obstacle2＝d^p _omin；

其中，d_{path, min}代表求解出的轨迹序列中点与全局路径序列中相应点的距离中的最小值，r_pmax代表最大容许跟踪误差；d^p _omin代表求解出的轨迹序列中点与障碍物距离中的最小值，r_omin代表求解出的容许机器人靠近障碍物的最短距离；

e_Γ(b,b_r,ε,S,n)为约束函数，b代表目标变量，b_r代表期望目标变量的上限，S、 n、ε为人为设定参数；x_goal、y_goal是机器人终点位置坐标，x_i、y_i是相应的机器人位置坐标，θ_i是相应的机器人航向角；v代表优化问题求解出的机器人局部轨迹行驶的平均速度；

所述优化问题的约束如下：

0≤v_i≤v_max；

-ω_max≤ω_i≤ω_max；

其中，ΔT表示机器人控制周期，v_i,ω_i分别为机器人从路径点i-1到路径点i的速度和角速度，a_i,α_i分别为机器人从路径点i-1到路径点i点加速度与角加速度， a_max,α_max分别为机器人加速度和角加速度的上限。

上述步骤A)、B)采用自适应的权重，根据差动机器人当前位置同障碍物间的最短距离相应调整目标函数中的权值，使得机器人在进行轨迹跟踪时能兼顾安全性、运行速度，改善障碍物区域的轨迹跟踪性能。将机器人路径跟踪构建成最优化问题，保证机器人满足运动学和动力学约束的情况下，可以更准确、全面的描述机器人轨迹跟踪时需要完成的跟踪与避障。

为了提高移动机器人避障性能，步骤2)中，利用所述动态窗口法求解所述最优化规划问题的具体实现过程包括：

I)根据机器人速度/角速度限制V_l1＝{v∈[0,v_max],ω∈[-ω_max,ω_max]}、加速度/角加速度限 V_l2＝{v∈[v-a_maxΔT,v+a_maxΔT],ω∈[ω-α_maxΔT,ω+α_maxΔT]}，得到机器人速度/角速度空间；

II)在速度/角速度空间中按Δv，角速度空间Δw等间隔采样，将不同的速度与角速度组合起来，模拟机器人在未来T时间内的行进轨迹；

III)通过代价函数J对机器人行进轨迹评分，选出局部最优轨迹对应路径序列X^*:＝{x_i,y_i,θ_i}_i＝1,...n和控制量序列U:＝{v_i,ω_i}_i＝1,...n-1。

为了保证求解优化问题足够快，满足实时性要求，步骤3)的具体实现过程包括：

A)组合速度、角速度序列的前m个值U^*:＝{v_i,ω_i}_i＝1,...m与控制周期ΔT，得到组合后的数据{v₁,ω₁,ΔT},{v₂,ω₂,ΔT},...,{v_m,ω_m,ΔT}；

B)将{v₁,ω₁}下发至执行器，并保持ΔT时间，之后将{v₂,ω₂}下发给执行器，并保持ΔT时间，以此类推，直至发送完{v_m,ω_m}。

步骤4)的具体实现过程包括：

i)获取机器人当前位置x_i＝(x_i,y_i)；

ii)将机器人当前位置与目标位置x_goal＝(x_goal,y_goal)比较，若位置差在以目标位置为圆心，半径为容许误差的范围内，则结束，否则跳转至步骤2)，继续进行轨迹跟踪。

通过比较当前位置与目标位置的距离来判断是否到达目标位置的容许误差范围内，方法简单易行，资源消耗小，可实现性佳。

相应地，本发明还提供了一种机器人控制系统，其包括：

上层规划器，用于获取全局路径序列

其中，

表示全局规划路径在全局坐标系下的第i个路径点对应的横坐标，

表示全局规划路径在全局坐标系下的第i个路径点对应的纵坐标，

表示全局规划序列在全局坐标系下的第i个路径点对应的航向角，N对应全局规划的路径中路径点的数量；

1)轨迹跟踪器，用于将机器人轨迹跟踪问题构建成最优化规划问题，即代价函数

采用动态窗口法求解所述最优化规划问题，得到局部最优轨迹序列X^*:＝{x_i,y_i,θ_i}_i＝1,...n和控制量序列U^*:＝{v_i,ω_i}_i＝1,...n-1，并将求解得到的速度、角速度序列的前m个值 U_m ^*:＝{v_i,ω_i}_i＝1,...m，按照控制周期下发给执行器；其中，γ_k代表各个优化目标的权重，f_k(B)代表相应的各部分子代价函数；B:＝{X,U}代表可行解，X:＝{x_i}_i＝1,...,n,x_i＝(x_i,y_i,θ_i)代表轨迹序列 U:＝{u_i}_{i＝1,...,n-1},u_i＝(v_i,ω_i)代表控制量序列，v_i,ω_i分别为机器人从路径点 i-1到路径点i的速度和角速度，n对应求解出的轨迹点数量；

本发明的系统还包括判断模块，用于判断机器人是否到达终点，若到达，则向轨迹跟踪器反馈终止命令；否则，向轨迹跟踪器反馈继续进行轨迹跟踪的命令。

本发明所述轨迹跟踪器包括：

建模单元，用于将机器人轨迹跟踪问题构建成最优化规划问题，即代价函数

其中，γ_k代表各个优化目标的权重，f_k(B)代表相应的各部分子代价函数；B:＝{X,U}代表可行解，X:＝{x_i}_i＝1,...,n,x_i＝(x_i,y_i,θ_i) 代表轨迹序列U:＝{u_i}_{i＝1,...,n-1},u_i＝(v_i,ω_i)代表控制量序列，v_i,ω_i分别为机器人从路径点i-1到路径点i的速度和角速度，n对应求解出的轨迹点数量；

计算单元，用于采用动态窗口法求解所述最优化规划问题，得到局部最优轨迹序列X^*:＝{x_i,y_i,θ_i}_i＝1,...n和控制量序列U^*:＝{v_i,ω_i}_i＝1,...n-1；

传输单元，用于将求解得到的速度、角速度序列的前m个值U_m ^*:＝{v_i,ω_i}_i＝1,...m，按照控制周期下发给执行器；

优选地，所述计算单元包括：

采样模块，用于在速度/角速度空间中按Δv，角速度空间Δw等间隔采样，将不同的速度与角速度组合起来，模拟机器人在未来T时间内的行进轨迹；

评分模块，用于通过代价函数J对机器人行进轨迹评分，选出局部最优轨迹对应路径序列X^*:＝{x_i,y_i,θ_i}_i＝1,...n和控制量序列U^*:＝{v_i,ω_i}_i＝1,...n-1。

作为一个发明构思，本发明还提供了一种计算机存储介质，其存储有程序，该程序被配置为用于执行本发明方法的步骤。

与现有技术相比，本发明所具有的有益效果为：

1.相比于传统机器人轨迹跟踪方法，本发明方法将轨迹优化和控制融合到一起，可以更好的保证机器人进行轨迹跟踪的可行性。

2.机器人穿越稠密障碍物时，本发明采用自适应的权重，根据差动机器人当前位置同障碍物间的最短距离相应调整目标函数中的权值，从而使得机器人在进行轨迹跟踪时能兼顾安全性、运行速度，改善障碍物区域的轨迹跟踪性能。

3.本发明通过将避障约束放到目标函数中，将其转化为软约束，通过在容许控制空间中采样的方式寻找到最优控制输入，解决了非凸的优化问题，同时，通过动态调整权重的方式提高了移动机器人避障性能。

附图说明

图1本发明所述方法的流程图；

图2动态窗口法流程图；

图3 e_Γ(x,x_r,ε,S,n)函数的约束效果示意图；

图4动态窗口法示意图；

图5为轨迹跟踪仿真示意图；

图6仿真跟踪误差；

图7仿真速度图；

图8仿真角速度图；

图9机器人控制系统框图。

具体实施方式

本发明实施例提供了一种基于弹性约束的室内双轮差动机器人的轨迹跟踪方法，如图1所示，包括以下步骤：

步骤一，差动机器人接收上层规划器发来的全局路径序列

步骤二，全局路径序列，将差动机器人轨迹跟踪问题构建成最优化规划问题，采用动态窗口法求解得到局部最优轨迹序列X^*:＝{x_i,y_i,θ_i}_i＝1,...n和控制量序列 U^*:＝{v_i,ω_i}_i＝1,...n-1；

步骤三，将求解得到的速度、角速度序列的前m个值U_m ^*:＝{v_i,ω_i}_i＝1,...m，按照控制周期下发给机器人底盘执行器，m取为1；

步骤四，若机器人到达终点则停止，否则跳转至步骤二。

步骤一中的方法具体步骤如下：

步骤1，在已经构建好的机器人全局代价地图中利用Hybrid A_star或Kinodynamic RRT_star、PRM_star等方法搜索到全局最优路径点序列

步骤2，规划层将全局最优路径点序列

发送给轨迹跟踪层；

步骤3，轨迹跟踪层接收到全局路径后，向上层给出反馈并开始轨迹跟踪任务。步骤二中的方法具体步骤如下：

步骤1，由于机器人在进行轨迹跟踪的过程中需要时时刻刻获取位姿反馈(机器人运动坐标系相对于全局坐标系的平移和旋转)，所以需要在机器人运行前规定好相应的坐标系。本发明中，构建机器人全局坐标系和机器人运动坐标系，以获取机器人相对于全局坐标的位姿，其中，机器人运动坐标系以机器人质心为原点，机器人速度方向为x轴正向，垂直x轴逆时针方向为y轴正向；全局坐标系与大地固连，定义为初始时刻机器人的运动坐标系，即机器人将要开始运动时，以机器人质心为原点，机器人将要行进的速度方向为x轴正向，垂直x轴逆时针方向为y 轴正向构建的坐标系；

步骤2，将机器人轨迹跟踪构建成多目标约束最优化问题

对于优化目标函数

其中，γ_k代表各个子代价函数对应的权重，f_k(B)代表各个子代价函数，本发明中，γ_k包括γ_path1、γ_path2、γ_obstacle1、γ_obstacle2、γ_goal、γ_velocity，相应的对应的子代价函数分别为f_path1、f_path2、 f_obstacle1、f_obstacle2、f_goal、f_velocity。B:＝{X,U}代表可行解， X:＝{x_i}_i＝1,...,n,x_i＝(x_i,y_i,θ_i)代表轨迹序列，U:＝{u_i}_{i＝1,...,n-1},u_i＝(v_i,ω_i)代表控制量序列，v_i,ω_i分别为机器人从路径点i-1到路径点i的速度和角速度，n对应求解出的轨迹点数量；

各子代价函数具体形式如下：

f_path1＝e_Γ(d_path,min,j,r_pmax,ε,S,n)

f_path2＝d_path,min

f_obstacle1＝e_Γ(-d^p _omin,-r_omin,ε,S,n)；

f_obstacle2＝d^p _omin；

函数可以保证，当b＞b_r时，代价函数值迅速增大，在优化问题中利用这个函数以将目标变量约束到一定范围内，其中， S、n、ε是影响函数约束性能的一些参数，其中，S、n、ε是影响函数约束性能的一些参数，分别设为0.1,2.1,0.1。

f_path1＝e_Γ(d_{path, min},r_pmax,ε,S,n)，f_path2＝d_{path, min}代表轨迹点限制，其目的是将上层规划器传来的全局路径点的一定范围内从而保证跟踪精度，其中，d_{path, min}代表求解出的轨迹序列中点与全局路径序列中相应点的距离中的最小值，r_pmax代表最大容许跟踪误差；相比于f_path2，f_path1在机器人远离轨迹到一个阈值时，惩罚呈指数增长，提供强效的约束作用，能将机器人限制在全局路径的一定范围内，本实例中r_pmax取为0.25。

而f_obstacle1＝e_Γ(-d^p _omin,-r_omin,ε,S,n)，f_obstacle2＝d^p _omin代表障碍物点限制，其目的是限制机器人是其远离障碍物，d^p _omin代表求解出的轨迹序列中点与各个障碍物的距离中的最小值，r_omin代表容许机器人靠近障碍物的最短距离；相比于f_obstacle2， f_obstacle1在机器人远离轨迹到一个阈值时，惩罚呈指数增长，提供强效约束作用，能将机器人限制在远离障碍物的一定范围内，本实例中r_omin取为0.5。

这一项中，arctan(·)表示反正切函数、y是机器人当前坐标，而x_goal、y_goal是机器人终点位置坐标，这一项使机器人可以尽量保持朝向终点位置行进，可以保证全局轨迹失效时，机器人还有到一定达终点位置的能力。

这一项的作用是使得机器人可以行进的更快，尽量快速的完成任务。 e_Γ(a,a_r,ε,S,n)函数保证当a＞a_r时，代价函数值迅速增大，在优化问题中利用这个函数以将目标变量约束到一定范围内。

相应的约束如下：

0≤v_i≤v_max

-ω_max≤ω_i≤ω_max

其中，ΔT表示机器人控制周期，v_i,ω_i分别为机器人从路径点i-1到路径点i的速度和角速度，a_i,α_i分别为机器人从路径点i-1到路径点i点加速度与角加速度， a_max,α_max分别为机器人加速度和角加速度的上限。

当机器人按照规划轨迹进行跟踪时，若原定路径附近出现了新的障碍物，则迅速远离障碍物成为第一要务，此时，释放掉机器人因远离全局轨迹所造成的代价，当机器人远离障碍物后再将这部分重新加入到代价函数中，因此，令f_path1和f_path2对应的权重自适应改变

同理，为了提高机器人通过障碍物区域的能力，令f_obstacle2对应的权重自适应改变，使得当机器人距障碍物过近时，距离障碍物代价项对应的权重变大

以保证机器人尽量远离障碍物，其中，d_omin代表机器人与障碍间的最小距离。本实例中D_s取为0.5，K_c取为0.1，a取为1。

在本发明中，起点坐标设置为(0,0)，期望坐标设置为(10，10)，γ_path2,max＝0.005，γ_path1，max＝0.3，γ_path1，min＝γ_path2，min＝0，γ_obstacle1＝0.03，γ_{obstacle2,min}＝0.15，γ_goal＝0.08，γ_v＝0.08。

步骤3，使用动态窗口法求解优化问题。

步骤二中的动态窗口法步骤如下：

步骤1，根据机器人速度/角速度限制V_l1＝{v∈[0,v_max],ω∈[-ω_max,ω_max]}、加速度/角加速度限制V_l2＝{v∈[v-a_maxΔT,v+a_maxΔT],ω∈[ω-α_maxΔT,ω+α_maxΔT]} 得到机器人速度/角速度空间，ΔT取为0.1s，v_max取得为1m/s，w_max取为20°/s， a_max取为0.2m/s²，α_max取为50°/s²。

步骤2，在速度空间中按Δv＝0.1m/s，角速度空间Δw＝0.1°/s等间隔采样，将不同的速度与角速度组合起来模拟机器人在未来T时间内的行进轨迹，T取为3s。Δv,Δw和T的取值范围同机器人上控制器的性能相关，根据经验选取，但是，当Δv，Δw，和T增大时算法的复杂度会增大，求解时间随着Δv，Δw，和T的增大而增大，因此在本发明中，设定Δv＝0.1，Δw＝0.1，对于一般的轨迹跟踪来说就已经可以较好的完成任务了，将他们的值取得更小可能能获得更好的效果。对于时间T，对于室内机器人，运行速度较低，一般取2-5s即可；

步骤3，通过代价函数对机器人行进轨迹评分，选出局部最优轨迹对应路径序列X^*:＝{x_i,y_i,θ_i}_i＝1,...n、控制量序列U^*＝{v_i,ω_i}_i＝1,...n-1

步骤四中的方法具体步骤如下：

步骤1，将求解得到的速度、角速度序列的前m个值U_m ^*:＝{v_i,ω_i}_i＝1,...m同控制周期ΔT组合起来，{v₁,ω₁,ΔT},{v₂,ω₂,ΔT},...,{v_m,ω_m,ΔT}

步骤2，将{v₁,ω₁}下发到执行器，并保持ΔT时间，之后将{v₂,ω₂}下发给执行器，并保持ΔT时间，以此类推，直到第m个值，本发明中，m取为1；

步骤五中的方法具体步骤如下：

步骤1，接收传感器发来的机器人当前位置x_i＝(x_i,y_i)；

步骤2，将机器人当前位置与目标位置x_goal＝(x_goal,y_goal)比较，若当前位置距离目标位置的距离在容许误差(可选择0.05～0.2m)范围内，则结束轨迹跟踪流程，否则跳转至步骤二，继续进行轨迹跟踪。本发明中到达距目标位置0.1米范围内即停止跟踪。

本实施例跟踪效果如图5所示，其中，虚线为全局规划路径，实线为机器人实际行进轨迹，椭圆表示障碍物，星形代表目标位置，圆圈代表机器人本体。在图中，当机器人沿着路径行进到靠近坐标为(10，4)障碍物附近时，代价函数中γ_path1和γ_path2变为0，偏离全局路径代价释放，而γ_obstacle2增大，达到使机器人远离障碍物的方向行进的效果。

本发明的另一实施例提供了机器人控制系统，其包括：

上层规划器，用于获取全局路径序列

其中，

表示全局规划路径在全局坐标系下的第i个路径点对应的横坐标，

表示全局规划路径在全局坐标系下的第i个路径点对应的纵坐标，

表示全局规划序列在全局坐标系下的第i个路径点对应的航向角，N对应全局规划的路径中路径点的数量；

轨迹跟踪器，用于将机器人轨迹跟踪问题构建成最优化规划问题，即代价函数

采用动态窗口法求解所述最优化规划问题，得到局部最优轨迹序列X^*:＝{x_i,y_i,θ_i}_i＝1,...n和控制量序列U^*:＝{v_i,ω_i}_i＝1,...n-1，并将求解得到的速度、角速度序列的前m个值U^*:＝{v_i,ω_i}_i＝1,...m，按照控制周期下发给执行器(即机器人底盘处的电机及其驱动模块，功能是将输入的速度与角速度信号转化为电机的转矩)；其中，γ_k代表各个优化目标的权重，f_k(B)代表相应的各部分子代价函数；B:＝{X,U}代表可行解，X:＝{x_i}_i＝1,...,n,x_i＝(x_i,y_i,θ_i)代表轨迹序列U:＝{u_i}_{i＝1,...,n-1},u_i＝(v_i,ω_i)代表控制量序列，v_i,ω_i分别为机器人从路径点i-1 到路径点i的速度和角速度，n对应求解出的轨迹点数量；

判断模块，用于判断机器人是否到达终点，若到达，则向轨迹跟踪器反馈终止命令；否则，向轨迹跟踪器反馈继续进行轨迹跟踪的命令。

本实施例控制系统中各部分子代价函数与上述实施例中子代价函数表达式相同，最优化规划问题的求解过程与上述实施例中的求解过程相同，此处不再赘述。需要注意的是，本发明控制系统中的上层规划器、轨迹跟踪器可以集成于一个微处理器中，也可以分别配置在两个微处理器中。

本实施例的轨迹跟踪器包括：

建模单元，用于将机器人轨迹跟踪问题构建成最优化规划问题，即代价函数

其中，γ_k代表各个优化目标的权重，f_k(B)代表相应的各部分子代价函数；B:＝{X,U}代表可行解，X:＝{x_i}_i＝1,...,n,x_i＝(x_i,y_i,θ_i) 代表轨迹序列U:＝{u_i}_{i＝1,...,n-1},u_i＝(v_i,ω_i)代表控制量序列，v_i,ω_i分别为机器人从路径点i-1到路径点i的速度和角速度，n对应求解出的轨迹点数量；

计算单元，用于采用动态窗口法求解所述最优化规划问题，得到局部最优轨迹序列X^*:＝{x_i,y_i,θ_i}_i＝1,...n和控制量序列U^*:＝{v_i,ω_i}_i＝1,...n-1；

传输单元，用于将求解得到的速度、角速度序列的前m个值U_m ^*:＝{v_i,ω_i}_i＝1,...m，按照控制周期下发给执行器；

本实施例中的计算单元包括：

采样模块，用于在速度/角速度空间中按Δv，角速度空间Δw等间隔采样，将不同的速度与角速度组合起来，模拟机器人在未来T时间内的行进轨迹；

评分模块，用于通过代价函数J对机器人行进轨迹评分，选出局部最优轨迹对应路径序列X^*:＝{x_i,y_i,θ_i}_i＝1,...n和控制量序列U^*:＝{v_i,ω_i}_i＝1,...n-1。

本发明另一实施例还提供了一种计算机存储介质，其存储有程序，该程序被配置为用于执行本发明实施例方法的步骤。

Claims (10)

1.一种机器人轨迹跟踪方法，其特征在于，包括以下步骤：

1)获取全局路径序列

其中，

表示全局规划路径在全局坐标系下的第i个路径点对应的横坐标，

表示全局规划路径在全局坐标系下的第i个路径点对应的纵坐标，

表示全局规划序列在全局坐标系下的第i个路径点对应的航向角，N对应全局规划的路径中路径点的数量；

2)将机器人轨迹跟踪问题构建成最优化规划问题

B:＝{X,U}，采用动态窗口法求解所述最优化规划问题，得到局部最优轨迹序列X^*:＝{x_i,y_i,θ_i}_i＝1,...n和控制量序列U^*:＝{v_i,ω_i}_i＝1,...n-1；其中，γ_k代表各个优化目标的权重，f_k(B)代表相应的各部分子代价函数；B:＝{X,U}代表可行解，X:＝{x_i}_i＝1,...,n,x_i＝(x_i,y_i,θ_i)代表轨迹序列；U:＝{u_i}_{i＝1,...,n-1},u_i＝(v_i,ω_i)代表控制量序列，v_i,ω_i分别为机器人从路径点i-1到路径点i的速度和角速度，n对应求解出的轨迹点的数量；构建最优化规划问题的具体实现过程包括：

A)构建机器人全局坐标系和机器人运动坐标系，并获取当前机器人在全局坐标系下的位姿x_i＝(x_i,y_i,θ_i)，即运动坐标系相对于全局坐标系的位姿；其中，机器人运动坐标系以机器人质心为原点，机器人速度方向为x轴正向，垂直x轴逆时针方向为y轴正向；全局坐标系与大地固连，定义为初始时刻机器人的运动坐标系，即机器人将要开始运动时，以机器人质心为原点，机器人将要行进的速度方向为x轴正向，垂直x轴逆时针方向为y轴正向构建的坐标系；

B)在全局坐标系下，将机器人轨迹跟踪构建成最优化问题，即代价函数

B:＝{X,U}；γ_k包括γ_path1、γ_path2、γ_obstacle1、γ_obstacle2、γ_goal、γ_velocity，分别对应为各个子代价函数为f_path1、f_path2、f_obstacle1、f_obstacle2、f_goal、f_velocity的权重，其中，γ_obstacle1、γ_goal、γ_velocity为设定的固定权重，γ_path1、γ_path2、γ_obstacle2为自适应变动的权重：

d_omin表示当前位置距离各个障碍物的最小距离，γ_path1,min和γ_path1,max分别表示权重γ_path1的最小值和最大值，γ_path2,max和γ_path1,min分别表示权重γ_path2的最大值和最小值，γ_obstacle2min表示权重γ_obstacle2的最小值，D_s为设定的阈值，K_c为设定的参数，a为设定的参数；

f_k(B)表示各个子代价函数，各子代价函数具体形式如下：

f_path1＝e_Γ(d_path,min,r_pmax,ε,S,n)；

f_path2＝d_path,min；

f_obstacle1＝e_Γ(-d^p _omin,-r_omin,ε,S,n)；

f_obstacle2＝d^p _omin；

其中，d_{path, min}代表求解出的轨迹序列中点与全局路径序列中相应点的距离中的最小值，r_pmax代表最大容许跟踪误差；d^p _omin代表求解出的轨迹序列中点与障碍物距离中的最小值，r_omin代表求解出的容许机器人靠近障碍物的最短距离；e_Γ(b,b_r,ε,S,n)为约束函数，b代表目标变量，b_r代表期望目标变量的上限，S、n、ε为人为设定参数；x_goal、y_goal是机器人终点位置坐标，x_i、y_i是相应的机器人位置坐标，θ_i是相应的机器人航向角；v代表优化问题求解出的机器人局部轨迹行驶的平均速度；

所述优化问题的约束如下：

0≤v_i≤v_max；

-ω_max≤ω_i≤ω_max；

其中，ΔT表示机器人控制周期，v_i,ω_i分别为机器人从路径点i-1到路径点i的速度和角速度，a_i,α_i分别为机器人从路径点i-1到路径点i点加速度与角加速度，a_max,α_max分别为机器人加速度和角加速度的上限；

3)将求解得到的速度、角速度序列的前m个值U_m ^*:＝{v_i,ω_i}_i＝1,...m，按照控制周期下发给机器人执行器；

4)若机器人到达终点，则停止；否则跳转至步骤2)。

2.根据权利要求1所述的机器人轨迹跟踪方法，其特征在于，步骤2)中，利用所述动态窗口法求解所述最优化规划问题的具体实现过程包括：

I)根据机器人速度/角速度限制V_l1＝{v∈[0,v_max],ω∈[-ω_max,ω_max]}、加速度/角加速度限V_l2＝{v∈[v-a_maxΔT,v+a_maxΔT],ω∈[ω-α_maxΔT,ω+α_maxΔT]}，得到机器人速度/角速度空间；

II)在速度/角速度空间中按Δv，角速度空间Δw等间隔采样，将不同的速度与角速度组合起来，模拟机器人在未来T时间内的行进轨迹；

III)通过代价函数J对机器人行进轨迹评分，选出局部最优轨迹对应路径序列X^*:＝{x_i,y_i,θ_i}_i＝1,...n和控制量序列U^*:＝{v_i,ω_i}_i＝1,...n-1。

3.根据权利要求1所述的机器人轨迹跟踪方法，其特征在于，步骤3)的具体实现过程包括：

A)组合速度、角速度序列的前m个值U_m ^*:＝{v_i,ω_i}_i＝1,...m与控制周期ΔT，得到组合后的数据{v₁,ω₁,ΔT},{v₂,ω₂,ΔT},...,{v_m,ω_m,ΔT}；

B)将{v₁,ω₁}下发至执行器，并保持ΔT时间，之后将{v₂,ω₂}下发给执行器，并保持ΔT时间，以此类推，直至发送完{v_m,ω_m}。

4.根据权利要求1所述的机器人轨迹跟踪方法，其特征在于，步骤4)的具体实现过程包括：

i)获取机器人当前位置x_i＝(x_i,y_i)；

ii)将机器人当前位置与目标位置x_goal＝(x_goal,y_goal)比较，若位置差在以目标位置为圆心，半径为容许误差的范围内，则结束，否则跳转至步骤2)，继续进行轨迹跟踪。

5.一种机器人控制系统，其特征在于，包括：

上层规划器，用于获取全局路径序列

其中，

表示全局规划路径在全局坐标系下的第i个路径点对应的横坐标，

表示全局规划路径在全局坐标系下的第i个路径点对应的纵坐标，

表示全局规划序列在全局坐标系下的第i个路径点对应的航向角，N对应全局规划的路径中路径点的数量；

轨迹跟踪器，用于将机器人轨迹跟踪问题构建成最优化规划问题，即代价函数

B:＝{X,U}，采用动态窗口法求解所述最优化规划问题，得到局部最优轨迹序列X^*:＝{x_i,y_i,θ_i}_i＝1,...n和控制量序列U^*:＝{v_i,ω_i}_i＝1,...n-1，并将求解得到的速度、角速度序列的前m个值U_m ^*:＝{v_i,ω_i}_i＝1,...m，按照控制周期下发给执行器；其中，γ_k代表各个优化目标的权重，f_k(B)代表相应的各部分子代价函数；B:＝{X,U}代表可行解，X:＝{x_i}_i＝1,...,n,x_i＝(x_i,y_i,θ_i)代表轨迹序列，U:＝{u_i}_{i＝1,...,n-1},u_i＝(v_i,ω_i)代表控制量序列，v_i,ω_i分别为机器人的速度和角速度，n对应求解出的轨迹点的数量；

其中，轨迹跟踪器构建最优化规划问题的具体实现过程包括：

A)构建机器人全局坐标系和机器人运动坐标系，并获取当前机器人在全局坐标系下的位姿x_i＝(x_i,y_i,θ_i)，即运动坐标系相对于全局坐标系的位姿；其中，机器人运动坐标系以机器人质心为原点，机器人速度方向为x轴正向，垂直x轴逆时针方向为y轴正向；全局坐标系与大地固连，定义为初始时刻机器人的运动坐标系，即机器人将要开始运动时，以机器人质心为原点，机器人将要行进的速度方向为x轴正向，垂直x轴逆时针方向为y轴正向构建的坐标系；

B)在全局坐标系下，将机器人轨迹跟踪构建成最优化问题，即代价函数

B:＝{X,U}；γ_k包括γ_path1、γ_path2、γ_obstacle1、γ_obstacle2、γ_goal、γ_velocity，分别对应为各个子代价函数为f_path1、f_path2、f_obstacle1、f_obstacle2、f_goal、f_velocity的权重，其中，γ_obstacle1、γ_goal、γ_velocity为设定的固定权重，γ_path1、γ_path2、γ_obstacle2为自适应变动的权重：

d_omin表示当前位置距离各个障碍物的最小距离，γ_path1,min和γ_path1,max分别表示权重γ_path1的最小值和最大值，γ_path2,max和γ_path1,min分别表示权重γ_path2的最大值和最小值，γ_obstacle2min表示权重γ_obstacle2的最小值，D_s为设定的阈值，K_c为设定的参数，a为设定的参数；

f_k(B)表示各个子代价函数，各子代价函数具体形式如下：

f_path1＝e_Γ(d_path,min,r_pmax,ε,S,n)；

f_path2＝d_path,min；

f_obstacle1＝e_Γ(-d^p _omin,-r_omin,ε,S,n)；

f_obstacle2＝d^p _omin；

其中，d_{path, min}代表求解出的轨迹序列中点与全局路径序列中相应点的距离中的最小值，r_pmax代表最大容许跟踪误差；d^p _omin代表求解出的轨迹序列中点与障碍物距离中的最小值，r_omin代表求解出的容许机器人靠近障碍物的最短距离；e_Γ(b,b_r,ε,S,n)为约束函数，b代表目标变量，b_r代表期望目标变量的上限，S、n、ε为人为设定参数；x_goal、y_goal是机器人终点位置坐标，x_i、y_i是相应的机器人位置坐标，θ_i是相应的机器人航向角；v代表优化问题求解出的机器人局部轨迹行驶的平均速度；

所述优化问题的约束如下：

0≤v_i≤v_max；

-ω_max≤ω_i≤ω_max；

其中，ΔT表示机器人控制周期，v_i,ω_i分别为机器人从路径点i-1到路径点i的速度和角速度，a_i,α_i分别为机器人从路径点i-1到路径点i点加速度与角加速度，a_max,α_max分别为机器人加速度和角加速度的上限。

6.根据权利要求5所述的机器人控制系统，其特征在于，还包括判断模块，用于判断机器人是否到达终点，若到达，则向轨迹跟踪器反馈终止命令；否则，向轨迹跟踪器反馈继续进行轨迹跟踪的命令。

7.根据权利要求5所述的机器人控制系统，其特征在于，各部分子代价函数表达式为：

f_path1＝e_Γ(d_path,min,j,r_pmax,ε,S,n)；

f_path2＝d_path,min；

f_obstacle1＝e_Γ(-d^p _omin,-r_omin,ε,S,n)；

f_obstacle2＝d^p _omin；

其中，d_{path, min}代表求解出的轨迹序列中点与全局路径序列中相应点的距离中的最小值，r_pmax代表最大容许跟踪误差；d^p _omin代表求解出的轨迹序列中点与障碍物距离中的最小值，r_omin代表求解出的容许机器人靠近障碍物的最短距离；e_Γ(b,b_r,ε,S,n)为一个约束函数，其中，b代表目标变量，b_r代表期望目标变量的上限，S、n、ε为人为设定参数；x_goal、y_goal是机器人终点位置坐标，x_i、y_i是相应的机器人位置坐标，θ_i是相应的机器人航向角；v代表优化问题求解出的机器人局部轨迹行驶的平均速度。

8.根据权利要求5所述的机器人控制系统，其特征在于，所述轨迹跟踪器包括：

建模单元，用于将机器人轨迹跟踪问题构建成最优化规划问题，即代价函数

B:＝{X,U}；其中，γ_k代表各个优化目标的权重，f_k(B)代表相应的各部分子代价函数；B:＝{X,U}代表可行解，X:＝{x_i}_i＝1,...,n,x_i＝(x_i,y_i,θ_i)代表轨迹序列；U:＝{u_i}_{i＝1,...,n-1},u_i＝(v_i,ω_i)代表控制量序列，v_i,ω_i分别为机器人从路径点i-1到路径点i的速度和角速度；

计算单元，用于采用动态窗口法求解所述最优化规划问题，得到局部最优轨迹序列X^*:＝{x_i,y_i,θ_i}_i＝1,...n和控制量序列U^*:＝{v_i,ω_i}_i＝1,...n-1；

传输单元，用于将求解得到的速度、角速度序列的前m个值U_m ^*:＝{v_i,ω_i}_i＝1,...m，按照控制周期下发给执行器。

9.根据权利要求8所述的机器人控制系统，其特征在于，所述计算单元包括：

采样模块，用于在速度/角速度空间中按Δv，角速度空间Δw等间隔采样，将不同的速度与角速度组合起来，模拟机器人在未来T时间内的行进轨迹；

评分模块，用于通过代价函数J对机器人行进轨迹评分，选出局部最优轨迹对应路径序列X^*:＝{x_i,y_i,θ_i}_i＝1,...n和控制量序列U^*:＝{v_i,ω_i}_i＝1,...n-1。

10.一种计算机存储介质，其存储有程序，其特征在于，该程序被配置为用于执行权利要求1～4之一所述方法的步骤。

Images