一种未知环境下的集群无人车分布式控制方法

本发明属于集群无人车控制，具体涉及一种未知环境下的集群无人车分布式控制方法。

背景技术：

1、目前，在需要多无人车协作的室内搬运等常见场景中，大量且复杂的静态与动态障碍物，都使无人车的感知和协同控制难度显著提升。现有针对无人车的控制常用建图算法(slam)包括a-loam、lego-aloam等、基于视觉及其它算法实现。其中，a-loam、lego-aloam虽能够实时地进行高精度的建图，但是这两种算法都是基于雷达射线的算法，在无人车较多的场景容易造成相互干扰；msf、msckf、rovio等基于视觉slam框架的算法具有滤波效果，不会造成无人车之间的相互干扰，但其精度较低不足以保证无人车的安全性；okvis、vins、vi-orb、ice-ba在slam框架的基础上进行了优化，精度较高且计算量适中，但它们都是基于单车的建模算法，在群组控制场景下对其它无人车难以滤除；基于神经网络语义识别对动态物体进行剔除是一种新颖的算法，但是它的硬件成本高，计算量很大，非常不适用于实时性要求较高的场景。多无人车的控制依赖于分布式集群控制算法，分布式模型预测控制(dmpc)常被用于这项任务，但是该算法要求无人车之间的高频通讯，且对无人车的定位精度要求较高，因而对传感器和网络的波动过于敏感；基于相对伯恩斯坦多项式(rbp)的方法保证了求解轨迹的连续性，但是其规划的轨迹常过于保守，且容易出现局部最优，导致求解失败；且集群内部的碰撞时有发生，安全性不高；在应对复杂的障碍物和邻居分布情况下，容易出现死锁和失效。此外复杂环境下的导航和建图算法常常被单独考虑，本领域鲜见将建图和控制一体化设计的集群控制手段。

技术实现思路

1、有鉴于此，针对本领域中所存在的技术问题，本发明提供了一种未知环境下的集群无人车分布式控制方法，具体包括以下步骤；

2、步骤一、将携带有用于感知三维空间点云的传感器的车辆建立自组网，以形成所需的无人车集群基本配置；

3、步骤二、针对无人车集群所获取的所处场景中的点云，基于变分推断方法执行点云聚类，得到相应的j个分类；

4、步骤三、假设j个点云分类中的每一个均分别与一个障碍物对应，获取各障碍物的相应点云在地面上的投影，使用平面上的椭圆对其进行覆盖，使每个投影中的所有点均能被一最小椭圆所包含；

5、步骤四、在公共参考系中利用步骤三得到的椭圆求解各障碍物的运动速度，并基于与速度阈值的比较进行动态和静态障碍物的分类识别；

6、步骤五、基于slam算法提取各障碍物点云中的特征点并放置在栅格中，完成每一帧局部栅格地图的构建；

7、步骤六、针对集群中的每辆无人车，综合考虑路径损失、避障损失及终点误差损失构建单独的目标函数与约束，并求解使总损失最小的最优规划曲线；使无人车按照各自的最优规划曲线行驶，即完成集群行驶任务。

8、进一步地，无人车所使用的传感器具体选用立体视觉传感器或者3d激光雷达传感器。

9、进一步地，步骤二中在对点云聚类时，首先假设点云为以下形式的混合高斯分布：

10、

11、式中，v为点云中单个点的坐标，n(v；μj,σj)为正态分布，μj、σj分别为每个正态分布的均值和方差，πj为每个正态分布的权重，满足πj>0、

12、假设隐变量λ＝{λ1,...λj}，其中λj取值范围为{0,1}，表示样本属于或者不属于第j个正态分布；θ＝{(μ1,σ1),...(μj,σj)}表示待估计的参数，则证据下界(evidencelower bound(elbo))可以写作以下形式：

13、l(φ,θ；x)＝ln(p(v|θ))-dkl(q(λ|v)||p(λ|v,θ))

14、利用期望最大化方法(expectation maximization(em))求解l的最大值，进而求解得到λ的最佳分布以及θ的最佳估计值，从而估计出聚类的总类别数目；

15、为了减小计算量，总是设置j的初始分布为整数区间1～n上的均匀分布，n为预设的最大类别数目；通过每一轮em计算后获得j对应的l函数，即可以估计出j的分布，并最终最佳的点云总分类数j。

16、进一步地，步骤三中对于所述最小椭圆具体构造以下优化问题来进行求解：

17、

18、

19、式中，ai为第i个椭圆的参数矩阵，它是一个对称正定矩阵，它的特征值分别为长半轴和短半轴的平方的倒数，它的特征向量分别为长半轴和短半轴的方向向量，|ai|为椭圆的面积；ei表示通过步骤二聚类得到的第i个点云类别，优化问题的约束表示对应类别下的所有点必须被椭圆(x-v)tai(x-v)所包含；针对优化问题使用任意二次型求解器求解。

20、进一步地，步骤四中具体选用大地坐标系并求解各椭圆的四个参数：param(a)＝[pc,ra,rb,θ]，式中pc、ra、rb、θ分别为椭圆的中心坐标、椭圆的长轴长度、椭圆的短轴长度、椭圆长轴与该公共参考系的起始轴夹角；

21、利用步骤三得到的参数矩阵，计算在时刻t所得到的第i个椭圆与在时刻t+1所得到的第j个椭圆之间的欧氏距离：

22、di,j＝||param(ai)-param(aj)||2

23、取欧式距离最小的一组椭圆，将其识别为在相邻时刻所检测到的同一组障碍物，则障碍物的线速度和角速度分别估计为：

24、

25、将各椭圆对应的线速度和角速度分别与相应的阈值thresholdv和thresholdω比较，识别出障碍物具体是静态障碍物还是动态障碍物，并将这些动态障碍物构成集合b；

26、集群中的无人车同时也作为一种动态障碍物，利用半径为r的球体最小地将各无人车自身包围，当无人车在自组网中传递自身坐标pv时，可以确定点云c＝{x|(x-pv)2<r}中的点为动态点，继而可以确定无人车所获点云中必然为动态障碍物的点云为：则将场景中所有动态障碍物表示为dynamic＝b∪d，其余的均为静态障碍物，记为static＝dynamicc。

27、进一步地，步骤五中定义使用的特征提取子为feature，则在时刻t最终提取得到的特征点为：fp＝feature(v)，其中v为所有传感器所获取的点云；根据所采用的不同传感器类型，特征提取子的选择方式分别为：对于双目视觉和rgbd摄像机选择orb特征提取子，对于3d扫描雷达则依照点云离散距离或者三维协方差矩阵等特征；

28、从特征点中再进一步去除动态点：scene＝fp/dynamic，上式中scene表示滤波后的点云，fp表示提取到的所有点云，dynamic表示识别出来的动态点云，/符号表示从总点云做求余运算除去动态点云的过程；假设在时刻t和时刻t+1获取的特征点分别为scenet、scenet+1，使用ransac算法求解帧间位移：t(t)＝pnpransac(scenet、scenet+1)，然后将特征点放置在栅格中，完成每一帧局部地图的构建。

29、进一步地，步骤六中具体构建的目标函数具体形式如下：

30、jplanenr＝∑wrjr、r∈{s,c,t}

31、其中，js、jc、jt分别表示光滑化路径的损失函数、避免碰撞的损失函数、最小化终点误差的损失函数，jplanenr表示总损失，使其达到最小为优化目标；

32、所述滑化路径的损失函数js的构建过程包括：

33、假设无人车的终点位置为pt，起始位置为ps，并构建连接这两点的样条曲线为：

34、x(t)＝a0,x+a1,xt+a2,xt2+a3,xt3

35、y(t)＝a0,y+a1,yt+a2,yt2+a3,yt3

36、即样条曲线的x坐标和y坐标分别为独立的对时间t的三次曲线，曲线的优化参数为p＝[a0,x、a1,x、a2,x、a3,x、a0,y、a1,y、a2,y、a3,y]，为了保证曲线的光滑度，取最小化的曲线参数大小：js＝phpt，其中h是某个正定对称的权重矩阵；

37、所述避免碰撞的损失函数jc由对障碍物的避障以及集群内无人车之间的避障两项组成，其构建过程包括：

38、由于所有障碍物都已经使用椭圆最小的包含，则对于曲线上的任意一点[x(ti),y(ti)]均通过椭圆中心做一条直线v连接该点到椭圆的边上，将这一点pe称作[x(ti),y(ti)]的最近点，直线l方向向量记作v，则定义内积<pe,v>为距离积，考虑通过最大化内积<pe,v>来保证无人车对障碍物的回避，由此来得到以下形式的第一项：

39、

40、对于集群内无人车也即邻居，假设邻居之间通过自组网交换下一步重规划之前的轨迹：{[x(t1),y(t1)]....[x(tn),y(tn)]}，则考虑通过最大化每一相同时刻的邻居之间的相对距离di,j(t)＝||[xi(t),yi(t)]-[xj(t),yj(t)]||2来实现无人车之间的避障，由此得到以下形式的第二项：

41、

42、最终得到避免碰撞的损失函数jc形式为jc＝jc1+jc2；

43、对于最小化终点误差的损失函数jt，假设车辆的设置终点为[xt,yt]，进而考虑将规划曲线的终点和设定终点之间的距离最小化，由此得到该损失函数形式为：

44、jt＝-||[x(tn),y(tn)]-[xt,yt]||2

45、其中x(tn)、y(tn)表示设定终点的坐标；使用任意二次型或者非线性求解器来求解上述优化问题，并得到最终的规划曲线。

46、上述本发明所提供的未知环境下的集群无人车分布式控制方法，将无人车slam算法与集群式分布控制算法有机结合，有别于现有技术中将无人车建图算法和导航算法分开考虑的处理方式。本发明利用了无人车之间的自组网通讯，辅助slam算法中的动态点云滤除过程，从而在降低计算量的同时提高了点云分类的可靠性，且对现有各类三维点云传感器，以及多种以特征点为输出的特征提取子，均可提供较强的适应性。该方法借助对elbo函数的估计，实现了自动估计障碍物数量，同时在无人车轨迹规划部分，通过最大化无人车之间每一时刻的欧式距离，实现了无人车之间的避障。通过本发明优化所得的轨迹允许交叉，因而能够使得规划算法更具灵活性，更有利于为无人车群的精准导航提供强有力支持。