基于非线性优化的移动机器人平滑、实时碰撞规避方法与流程

本发明涉及移动机器人领域的一种机器人移动控制方法，特别涉及一种基于非线性优化的移动机器人平滑、实时碰撞规避方法。

背景技术：

1、随着人工智能和移动机器人技术的不断发展，移动机器人逐渐代替人类完成复杂繁重的工作已是大势所趋。人类对于能够在高动态环境中安全运动并完成复杂任务的自主移动机器人的需求日益增长，典型应用如无人驾驶汽车、家庭服务移动机器人，自动导引运输车等。在这些应用中要实现完全自主，关键难点是如何设计安全、平滑、实时的碰撞规避方法，即在碰撞规避的前提下，使得移动机器人在动态环境中的快速、平稳运动。自主移动机器人通常携带较重或者敏感的载荷，所以移动机器人需具备精确的碰撞规避能力。碰撞规避算法应考虑移动机器人的运动学、安全性等约束，实时规划最优平滑轨迹。需要说明的是，轨迹是在路径的基础上，增加时间序列信息，也就是在路径规划的基础上进行速度规划。只有这样，下层的控制模块才能准确地跟踪规划模块输出的标称轨迹。

2、传统的路径规划方法，例如基于图搜索的dijkstra、a*算法以及基于采样的概率路图、快速扩展随机树算法，都是在已知地图的情况下找到最优路径(通常是路径最短)，但是这些最短路径不能满足平滑性要求。现广泛应用的ros导航包中包含一种时间弹性带法(teb)。teb考虑非完整运动学、动力学等约束，将时间信息加入到路径中，但是为了提高效率teb将约束条件均转换为软约束，在出现动态障碍物时可能会陷入局部最优，导致不能满足安全性、平滑性等要求。百度apollo无人驾驶系统采用基于优化的碰撞规避方法用于自动泊车场景，通过引入对偶变量和相应非线性约束条件，将车辆动力学和碰撞规避约束巧妙地建模为一个只包含连续变量的大型非线性模型预测控制问题。该方法的缺点是计算量大，能够应用于实时性要求不高的自动泊车，不能满足高动态环境中的移动机器人碰撞规避的实时性要求。

技术实现思路

1、为了解决背景技术中存在的问题，本发明提出一种用于高动态复杂环境中自主移动机器人的平滑、实时碰撞规避方法，在规避动态障碍物的同时，最小化平滑性指标和移动机器人运行时间，克服现有方法存在的问题。

2、本发明的技术方案如下：

3、步骤一：输入必要的信息；

4、步骤二：前端初始轨迹生成；

5、对于给定一个目标点位置，使用局部路径规划模块和跟踪控制模块模拟移动机器人的运动，生成初始轨迹；

6、步骤三：后端非线性轨迹优化，获得最优轨迹；

7、步骤四：根据步骤三获得的最优轨迹输入到移动机器人的控制模块中，实时更新移动机器人的运动路线，进而实现移动机器人平滑、实时碰撞规避。

8、所述步骤一中，输入的信息包括：静态地图和实时激光数据的周围环境信息、目标点位置、移动机器人定位位置和移动机器人运动能力信息；其中静态地图包括栅格地图、地图分辨率、地图原点坐标以及障碍物信息。

9、障碍物信息是已知的，可通过激光雷达获得。

10、所述的移动机器人自身安装有激光雷达。

11、所述步骤二中，具体是：使用局部路径规划模块从事先已有的离线生成的路径库中选择到达目标点位置概率最大的路径作为跟踪规划路径，跟踪控制模块计算跟踪规划路径所需的控制指令，然后基于移动机器人的运动学模型和已知移动机器人初始状态根据跟踪控制模块输出的控制指令模拟移动机器人在一段时间内的状态进而产生移动机器人的状态量xws和控制量uws，作为步骤三非线性轨迹优化的热启动初值，由状态量xws和控制量uws生成初始轨迹。

12、所述步骤二中跟踪控制模块计算的频率高于局部路径规划模块选择的频率。

13、所述步骤三中，具体为：

14、s1、建立以下包含非线性约束条件的、带有包围盒的轨迹优化模型为目标函数及其约束条件：

15、

16、s.t.x0＝xs,xk+1＝xf

17、xk+1＝f(xk,uk),h(xk,uk)≤0

18、xk∈bk

19、for k＝0,…k

20、且xk＝[xk,yk,θk]t,uk＝[vk,wk]t

21、δuk＝(uk﹣uk-1)/τ

22、其中，min表示最小化，p为控制代价和运行时间之间的权重系数，q和qδ分别为事先设定的控制量和控制量平滑性的权重矩阵，δuk表示轨迹的平滑性参数，t表示矩阵转置；τ为模拟时的时间步的步长，τ>0；xk、uk分别表示第k时间步时移动机器人的状态量和控制量；xs表示移动机器人初始时在地图上的x轴位置，xf表示移动机器人在到达目标点位置时在地图上的x轴位置；f()表示运动学约束函数，h()表示运动能力约束函数，bk表示包围盒参数，for表示循环，k＝0～k，遍历所有的k累加，k表示时间步的序数，k表示时间步的总数；xk、yk分别表示第k时间步时移动机器人在地图上的x轴位置和y轴位置，θk表示第k时间步时移动机器人本体坐标系x轴(正前方向)与全局坐标系的x轴之间的夹角，vk、wk分别为第k时间步时移动机器人在地图上的线速度和角速度；

23、s2、在给定包围盒的情况下，将目标函数及其约束条件采用非线性数值优化工具进行求解和迭代优化，最终获得最优的时间步的步长τ、移动机器人的状态量集合x和控制量集合u，其中：

24、x＝[x0,x1,…,xk+1],u＝[u0,u1,…,uk]

25、其中，x、u为移动机器人的状态量集合和控制量集合，共有k+1步，字母为粗体；

26、s3、根据最优的时间步的步长τ、移动机器人的状态量集合x和控制量集合u生成新的轨迹作为最优轨迹。

27、本发明的目标函数能够使得移动机器人尽快到达目标点，同时减少控制代价。

28、同时提出的采用包围盒迭代调整策略来满足碰撞规避约束的方式能够提高目标函数的求解效率，满足实时性要求。

29、所述步骤三的s1中，运动学约束函数f()具体为：

30、xk+1＝xk+τvkcos(θk+0.5τwk)

31、yk+1＝yk+τvksin(θk+0.5τwk)

32、θk+1＝θk+τwk

33、所述步骤三的s1中，运动能力约束函数h()具体为：

34、。vk∈[0,vmax],wk∈[-wmax,wmax],avk∈[-avmax,avmax],awk∈[-awmax,awmax]

35、其中，vmax为最大线速度，wmax为最大角速度，avmax为最大线加速度，awmax为最大角加速度，vk表示第k时间步时的移动机器人线速度，wk表示第k时间步时的移动机器人角速度，avk表示第k时间步时的移动机器人线加速度，awk表示第k时间步时的移动机器人角加速度。

36、所述步骤三的s1中，xk∈bk表示设置一个包围盒作为移动机器人位置的安全区域，包围盒bk设置表示为：

37、||xk﹣xws,k||≤dxk,||yk﹣yws,k||≤dyk

38、其中，xws,k、yws,k分别为初始轨迹中移动机器人在第k时间步时的位置，dxk、dyk分别为包围盒在x轴和y轴两个方向的长度。

39、所述步骤三的s2中，每次求解后按照以下方式迭代优化：

40、将获得的最优轨迹输入到移动机器人，移动机器人用自身的激光雷达对最优轨迹上的每一个点进行碰撞检测：

41、若检测到障碍物碰撞，则轨迹是不安全的，缩小包围盒，更新为bk＝β*bk，其中β是变化率，满足0<β＜1；

42、若未检测到障碍物碰撞，则轨迹是安全的，迭代优化终止。

43、由此本发明还提出一种迭代方法来确定合适大小的包围盒，确保碰撞规避的同时，不过度牺牲轨迹的平滑性和运行时间。

44、本发明方法将碰撞规避问题主要解耦为前端的初始轨迹生成和后端的非线性轨迹优化。

45、初始轨迹生成时最大化移动机器人到达目标点的概率，使得移动机器人倾向于通过更开阔的区域，从而为导航过程中成功绕开障碍物提供更多选择。由于没有考虑平滑性和到达目标点的运动时间，前端初始轨迹需要进一步优化。

46、后端非线性轨迹优化中，采用包围盒来保证轨迹的安全性，并迭代调整包围盒的大小，加速后端非线性轨迹优化过程且准确优化。通过引入包围盒，可以去除对偶变量及其相关约束条件，从而大大减小非线性优化问题的规模，初始轨迹能够实时收敛到平滑、安全和动态可行的轨迹。

47、与现有技术相比，本发明具有以下优点：

48、本发明提出将碰撞规避问题解耦为前端初始轨迹生成和后端轨迹非线性优化，得到的最优轨迹既能最大化到达目标点的概率，又满足平滑性要求。

49、本发明提出采用包围盒来保证轨迹的安全性，并迭代确定其大小，大大降低了优化问题的复杂度，能够满足实时性要求。