智能车

Chapter 1

Definition of the Smart Vehicle

1. Perception
2. Autonomous Decision : brain of the vehicle (autonomously analyze, learn and decide)
3. Automatic Control

Different Levels of Smart Vehicles

1. L1 - driving assistance
2. L2 - partially-automated
3. L3 - conditional-automated
4. L4 - highly-automated
5. L5 - fully-automated

Perception

Sensor

1. Radar（超声波雷达）
2. LiDAR（激光雷达）
3. RGB Camera
4. GPS, IMU

Dataset Preparation

1. image labeling tools
2. road analysis tool

Control

1. Interactive Gaming(博弈) Algorithms

2. Protection Algorithms of In-Vehicle Network

   1. ECU(electronic control unit 电子控制单元) Authentication
   2. Encryption communication（数据加密）

3. Human Machine Interaction（人机交互）

4. Automatic Control（自动控制）

V2X Communication

1. V2I (infrastructure)
2. V2V (vehicle)
3. V2P (pedestrian)

Energy Efficient Computing

1. Asynchronous Circuits (异步电路)

   Combi Block1 -> Handshaking -> Combi Block2 -> ...

   通过 Handshaking电路实现异步数据传输

2. Synchronous Circuits (同步电路)

   时序逻辑电路，通过 Clock时钟信号实现同步数据计算

Power Consumption

1. Dynamic power dissipation

   $P_{dynamic} = \alpha * C_{phy} * V_{dd}^2 * f_{clk}$

2. Leakage Power Mechanism

Compute Circuit

Simplified FPGA Architecture

1. Functional Block (逻辑门)
2. I/O Block (输入输出信号)
3. Routing Network (导线)

Digital Systems Implement Computations on Platforms

1. Graph

   1. Data flow graph (DFG)
   2. Control data flow graph (CDFG)

2. Clock cycles

Hardware Acceleration Design Flow

1. Fixed-Point Data Type Refinement (float-point -> fixed-point)

   Matlab has a useful tool.

2. Logic Optimization

3. Datapath and Controller Generation

4. FPGA Technology mapping

Chapter 2

Sensor Performance

1. Range
2. Resolution
3. Bandwidth of Frequency
4. Sensitivity

Types of error

1. Systematic error
2. Random error

Range Sensor

Stereo Camera

Drawback : short range

1. Intrinsic calibration
2. Extrinsic calibration

Time of flight

GPS (Global Positioning System)

• GPS coordinate errors
  • Wrong survey (人为测量温差)
  • Continental drift (大陆漂移)

IMU

• Gray Encoder (格雷编码，用于测量旋转角度)

• Magnetic compass (磁力计，测量磁场方向)

• Gyroscope (陀螺仪)

  • Mechanical gyroscope
  • Optical gyroscope

• Accelerator (加速度计)

• IMU (Inertial Measurement Unit)

  • IMU Error and Drift

Synchronization & Calibration

Camera Calibration

• ROS Image Processing

• Right Hand Coordinate System

• Tools & Tutorials

  • Camera to Camera (Stereo Camera)
  • Camera to LiDAR
  • Camera to IMU
  • Multi-Sensor Calibration

HD Maps

• Path Planning
• Traffic Rules and Constraints

PCL (Point Cloud Library)

• K-d tree

  对于 $d$ 维的数据，利用分割轴对数据进行划分，每次将分割轴上值的中位数作为新的分割轴来分割数据

• NDT (Normal Distributions Transform)

• Quad-tree (四叉树)

• Oct-tree (八叉树)

Vector Maps

• OSM (Open-source-map)

Map

• Sematic Map

• Topologic Map

  vertices and edges with coordinates

• Voronoi Diagram

• Geographic Information System (GIS)

NDS (Navigation Data Standard)

The worldwide standard for map data in automotive eco-systems

• Road Network Layer

Odometry (里程计)

• Non-Gaussian Error
• odometry drift

Aligning 3D Data (三维点云配准)

• ICP (Iterative Closest Point, 迭代最近点法)

  对两个长度相同的三维点集进行配准

• Horn's Method

Semantic SLAM

Chapter 3

3D Representations

• Point Cloud
• Mesh
• Volumetric (类比图像的表示，使用三维坐标表示三维的点云，利用Conv3D进行计算)
• Implicit representation (NeRF, 使用神经网络的参数来表示点云)
• 3D gaussian

Point Clouds Feature Extraction

Challenge

• unordered point set as input (与点的顺序无关)
• invariance under geometric transformations (与位移变换无关)

PointNet

• T-Net
• MLP
• Global Max Pooling
• Local Features + Global Features

BEV (Bird's eye view，鸟瞰图)

PointP后illar

Voxel-base (体素特征)

将无序三维点云数据转换为规则体素网格，每个点包含特征：空间中点的位置坐标、局部密度信息、局部结构信息等

PolarNet (基于极坐标BEV进行卷积)

• Ring CNN (环卷积)
• Cylinder 3D

Detection

Task

• pedestrian detection
• traffic sign

Metric

• 预测是否正确	标签为真	标签为假
  预测正确	TP	TN
  预测错误	FP	FN

• $Precision=\frac{TP}{TP+FP}$（在标签为真的所有样本中预测正确的比率）
• $Recall=\frac{TP}{TP+FN}$ （所有预测为真的样本中预测正确的比率）

2D Detection

• DPM: Deformable Part Model

• R-CNN: Region CNN

  最后的分类器使用的SVM

• SPPNet: Spatial Pyramid Pooling

  最后的分类器使用的SVM

• Faster RCNN

• FPN: Feature Pyramid Network(特征金字塔)

• Mask R-CNN

• YOLO (anchor-based)

• RetinaNet

  FPN + Focal loss

• CenterNet (anchor-free)

3D Detection

• 3D-CNN
• PolarPillar
• 3D-SSD
• VoteNet

Evaluation

TSNE: t-Distributed Stochastic Neighbor Embedding (t-SNE)是一种降维技术，用于在二维或三维的低维空间中表示高维数据集，从而使其可视化

将高维数据进行降维可视化

Fusion-based methods

2D、3D特征融合（多模态融合）

• Transformer
• CLIP (Contrastive Language-Image Pre-Training，多模态预训练神经网络)

Voxel-based View Transformation

• LSS

• BEVDet（MLP）

• IPM

  通过数学几何运算进行图像矫正（逆透视）

Transformer-based Methods

End-to-End Model

Chapter 4

The Path Planning Problem

• RRT (Rapidly exploring Random Tree,快速随机树搜索算法)

• Graph Search

  • DFS (Depth first search,深度优先搜索)

  • BFS (Breadth first search,广度优先搜索)

    维护一个队列，将临近的所有点都加入到队列中，然后逐个对出队的点找临近点并入队，重复上述操作
    
    从终点回溯，i的父节点为f，f的父节点为e，e的父节点为a，这样就可以得到a到i的最短路径为：a->e->f->i
    相较于DFS，BFS中使用了大量的入队、出队操作，耗时增加，但是能保证找到最优路径，DFS算法找到的并不一定是最优路径
    

  • Dijkstra

  • GBFS (Greedy best first search,贪婪最佳优先算法)

  • $A^*$ (启发式搜索算法)

    评估函数：$f(n)=g(n)+h(n)$

    $g(n)$: 从起始点到任何节点n的实际路径成本 （$g(n)=g(n-1)+1$）

    $h(n)$: 从起始点到目标节点n的预估成本，是一个启发式估计（与启发函数的选择有关）

    $h(n)$的定义：

    1. 欧几里得距离：$h(n)=\sqrt{(x-x_{goal})^2+(y-y_{goal})^2}$
    2. 曼哈顿距离：$h(n)=|x-x_{goal}|+|y-y_{goal}|$

Trajectory Planning

Kinematics(运动学) vs Kinetics(动力学)

Tire Models

• Differential gear (差速器)
  

Chapter 5

Simulation-Based Testing

• Scenario
• Domain Specific Language

Compiler

• Lexeme

• BNF

• AST Tree

• EBNF

• Propositional Logic (命题逻辑)

  It has a set of symbols, a syntax and a semantics

  • Well-formed formula (wff)

• Predicate Logic (谓词逻辑)

ROS2

Preparation

create workspace and package

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python <pkg_name>  # 创建一个空的package
cd ~/ros2_ws
colcon build [--packages-select <pkg_name>] # 编译

ros2 pkg create

# ros2 pkg create
ros2 pkg create --build-type ament_python <pkg_name> \ # 创建一个空的package
				--dependencies rclpy ...  # 指定依赖

<!-- 在package.xml文件中指定需要引入的依赖 -->
<exec_depend>rclpy</exec_depend>
<exec_depend>std_msgs</exec_depend>

colcon build

colcon build  # 编译所有packages
colcon build [--packages-select <pkg_name1> ...]  # 编译指定packages

CLI Tools

Topic

ros2 topic list [-t]
# list all active topics
# -t : with the message type of the topic

ros2 topic echo <topic_name>
# show the data being published on the topic

ros2 topic info <topic_name>
# show the detail information of the topic <topic_name>

ros2 topic pub [--once] [-w 1] <topic_name> <msg_type> '<args>'
# publish the message '<args>' of <msg_type> to <topic_name>
# --once : publish once
# -w N : 表示匹配多个subscription节点

ros2 topic hz <topic_name>
# show the detail information of the topic <topic_name>

ros2 interface show <msg_type>
# see the details of the message <msg_type>
# ex: <var_type> <var_name>

Service

ros2 service list [-t]
# show all active services
# -t: with the type of all services

ros2 service type <srv_name>
# show the type of the serive <srv_name>

ros2 service list
# show all active services

ros2 service find <srv_type>
# find the services which are the type of <srv_type>

ros2 interface show <srv_type>
# show the details of the service <srv_type>
# ex: 
# <request_srv_detials>
# ---
# <response_srv_details>

ros2 service call <srv_name> <srv_type> <arguments>
# call the service <srv_name> with arguments

Parameters

A parameter is a configuration value of a node. You can think of parameters as node settings. A node can store parameters as integers, floats, booleans, strings, and lists. In ROS 2, each node maintains its own parameters.

ros2 param list
# show all parameters of all nodes(packages)

ros2 param get <node_name> <parameter_name>

ros2 param set <node_name> <parameter_name> <value>

ros2 param dump <node_name> <output_yaml_file_path>

ros2 param load <node_name> <input_yaml_file_path>

Bag

Convert ROS1 bag to ROS2 bag

pip install rosbags
rosbags-convert --src <ros1-bag> --dst <ros2-bag>  # or rosbags-convert --src <ros2-bag> --dst <ros1-bag>

evo

用于评估SLAM输出的轨迹的精度，可以自动生成均值、方差、轨迹等信息的图或者表

evo_traj

evo_traj tum ... [-p]
evo_traj kitti ...
# -p|--plot: 输出轨迹的可视化图形

Python

publisher and subscriber

1. 创建Publisher节点文件

   import rclpy
   from rclpy.node import Node
   from std_msgs.msg import String, Int32
   # from tutorial_interfaces.msg import Num  # custom interface
   import time
    
   class MyPuberNode(Node):
       def __init__(self):
           super().__init__('my_puber_node')
           self.pub = self.create_publisher(Int32, 'topic', 10)
           self.timer = self.create_timer(1, self.timer_callback)
           self.i = 0
    
       def timer_callback(self):
           msg = Int32()
           msg.num = self.i
           self.pub.publish(msg)
           # self.get_logger().info(f'Publishing: timestamp={time.time()})')  # 发布时间戳
           self.get_logger().info(f'Publishing: Int32({msg.data})')  # 发布Int32类型数据
           self.i += 1
    
   def main(args=None):
       rclpy.init(args=args)
       puber = MyPuberNode()
       rclpy.spin(puber)
       puber.destroy_node()
       rclpy.shutdown()

2. 创建Subscriber节点文件

   import rclpy
   from rclpy.node import Node
   from std_msgs.msg import String, Int32
   # from tutorial_interfaces.msg import Num
   import time
    
   class MySuberNode(Node):
       def __init__(self):
           super().__init__('my_suber_node')
           self.sub = self.create_subscription(Int32, 'topic', self.listener_callback, 10)
           self.sub
       def listener_callback(self, msg):
           self.get_logger().info(f'Subscribing: {msg.data}')
    
   def main(args=None):
       rclpy.init(args=args)
       suber = MySuberNode()
       rclpy.spin(suber)
       suber.destroy_node()
       rclpy.shutdown()

3. 修改 setup.py中的入口点

   entry_points={
       'console_scripts': [
           'puber = py_pubsub.test_puber:main',
           'suber = py_pubsub.test_suber:main'
       ],
   },

4. 编译

   cd ~/ros2_ws
   colcon build [--packages-select py_pub_sub]

5. 运行

   # 终端1
   source install/setup.bash
   ros2 run py_pubsub puber

   # 终端2
   source install/setup.bash
   ros2 run py_pubsub suber

函数详解：

self.create_publisher(  # 创建发布节点
	<msg_type>,  # std_msgs.msg, 结点发布消息的类型
    <topic_name>,  # str，topic的名称
    [<callback_function>],  # function, callback函数
    <queue_size>  # int, 消息存储队列大小（防止subscriber节点未接收到消息）
)
 
self.create_subscriber(  # 创建订阅节点
	<msg_type>,  # std_msgs.msg, 结点发布消息的类型
    'topic',  # 
    <listener_callback_function>,  # [required] function, callback函数
    <queue_size>  # int, 消息存储队列大小
)
 
self.create_timer(  # 创建计时器
	<time_period>,  # float, 时间间隔
    <callback_function>  # function, 回调函数
)

# main function
def main(args=None):
    rclpy.init(args=args)
    suber = MySuberNode()  # 创建节点实例
    rclpy.spin(suber)  # 循环执行节点
    suber.destroy_node()  # destroy node
    rclpy.shutdown()  # shutdown ros2 server

service and client

1. 创建Service节点

   import rclpy
   from rclpy.node import Node
   from example_interfaces.srv import AddTwoInts
    
   class MyServiceNode(Node):
       def __init__(self):
           super().__init__('my_service_node')
           self.srv = self.create_service(AddTwoInts, 'add_two_ints', self.add_two_ints_callback)
    
       def add_two_ints_callback(self, req, resp):
           resp.sum = req.a + req.b
           self.get_logger().info(f'Request:\na: {req.a}, b: {req.b}')
           return resp
    
   def main(args=None):
       rclpy.init(args=args)
       srv = MyServiceNode()
       rclpy.spin(srv)
       srv.destroy_node()
       rclpy.shutdown()

2. 创建Client节点

   import rclpy
   from rclpy.node import Node
   from example_interfaces.srv import AddTwoInts
    
   class MyClientNode(Node):
       def __init__(self):
           super().__init__('my_client_node')
           self.client = self.create_client(AddTwoInts, 'add_two_ints')
           self.req = AddTwoInts.Request()
    
       def send_request(self, a, b):
           self.req.a = a
           self.req.b = b
           self.future = self.client.call_async(self.req)
           rclpy.spin_until_future_complete(self, self.future)
           return self.future.result()
    
   def main(args=None):
       rclpy.init(args=args)
       client = MyClientNode()
       resp = client.send_request(40, 20)
       client.get_logger().info(f'Result of add_two_ints: {client.req.a} + {client.req.b} = {resp.sum}')
       client.destroy_node()
       rclpy.shutdown()

3. 修改 setup.py中的入口点

4. 编译并运行

函数详解：

self.create_service(  # 创建service节点（接受client发送的请求，处理请求后返回响应）
	<srv_type>,  # srv, 自定义Service的类型
    <srv_name>,  # str，自定义Service名称
    <callback_function>  # function, callback函数
)
 
self.create_client(  # 创建client节点
	<srv_type>,  # srv, 自定义Service的类型
    <srv_name>  # str，自定义Service名称
)
self.client.wait_for_service(timeout_sec=1.)
 

custom msg and srv

It's better to use C++ to custom interface!

1. create new package

   ros2 pkg create --build-type ament_cmake custom_interface

2. create the directories of msg and srv

3. msg definition

   # Num.msg
   int64 num
    
   # Sphere.msg
   geometry_msgs/Point center  # include package geometry_msgs
   float64 radius

4. srv definition

   # AddThreeInts.srv
   int64 a
   int64 b
   int64 c
   ---
   int64 sum

5. CMakeLists.txt

   find_package(geometry_msgs REQUIRED)
   find_package(rosidl_default_generators REQUIRED)
    
   rosidl_generate_interfaces(${PROJECT_NAME}
     "msg/Num.msg"
     "msg/Sphere.msg"
     "srv/AddThreeInts.srv"
     DEPENDENCIES geometry_msgs # Add packages that above messages depend on, in this case geometry_msgs for Sphere.msg
   )

6. package.xml

   <depend>geometry_msgs</depend>
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
   <member_of_group>rosidl_interface_packages</member_of_group>

7. Build the package

   colcon build --packages-select custom_interface

8. Test

   # source the env first
   ros2 interface show custom_interface/msg/Sphere
   ros2 interface show custom_interface/srv/AddThreeInts

9. Implementing custom interfaces

   <!-- Python package.xml -->
   <depend>custom_interface</depend> <!-- 导入自定义的包 -->

parameters

C++

publisher and subscriber

1. Publisher

   #include <chrono>
   #include <memory>
   #include <string>
   #include "rclcpp/rclcpp.hpp"
   #include "std_msgs/msg/string.hpp"
    
   using namespace std::chrono_literals;
    
   class MyPuberCpp : public rclcpp::Node {
    
   private:
       size_t count;
       rclcpp::TimerBase::SharedPtr timer;
       rclcpp::Publisher<std_msgs::msg::String>::SharedPtr puber;
    
    
   public:
       MyPuberCpp() : Node("my_puber_cpp"), count(0) {
           puber = this->create_publisher<std_msgs::msg::String>("topic", 10);
           auto timer_callback = [this]() -> void {
               auto message = std_msgs::msg::String();
               message.data = "Hello ROS2: " + std::to_string(this->count++);
               RCLCPP_INFO(this->get_logger(), "Publishing: %s", message.data.c_str());
               this->puber->publish(message);
           };
           timer = this->create_wall_timer(500ms, timer_callback);
       }
   };
    
   int main(int argc, char* argv[]) {
       rclcpp::init(argc, argv);
       rclcpp::spin(std::make_shared<MyPuberCpp>());
       rclcpp::shutdown();
       return 0;
   }

2. Subscriber

   #include <chrono>
   #include <memory>
   #include <string>
   #include "rclcpp/rclcpp.hpp"
   #include "std_msgs/msg/string.hpp"
    
   using namespace std::chrono_literals;
    
   class MySuberCpp : public rclcpp::Node {
    
   private:
       size_t count;
       rclcpp::Subscription<std_msgs::msg::String>::SharedPtr suber;
    
   public:
       MySuberCpp() : Node("my_suber_cpp"), count(0) {
           auto topic_callback = [this](std_msgs::msg::String::UniquePtr msg) -> void {
               RCLCPP_INFO(this->get_logger(), "Subscribing: %s", msg->data.c_str());
           };
           suber = this->create_subscription<std_msgs::msg::String>("topic", 10, topic_callback);
       }
   };
    
   int main(int argc, char* argv[]) {
       rclcpp::init(argc, argv);
       rclcpp::spin(std::make_shared<MySuberCpp>());
       rclcpp::shutdown();
       return 0;
   }

Homework

HW1

完成表中2020-2023年的数据搜集

数据参考：

https://www.dmv.ca.gov/portal/vehicle-industry-services/autonomous-vehicles/disengagement-reports/

HW2

完成车道线检测的代码

Project

评估

• Relative Translation Error (RTE)