Only Truth Matters

1. Intro

Feedback control

• Use the model of the system to predict its behavior
• Compute an optimal control input that minimize a given cost function
• Subject to constrainsts

Basic Idea:

• formulate a dynamic optimization problem
• tracking trajectory
• regulating process variable
• minimize energy consumption

Advantage:

• Handle system with nonlinear dynamics and constraints
• explicitly use the model of system
• Handle multivariable system
• Handle time-varying parameters

2. For Car-like Control

Trajectory

1. Intro

Path tracking for car-like robot

Or can be called as vehicle lateral control

Some most common method

• PID control
• Stanley controller, based on cross-track error and heading error
• Pure pursuit, based on current position and a point
• Model predictive control(MPC), use a model of the vehicle dynamics to predict the future behavior, then optimize the control input
• Linear Quadratic Regulator (LQR), use a linearized model to calculate the optimal control input

2. Compare

3. Pure Pursuit

Geometric path tracking controller

Look ahead point: A fixed distance on the reference path ahead of the vehicle

1. 测试流程

1. Windows上用上位机查看数据，确保能获取IMU数据
2. ROS包安装，录制数据
3. 校准

2. ROS

Lidar, IMU ROS package

1

sudo apt install ros-XXX-serial

Fix the port name for IMU

1
2
3
4
5
6
7

echo KERNEL=E"ttyUSB*", ATTRSfldVendor}=="10c4", ATTRStLdProduct)=="ea60",ATTRS(ser1al)=="0003", MODE:="0777", GROUP:="dialout",SYMLINK+="fd1Link_ahrs"' >/etc/udev/rules.d/fdtLink_ahrs.rules

service udey reload

sleep 2

service udev restart

1. Basic

Goal:

• FK
• IK

Math:

• Wheel
• w: angular speed
• v: linear speed
• Frame
• world
• vehicle
• steer
• contact
• Car:
• Yaw: defined by the car pose and world frame
• Heading: defined by the path
• Curvature: the 2nd directvie of path

2. Steering

More freedom joint $\to$ over determined configuration

ICR: instance center of rotation

ALL motion can be considered as a rotation around some point

1. Overview

制造，是人类很早就开始的活动

本书是作者的探险之旅，分享一些经验

主要四部分

• 创造的动机和物理学
• 观察自己的工作方式
• Tolerance，容忍
• 创客空间

2. 为什么？刨根问底

好奇心，相信自己的想法，去关注自己感兴趣的东西

本能？

• 当我们被作品打动，探究的过程其实是在弄清楚为什么它会打动我

3. 列清单

1. What

Steering system are mechanisms that control the direction of movement

Goal: Control direction, maintaining stability and safety

2. Common Type

• Rack and pinion: translate rotation into linear motion

• Recirculating ball steering: worm gear and sector gear, convert rotational motion into linear motion

• Ackermann steering: used in tractors and heavy-duty vehicles, all turn smoothly

1. What

Integrate the measurements from the sensor (e.g. IMU)

2. Why

Obtain a predicted pose and velocity, used to correct the odometry

Advantage:

• more accurate and robust estimation
• Enable real-time operation
• Can handle asynchronous measurements from different sensor

3. How

1. Collect IMU data
2. Pre-integrate IMU data
3. Use Kalman filter to update the state estimation

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49

import numpy as np

class Preintegration:
def __init__(self, gravity):
self.gravity = gravity
self.p_bias = np.zeros((3, 1))
self.v_bias = np.zeros((3, 1))
self.R = np.eye(3)
self.p = np.zeros((3, 1))
self.v = np.zeros((3, 1))

def integrate(self, omega, a, dt):
omega = omega - self.bias[:, np.newaxis]
a = a - self.a_bias[:, np.newaxis]

R_old = self.R
p_old = self.p
v_old = self.v

# update orientation
dR = self._skew(omega) @ R_old
self.R = R_old + dR * dt
self.R = self._orthonormalize(self.R)

# update position and velocity
d_v = self.gravity + self.R @ a
self.v = v_old + d_v * dt
self.p = p_old + v_old * dt + 0.5 * d_v * dt * dt

# update covariance
F = np.block([
[np.zeros((3, 3)), -self._skew(a), -self.R],
[np.zeros((3, 3)), np.zeros((3, 3)), self._skew(omega)],
[np.zeros((3, 3)), np.zeros((3, 3)), np.zeros((3, 3))]
])

Q = np.diag([0.01, 0.01, 0.01, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001])**2
self.cov = F @ self.cov @ F.T + Q * dt

def _skew(self, v):
return np.array([
[0, -v[2], v[1]],
[v[2], 0, -v[0]],
[-v[1], v[0], 0]
])

def _orthonormalize(self, R):
u, _, vh = np.linalg.svd(R)
return u @ vh

1. What

Lidar SLAM coupled with IMU and GPS (optional)

2. Components

• ImuPreintergration
• ImageProjeection
• Feature Extraction
• MapOptimization

Two odometry

• Incremental, use only local matching
• Global, for loop detection and optimization

3. IMU

• Input: raw IMU, lidar odometry output
• Output: local IMU odometry, global IMU odometry

1. Why

CMake is indeed not a modern design for package management

1
2
3
4
5
6

find_package(Boost 1.55 COMPONENTS asio)
list(APPEND INCLUDE_DIRS ${BOOST_INCLUDE_DIRS})
list(APPEND LIBRARIES ${BOOST_LIBRARIES})

include_directories(${INCLUDE_DIRS})
link_libraries(${LIBRARIES})

• lack of structure
• the order of linking is missing

2. Target

Modern CMake is about targets

• executable is target
• library is target

Properties is the

• source file
• compiler option
• Link against

Properties have two domain

1. String

• \n and endl?
• Use \n for general
• endl for flush the output and immediately visible
• using: can be in different level
• Used as alias
• Cout? cerr? Buffer or not

String could be see as a read-only array with zero terminator, so the “Hello” is 6 byte rather than 5

2. Variable

Static type

Variable could be declared or assigned multiple times, but only define once

• Heap? Stack?
• Stack for local varariable and function call frame
• Heap for dynamic data

define array

1

int a[]={1,2,3,...}

• size_t: represent the maximum size of any object
• float should be noted as 0.f as example
• byte 8 bit, unsigned char 8 bit, but the byte can only take bitwise operation
• :: would use the global varaiable
• thread_local and static
• use const