一种基于自适应MPC的机器人安全跟踪控制方法

本发明涉及机器人，尤其涉及一种基于自适应mpc的机器人安全跟踪控制方法。

背景技术：

1、机器人按发展进程一般可分为三代：第一代的机器人是一种“遥控操作器”；第二代的机器人是一种按人事先编好的程序对机器人进行控制，使其自动重复完成某种操作的方式；第三代机器人是智能机器人，它是通过各种传感器、测量器等来获取环境的信息，然后利用智能技术进行识别、理解、推理并最后做出规划决策，能自主行动实现预定目标的高级机器人。

2、然而，不管是机器人还是其他的工程系统都应该首要考虑安全性问题。为了确保差速机器人系统的安全，在设计以及操作过程中必须考虑诸多因素。例如：来自硬件系统自身的约束(工作电压以及扭矩限制等)，比如差速机器人的电机应运行在额定电压下，若长时间工作的实际电压高于额定电压则会损坏电机。另外，人形机器人的关节由许多舵机组成，由于舵机可以施加的最大扭矩是有限的，若超过铭牌的扭矩限制则会导致舵机过热而烧坏。如果能在设计及使用过程中严格执行这些约束，则可以在硬件层面保证机器人不会出现损坏，从而确保他们的安全。除此以外，其他类型的约束大多来自于与实际环境的交互。例如：无人机必须与高空物体保持一定的安全距离以避免碰撞；无人驾驶汽车要与道路上的人、静态物体以及动态的车辆保持安全距离以避免碰撞，同时也需要自主识别道路环境以避开泥沼或者坑洞等可能使自身出现不安全的路径；机器狗必须选择合适的姿态以平衡其躯干。从上述示例中可以看出，以上约束得到满足才可能使得系统能够正常运行，即保证“坏”的事情不发生，也如上文所述的安全得到满足，所以称之为“安全约束”。同时满足“安全约束”的系统则被称之为“安全攸关系统”。基于安全约束的概念，安全攸关控制问题被描述为：选择合适的控制器，使闭环系统的轨迹始终满足安全约束。

3、模型预测控制(model predictive control，mpc)由于可以相对容易地结合约束条件，因此对于利用控制障碍函数(control barrier function，cbf)来研究系统的安全性提供了极大的便利。大多数物理系统都具有不可测量的参数或者在实际过程中含有未建模动态和环境扰动。例如化学工程中反应速率、活化能、污染因子和微生物生长速率等参数不可测量。由于参数的不确定性会降低mpc的性能，因此在实际应用中需要一种机制来更新未知或不确定的参数。同时，cbf严重依赖于模型，因此当模型存在不确定性时，开发保持安全性的方法至关重要。在现有文献中，zeroing cbf(zcbf)用于证明安全集的超水平集具有前向不变的属性。还有超水平集的大小通过引入输入-状态安全来表征，使用鲁棒优化可以获得更强的安全保证，基于学习的方法被开发出来解决鲁棒策略的保守性，但是这些方法需要大量的离线训练。还有的adaptive cbf(acbf)使用自适应控制理论的思想，通过在线参数自适应来确保期望的安全集是前向不变的。然而，acbf需要一个不变性条件来限制系统保持在安全集的子集内，最终导致保守行为。为了解决保守性问题，有研究者将参数适应限制在安全集边界附近的区域。然而，所得到的安全控制器不一定是局部lipschitz连续的，并且可能表现出闭环抖动。还有研究者提出了robust adaptive cbf(racbf)，即鲁棒自适应控制障碍函数，当与参数自适应相结合时，racbf确保了收缩集合的前向不变性，其中收缩程度可以基于所需的保守性来选择。racbf较acbf有更弱的保守性并可使安全控制器是局部lipschitz连续的。

4、此外，申请号为201811636787.5的中国专利涉及参数不确定自主车辆变速巡航系统鲁棒控制方法，但仅实现车辆的鲁棒控制，没有考虑控制器的自适应能力与安全约束；申请号为202010614155.x的中国专利公开了一种智能车辆路径跟踪控制方法，这种方法将设计的最优控制序列输入智能车辆中控制车辆对目标轨迹进行跟踪，但这种方法没有进行全局路径规划，导致规划的路径可能不是最优的情况。申请号202111624520.6的中国专利公开了一种mpc的轨迹规划及跟踪方法，对于路径进行规划并进行轨迹跟踪，但这种方法没有考虑系统含有未知参数的情况，不能有效地对含有未知参数的情况进行控制，还可能在控制中破坏系统的安全约束。

5、综上所述，现有的机器人控制方法大多是只考虑系统的稳定性(即完成到达目标地点的任务)，但这些方法并不能同时实现稳定性和安全性。特别是当系统中存在未知参数时，往往会使得机器人跟踪轨迹效果变差，从而可能会使得机器人撞到障碍物，破坏系统的安全约束。

技术实现思路

1、本发明要解决的技术问题是针对上述现有技术的不足，提供一种基于自适应mpc的机器人安全跟踪控制方法，针对含有未知参数的非线性控制系统，建立了一种鲁棒自适应控制障碍函数的方法，并在差速机器人运动框架上进行应用。

2、为解决上述技术问题，本发明所采取的技术方案是：

3、一种基于自适应mpc的机器人安全跟踪控制方法，包括以下步骤：

4、步骤1：建立含有未知参数的非线性系统模型；

5、步骤2：针对未知参数，设计状态预测器，保证未知参数的渐近估计；

6、步骤3：依据参考轨迹、障碍物以及差速机器人之间的位姿信息，设计鲁棒自适应控制障碍函数的约束条件；

7、步骤4：实时优化求解代价函数，得到满足安全约束的跟踪控制器；

8、步骤5：对差速机器人进行路径规划，并利用满足安全约束的跟踪控制器对机器人进行安全跟踪控制，完成基于自适应mpc的不确定机器人系统安全跟踪控制。

9、进一步地，所述步骤1中，含有未知参数的非线性系统为：

10、

11、其中，是未知常参数向量，和分别为系统状态和控制输入，为系统的状态空间集合，为容许控制输入集合，f(x,u)、g(x,u)是系统的向量场；

12、假设未知参数θ属于一个已知的紧集θ0，并且该紧集是一个以θ0为圆心，半径为的超球体，s是集合θ0内的任意向量。

13、进一步地，所述步骤2中，设计式(1)所示非线性系统的状态预测器如下：

14、

15、其中，为预测状态，是参数估计向量，k＞0是设计的常数，是预测误差，定义参数估计误差由式(1)和式(2)得到：

16、

17、其中，表示预测误差的动态，表示的参数更新率；

18、做如下中间变量的定义：

19、

20、由式(3)和式(4)得到：

21、

22、设计状态预测器中使用的参数更新律为：

23、

24、其中，γ＞0为设定常数，ω是式(7)所示滤波器的输出；

25、

26、状态预测器是对未知参数及其估计误差界进行估计，保证参数估计及其误差界的单调非增；

27、所述估计误差界为：

28、

29、定理1：考虑系统式(1)，假定估计参数通过式(6)更新，并假设未知参数θ属于一个已知的紧集θ0，则对于全部的t≥t0，参数估计误差是有界的且上界表示为：

30、

31、其中，λmin表示ωωt的最小特征值；

32、不确定性集通过更新参数估计以及误差边界实现在线更新；

33、算法一：不确定性集θ更新如下：

34、步骤2.1：初始化并且

35、步骤2.2：在任意时刻ti+1，若式(14)条件被满足，则执行式(15)所示的更新；

36、

37、

38、步骤2.3：递增i＝i+1，迭代执行步骤2.2，直到机器人到达目标点。

39、进一步地，所述步骤3中，鲁棒自适应控制障碍函数的设计具体如下：

40、定义由连续可微函数hr:构成的集合为其中，hr是鲁棒自适应控制障碍函数，是参数的最大估计误差界，γ是一个和系统安全性相关的对称正定矩阵；存在一个扩展类函数α，使得对于所有的θ∈θ0，任意的输入使得鲁棒自适应控制障碍函数满足下式：

41、

42、其中，

43、定理2：hr是一个鲁棒自适应控制障碍函数，任何满足式(16)的局部lipschitz连续控制器其中为θ的估计，使得对于具有如(17)所示自适应律的未知参数系统来说，集合是前向不变的；

44、

45、算法二：由于在更新过程中不一定在紧集θ(t)中，设计参数估计向量的更新如下：

46、步骤3.1：初始化

47、步骤3.2：在任意时刻ti+1，按照下式更新：

48、

49、步骤3.3：判断式(19)所示条件在ti+1时刻是否满足；

50、

51、若满足，则通过式(15)更新得到然后通过式(20)得到执行更新；

52、

53、步骤3.4：迭代回到步骤3.2，递增i＝i+1，直到机器人到达目标点。

54、进一步地，所述步骤4中，跟踪控制器如下：

55、

56、其中，l(x,u)为mpc的代价函数，矩阵q、r为正定矩阵，k、k2为正常数。

57、进一步地，所述步骤5中，对差速机器人进行路径规划中，所使用的路径规划算法是基于a star算法实现的。

58、进一步地，所述a star算法的评价函数为f(n)＝g(n)+w(n)h(n)，其中，n为当前遍历的节点，h(n)表示从节点n到目标t的最优路径的估计成本，即启发函数；f(n)表示对节点n计算的权值，它是限制该路径通过节点n的情况下，从起点到终点的最优路径的实际成本；g(n)表示从初始节点s到n的最优路径的实际成本；w(n)为权重系数；

59、权重系数w(n)引入动态加权的概念，即在一定数量的路径规划所遍历的节点之外，权重系数大于设定阈值，在这个规定数量点之内，权重系数小于设定阈值，保证在距离较近时追求轨迹最优，而在距离较远时兼顾速度与路径较优的状态。

60、进一步地，所述步骤5中，使用高阶贝塞尔曲线对a star算法所发布的轨迹加以优化处理，得到一条平滑的曲线。

61、采用上述技术方案所产生的有益效果在于：本发明提供的基于自适应mpc的机器人安全跟踪控制方法，基于自适应mpc和鲁棒自适应控制障碍函数实现安全跟踪控制，且该方法具有以下显著特点：

62、1)相较于数据驱动的模型估计方法，本发明采用了一种在线估计误差边界的状态预测器。这种方法具有更简单的结构，并且需要更少的数据量；

63、2)本发明使用状态预测器对机器人模型中含有的未知参数进行估计，得到较为精确的机器人模型；

64、3)本发明通过构造鲁棒自适应控制障碍函数的安全约束，保证了真实系统而非预测模型的安全性，避免了由于模型中含有不确定未知参数导致机器人在路径跟踪中可能不完全跟踪上规划路径而出现碰撞的问题。