提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

---

前言

这篇论文是作者前面几个工作的综合，其主要涉及了CBF，CLF，DRO，LiDar，SDF等方面，由于论文很长，所以做一个报告进行记录方便后面查看。本文设计大量证明稳定性和Lipschitzness的内容，这里不做介绍，只介绍论文主要内容

一、论文整体内容介绍

• 当存在状态估计误差（动捕）和雷达测量误差，并且是在未知环境下时，如何保证智能体的实时安全性？
• 论文提出的算法框架为：CLF-DR-CBF，分别用到了CLF保证跟踪的稳定性，CBF保证局部避障的安全性，DRO保证抗扰性。
• 对于存在不确定性时CBF的应用，通常会将约束转换为机会约束（chance
  constraints），以概率的方式来表示，但通常会造成优化问题的非凸，作者通过引入VaR和CVaR概念，将约束转换为凸约束，并将其称之为Distributionally
  robust safe control。
• 文中涉及到如何在未知环境下实时构建CBF，其中用到了SDF（signed distance function）

主要概念

$x_{+}=max(x,0)$
atan2(y,x)代表x轴与(x,y)的夹角
维数为 p 的特殊正交群记为 S O ( p ) = { R T R = I p , d e t ( R ) = 1 } SO(p)=\{ R^TR=I_p, det(R)=1 \} SO(p)={RTR=Ip​,det(R)=1}
$\mathcal{F}(t)={\mathbb{R}}^n\setminus O(t)$ 表示安全空间

---

二、CLF-CBF-QP

动力学表示为:

control Lyapunov constraint(CLC)：$CLC(x,u)\le 0$
不变集表示为:

则control barrier constraint（CBC）：$CBC(x,u,t)\ge 0$

综上，可以得到CLF-CBF-QP为

以上为关于CLF和CBF基础知识，只做简单的介绍

---

三、问题描述

1.智能体和障碍物的描述

智能体：$B(x)=A(x)B_0(x)+b(x)$
障碍物：$O(t)=A_O(t)O_0+b_O(t)$
先介绍SDF，SDF d表示从一个点q到边界$\partial S$的最小距离

则智能体的SDF表示为

描述B(x)和O(t)在实验中可以用tf变换完成，而SDF通过雷达扫描获得，若根据雷达发射的激光束数量，会采集到相应数量的SDF，求一次min即可得到最近的SDF。

---

四、Distributionally Robust Safe Control

在本文中假设相对度为1，则CBC可以重写为

• 建立$\xi$的意义在于$\xi$中的变量都可以通过动捕和雷达获得到的数据进行计算，但由于本文中假设了这两者都存在误差，所以无法得到准确的$\xi$情况下，如何保证实时安全性，这个问题就是本文最核心的问题

1. 为了处理不确定性，第一步先介绍机会约束

其中$ϵ$为容忍度，其将原来的CBC转换为了一个概率约束，意思是保证该安全约束满足的概率大于等于$1-ϵ$
但这样一个约束还是无法直接求解，所以需要再引入VaR和CVaR的概念进一步转换
V a R 1 − ϵ P ( Q ) : = i n f { s   ∣   P ( Q ≤ s ) ≥ 1 − ϵ } VaR_{1-\epsilon}^P(Q):=inf\{s\ |\ P(Q\le s)\ge 1-\epsilon \} VaR1−ϵP​(Q):=inf{s ∣ P(Q≤s)≥1−ϵ}
$CVa{R}_{1-ϵ}^P(Q)=E_P[Q|Q\ge Va{R}_{1-ϵ}^P(Q)]$
VaR（Value-at-Risk）是一个给定置信度下的最大可能损失值，而CVaR则表示超过这个损失值的期望损失，而原来的机会约束可以等价为

进一步的，该约束可以转换为凸约束

其中，参数$s\in \mathbb{R}$。这里可能疑惑的点在于为什么这个约束变成了凸约束，因为这里不再是相对于控制输入u而言，而是对于整个CBC约束$G(u,\xi )$而言。

为了方便理解VaR和CVaR，这里给出一张示意图（/robert_chen1988/article/details/85234904）。但不理解也没关系，你只需要知道经过这一系列转换后，原来无法处理的带不确定性的约束变为了一个凸约束，但整个转换过程还没结束，因为你不知道该CBC的具体真实分布P，所以还需要继续想办法如何通过雷达采样得到的 { ξ i } \{\xi_i\} {ξi​}模拟出真是分布P

在这之后又是引入了一系列概念来解决这样一个问题，由于我也只是懂他想干啥，但对其中的知识点不了解，所以我就不具体讲了，抽象过程可以用下图表示，即用雷达采样得到的定义为经验集，其在wasserstein ambiguity set映射为a点，而真实分布在wasserstein ambiguity set中的映射被包含在了半径为r的求内，因此只要算出半径r，即可得到真实分布和采样之间的关系

综上，经过上述复杂的转换过程，我们可以下图表示

最后，CBC被转换为了如下所示约束

其中，${\beta }_i\in \mathbb{R}$和s都是优化变量，因此总的优化变量为五维（u2，$\delta$ 1，$\beta$ 1，s1），$ϵ$为容忍度，N为雷达的采样数，$||u|{|}_{\infty }$表示无穷范数。

总结

至此，CBC彻底完成了转换，转换后的QP即被叫做Distributionally Robust Optimization，并且在仿真中构建的也是上述约束。

2.在有了控制器后，问题变为如何确定参考控制器

可以看到在上面的QP中需要一个局部参考输入，并且我们只是得到了一个QP去求解控制输入，但在这过程中路标点如何得到，我们应该往哪走并没有说明，所以接下来就是解决这个问题。

简单的，论文直接使用A*生成参考轨迹，但参考轨迹无法直接使用，我们需要在每一个δt告诉QP路标点在哪，参考输入是啥，这里论文引入了reference governor control techniques。考虑一个标量$g(t)\in [0,1]$满足

其中，k为标量参数，$\zeta$保证g渐进靠近1但永不到达。该设计使得参考点 γ(g) 沿着路径 γ 移动，其速度与当前机器人位置 ϕ(x) 和 γ(g) 之间的距离成反比，从而促进响应路径跟随行为。初始条件设置为g(0)=0对应参考轨迹初始点$\gamma (0)$

在仿真中，为了方便，其参考输入直接用了上一个时刻的参考输入，初始参考输入为0。但也可以设计一个简单的P控制器或者其他方法根据距离差生成一个参考输入都行。

---

五、针对差动车的设计实例

动力学如下：

1.设计CLF保证reference governor control可以稳定到达目标路标点

2.用SDF设计CBF

问题在于，雷达采集到的SDF很多，我们不可能都用来构建约束，这样会导致效率极低，因此这里会对整个采集到的SDF做一次排序，只取最小的五个SDF作为CBF使用，即只考虑当前状态下最紧急需要避障的状态。而排序标准则计算下式：

该式考虑了在当前时刻下的$\partial h/\partial x$影响，并选取了变换率最小的五个SDF作为CBF。

---

六、代码框架讲解

最后主要讲下动态环境下其开源代码的运行逻辑，若您对该工程感兴趣，则可以根据讲解快速入手。当然由于本人才疏学浅，可能讲的也有错的地方，请谨慎分辨。

1.加载世界

该世界中包含了静态障碍物和动态障碍物，动态障碍物用的是actor，并在world文件中设定了各自的速度和运动方式。

2.设置坐标系和tf变换

对齐map坐标系和odom坐标系，并添加odom到base_link的tf变换

  <node pkg="gazebo_p3d_utils" type="p3d_link_tf" name="link_odom_to_base_link"  output="log">
    <remap from="~input" to="$(arg gazebo_odom_topic)"/>
    <param name="parent_frame_id" value="odom"/>
    <!-- child frame is base_link setup in jackal.gazebo -->
  </node>

  <node pkg="tf2_ros" type="static_transform_publisher" name="map_odom_transformer"
      args="0 0 0 0 0 0 1 map odom"/>

3.加载jackal机器人和Hector雷达

  <!-- Spawn Jackal -->
  <include file="$(find jackal_gazebo)/launch/spawn_jackal.launch">
    <arg name="x" value="$(arg x)" />
    <arg name="y" value="$(arg y)" />
    <arg name="z" value="$(arg z)" />
    <arg name="yaw" value="$(arg yaw)" />
    <arg name="config" value="front_laser" />
    <arg name="joystick" value="true" />
    <arg name="keyboard" default="false" />
    <arg name="enable_ekf" default="false" />
  </include>

  <!-- slam hector as mapping and localization does not use external odom-->
  <include file="$(find jackal_gazebo)/launch/jackal_hector_map.launch">
    <arg name="scan_topic" value="$(arg scan_topic)" />
    <arg name="map_size" value="255" />
    <arg name="map_resolution" value="0.1" />
    <arg name="map_frame" value="$(arg map_frame)" />
    <arg name="map_start_x" value="0.5"/>
    <arg name="map_start_y" value="0.5"/>
    <arg name="map_pub_period" value="0.02"/>
  </include>

4.加载A*算法节点

输入目标位置/move_base_simple/goal和当前位置/gazebo_p3d/odom，返回计算得到的参考路径\path

  <node name="erl_astar_ros" pkg="erl_astar_ros" type="erl_astar_2d_online_node" output="log" launch-prefix="">
    <param name="inflation_value" value="0.25"/>
    <param name="planning_frame" value="$(arg map_frame)" />
    <param name="plan_freq" value="40.0"/>
    <param name="odom_topic" value="$(arg ctrl_odom_topic)"/>
    <param name="path_topic" value="/path"/>
    <param name="map_topic" value="/map"/>
    <param name="goal_topic" value="/move_base_simple/goal"/>
  </node>

5.加载reference governor节点

输入由A*生成\path，输出一系列路标点/ref_gvn_lite/local_goal

  <!--- Run reference governor(output desired robot states) -->
  <node pkg="ref_gvn_lite" type="ref_gvn_lite_node.py" name="ref_gvn_lite" output="log">
    <param name="display_nav_path" value="true"/>
    <param name="odom_topic" value="$(arg ctrl_odom_topic)"/>
    <param name="path_topic" value="/path"/>
    <param name="radius_topic" value="/sdf_val"/>
    <param name="gvn_restart_topic" value="/gvn_restart"/>
    <param name="ctrl_freq" value="50.0"/>
    <param name="kg" value="2.0"/>
  </node>

6.加载控制器节点

设置移动障碍物的速度和角速度限制，加载局部控制器节点（即CLF-DR-CBF）
具体QP的构建和求解可在clf_cbf_drccp_dynamic_controller中见

    <node pkg="erl_clf_cbf_controller" type="clf_cbf_controller_node.py" name="clf_cbf_controller"  output="log">
      <param name="odom_topic" value="$(arg ctrl_odom_topic)"/>
      <param name="path_topic" value="/path"/>
      <param name="goal_topic" value="/ref_gvn_lite/local_goal"/>
      <param name="ctrl_freq" value="50.0"/>

7.启动rviz仿真

<!--- Run rviz using config inside package -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find erl_clf_cbf_controller)/rviz/test_cone_controller_p3d.rviz" />
  <node name="plt_llc" pkg="rqt_multiplot" type="rqt_multiplot" args="--multiplot-config $(find erl_clf_cbf_controller)/rqt_config/clf_cbf_rqt.xml --multiplot-run-all"/>

最后给出rqt_gragh方便读者理解