Autonomous Driving

• Autonomous Driving
  • Perception
  • Decision-making
    • Architecture
      • Pre-trip decisions
      • In-trip decisions
      • Post-trip decision
  • Speed tracking
    • Dynamic system
      • Open-loop control
      • Closed-loop control
        • Linear controller
    • LTI system
      • Control of LTI system
  • Speed tracking
  • Trajectory tracking
    • Control policy

Perception

• Navigation tool
• Camera
• Radar
• LiDAR
• Infrared detector
• Speedometer
• veh-veh communication
• veh-infrastructure communication

Decision-making

• departure time
• good route
• good lane
• driving style
• way to accel/decel
• make a turn
• park location

Architecture

• Pre-trip

  trip generation, modal choice, routine choice

• In-trip

  Path planning, Maneuver regulation

• Post-trip

  Parking

Pre-trip decisions

• Centralized

  central command center send instructions
  Some win, some lose

• Decentralized

  plan trip independently, selfish
  little communication
  Sometime efficient, sometime inefficient

In-trip decisions

• offline info

  static, in form of map

• online info

  enable the route to be adapted to changing traffic conditions

Post-trip decision

Speed tracking

Dynamic system

• State Variable

  $v[t]$

• State space

  $R>= 0$

• Control input

  $u[t]$(accel)

• System dynamic

  $v[t+dt] = v[t]+u[t]*dt$

Open-loop control

Specify the control input $u[t]$ for all t in $[0,T]$
not a good idea for autonomous driving.

In practice, we need to consider the uncertainty of the system.
We should have:
$$
v[t+dt]=v[t]+u[t]dt+w(t)
$$
Where w(t) is the noise term capturing the uncertainty of the system.

Closed-loop control

Specify the control input $u[t]$ as a function of the state $v[t]$
able to adapt to the uncertainty of the system.
Compare the state $v[t]$ with the desired state $v_desired[t]$
Essentially, we need a mapping from state to control input
like map speed to acceleration
$$
v[t+dt]=v[t]+mu[v[t]]dt+w(t)
$$

Linear controller

the $u[t]$ has linear relationship with $v[t]$
$$
\mu[v] = k[v-v_{desired}]
$$

LTI system

linear time-invariant

Control of LTI system

exponential convergence is stronger than asymptotic convergent
if LTI system is asymptotic convergent, it is also exponential convergent
$$
x[t+1]=ax[t]+bu[t]
$$

Speed tracking

• given:

  $v_{desired}, \delta$

• determine:

  $u[t]$ $t = 0,1,2$

• state

  $v[t]$
  $v[t+1] = v[t] + u[t]\delta + w[t]$

select $u[t]$ so that $w[t]$ will not accumulate

$u[t]=\mu(v[t]) = kv[t]$

$$
v[t+1] = f(v[t],u[t])
$$

Remember the review question

Trajectory tracking

Overview:

• track means asymptotic convergent

• $x[t]$ and $x_{desired}[t]$

• $\lim_{t\rightarrow\infty}|x[t]-x_{desired}[t] | = 0$

• state of system:$[x[t], v[t]]^T$

• if successful:
  $$
  [x[t], v[t]]^T \rightarrow [x_{desired}[t], v_{desired}[t]]^T
  $$

Control policy

• A control policy is a function
• This function maps a state to a control input
  $$
  u[t] = f(x[t],v[t])
  $$

$$
u[t] = -k_1(x[t]-x_{desired}[t])-k_2(v[t]-v_{desired}[t])
$$

$$
k_1,k_2>0
$$

Aircraft control policy

State Variable

Intuitive

• Aircraft
  • Rotation (in euler angle: x,y,z)
  • Position (In the world coordinate system:x,y,z)
  • Linear velocity (In the world coordinate system: x,y,z)
  • Angular velocity (In the world coordinate system: x,y,z)
  • Acceleration (In the world coordinate system:x,y,z)
• Carrier Vessel Landing Runway
  • Position
  • Rotation (fixed)
  • Velocity (fixed)

Extensive

• Aircraft
  • AoA (angle of attack 攻角)

Decision Variable

• Aircraft
  • Pitch angle (control aircraft’s rotation along x axis)
  • Roll angle (control aircraft’s rotation along z axis)
  • Yaw angle (control aircraft’s rotation along y axis)
  • Throttle (control thrust)

Level 1 (Basic) Policy

Only contain the control over control surface (on control over throttle)

Attitude Control/Maintainance

Pitch（俯仰角） C/M

Function: KeepPitch()

Control the angle of horizontal tail (elevator)

1. First keep the angle of elevator same with AoA(攻角), to avoid the disturbance of the horizontal tail on the attitide of the aircraft.

   1	aircraft.PitchAngle = aircraft.AoA;

   这里的aircraft.PitchAngle指的是飞机升降舵的角度(下片为正)

2. 根据当前俯仰角，俯仰速度，和目标俯仰角，调整升降舵面的角度

   1	aircraft.PitchAngle += 5 * (p - aircraft.Pitch) / Mathf.Exp(10 * aircraft.PitchSpeed * Mathf.Sign(p - aircraft.Pitch));

Other Attitude C/M

与俯仰角类似（分别控制滚转舵面以及偏航舵面），不过不需要第一步

Level 2 Policy

Velocity Direction Control/Maintainance

Gliding Angle (下滑角)

Control the thrust of aircraft to change the angle of gliding

1. 保持飞机的俯仰角不变(5度仰角)

   1	KeepPitch(5f);

2. 计算当前下滑角

   1	float velAngle = 90 - Vector3.Angle(aircraft.rb.velocity, Vector3.up);

3. 根据下滑角更改油门大小

   1	aircraft.Thrust += (p - velAngle)/500f;

Level 3 Policy

Follow Trajectory

Vertical Approaching (竖直平面上接近预定下滑道)

通过调整飞机的下滑角来使飞机在竖直面上接近并维持在理想的下滑道

1. 计算偏差对应的向量

   1	Vector3 approachDirection = Vector3.ProjectOnPlane(route.Destination - transform.position, route.direction);

2. 根据偏差值调整下滑角的大小（-3为目标下滑轨道的下滑角）

   1	ThrustKeepGliding(-3 + Mathf.Clamp(approachDirection.y / 10f, -5,5));

Horizontal Approaching (水平面上接近预定下滑道)

通过调整飞机的偏航舵面角度（进而影响飞机的y轴rotation）来使飞机在水平面上接近并维持在理想的航道上

1. 计算偏差对应的向量

   1 2 3 4 5 6 7 8 9 10 11

   Vector3 approachDirection = Vector3.ProjectOnPlane(route.Destination - transform.position, route.direction); float distance = 0; if (Vector3.Angle(approachDirection, transform.right) < 90) { distance = Vector3.ProjectOnPlane(approachDirection, Vector3.up).magnitude; } else { distance = -Vector3.ProjectOnPlane(approachDirection, Vector3.up).magnitude; }

2. 根据偏差值调整飞机的目标偏航角度（-8为目标下滑轨道的偏航角度）

   1	KeepYaw(-8 + Mathf.Clamp(distance / 10f, -50, 50));

Get the ideal trajectory（补充）

基本的策略以及阐述完毕，那么所谓的理想下滑道到底应该怎么得到呢？
理想下滑道是一个射线，包含一个终点（飞机着陆的终点）和一个方向（飞机接近的方向）

1
2
3
4
5

public class Target_Route
{
	public Vector3 Destination;
	public Vector3 direction;
}

理想航道的偏航角和下滑角已知（分别为-8（平行于着陆跑道）和-3）
通过迭代飞机着陆所需时间来求得下滑道终点的位置

1
2
3
4
5
6
7
8
9
10
11
12

route.Destination = CV.transform.position;
route.direction = new Vector3(-0.14f, -0.05f, 0.99f).normalized;
// 获取飞机速度在下滑道方向上投影的大小
float axialVel = Vector3.Dot(aircraft.rb.velocity, route.direction);

// calculate precise destination
float FlightTime = 0;
for (int i = 0; i < 10; i++)
{
	FlightTime = Vector3.Dot(route.Destination - aircraft.transform.position,route.direction) / axialVel;// 获取预计到达终点需要的时间
	route.Destination = CV.transform.position + 15 * FlightTime * Vector3.forward; // 其中15是航母的速度，Vector3.forward是航母前进方向
}