PAMPC: Perception-Aware Model Predictive Control for Quadrotors 리뷰

신희준·2024년 11월 27일
0

Paper Review

목록 보기
29/30

Paper : [PAMPC: Perception-Aware Model Predictive Control for Quadrotors] (https://arxiv.org/abs/1804.04811 (Davide Falanga, Philipp Foehn, Peng Lu, Davide Scaramuzza / IEEE IROS 2018)

code: https://github.com/uzh-rpg/rpg_mpc

video : https://www.youtube.com/watch?v=9vaj829vE18

Highlights

  • Perception-Aware Model Predictive Controller를 제안
  • Action과 Perception objectives에 관한 control과 planning을 unify → 즉, MPC에 camera sensing에 관련된 objective를 추가했다고 볼 수 있겠음 → 여기서 objectives라 함은
    • Quadrotor system dynamics and input limits를 고려
    • Image plane에서 point of interest의 visibility를 향상
    • Point of interest의 projected velocity를 감소 (이미지 상에서 POI의 속도를 최소화)

→ 이를 통해 생성된 trajectory는 POI를 image centre에 잘 유지할 수 있음

  • Onboard low-power ARM computer에서 real-time으로 잘 작동

Introduction

  • Perception algorithm의 발전으로, 카메라를 활용한 vision-based 조종이 빠르게 발전
  • 하지만 vision-based perception은 몇 가지 한계가 있음
    • texture distribution이나 light condition과 같은 환경에 크게 영향을 받음
    • robot motion에도 영향을 많이 받음 (motion blur, camera pointing direction, 등)

→ perception과 action이 항상 함께 고려되어야만 함.

  • 예를 들어, 어떤 좁은 통로를 지나기 위해 camera sensor로 localization하기 위해서는 이 통로가 계속해서 보일 수 있도록 action을 취해줘야 함.

MPC 간단 정리

  • MPC는 미래의 short time horizon에 대한 vehicle dynamics를 예측 → 이를 통해 MPC는 expected motion을 알고 있기 때문에 iterative하게 reference (reference pose or dynamic trajectory)에 fitting

  • 이를 위해 MPC는 quadrotor의 dynamic model과 optimization process를 필요로 함

  • 일반적인 full iteration에서 MPC는 다음과 같은 process를 진행

    1. Initial state (last estimated state)로부터 future movement를 예측
    2. Optimization process에서 사용되는 cost gradient를 계산 (the Hessian Matrix)
    3. Optimization process를 시작 : reference, cost, last prediction, constraints, gradients를 이용
    4. Solution을 계산 : 이는 optimum으로의 converge되는 값들. 하지만 이는 non-linear system의 linearization으로 인해서 아직 optimal한 값은 아님
    5. 위 과정을 반복.

  • optimization이 constraints들을 다룰 수 있기 때문에, actuator limitation과 같은 외부 요인들을 다룰 수 있게 됌 → classical controller에 비해서 갖는 장점

  • 또한 vehicle의 미래 움직임을 예측할 수 있기 때문에 여러 cost function을 design하는 것이 편리함

Contributions

  • Action과 Perception objective를 동시에 최적화하는 MPC 알고리즘 제안 → 이를 통해 생성된 trajectory는 robot dynamic을 만족시키면서 POI의 visibility를 보장

  • Action objective와 Perception objective의 potential conflict를 numerical optimization으로 해결

    • perception objective를 constraint가 아닌 optimization component로 여김
    • perception을 cost function으로 합치는 것이 computational complexity를 줄여줌

  • 또한, 이러한 방법을 통해 vision-based localization, target tracking 등의 성능을 높일 수 있다.

    • land mark visibility가 높아지고, image plane velocity가 낮아지기 때문

Problem Formulation

  • 자율로봇항법을 위해서는 두 가지 요소가 필수적

    • Perception : ego-motion과 surrounding environment
    • Action : combination of motion planning and control algorithm

  • 이때 perception과 action의 coupling은 상당히 중요함

    • 안전한 비행을 위해서 정확하고 robust한 perception이 필수적인데, vision-based perception의 quality는 camera motion에 많이 영향을 받음
    • 또한 image로부터 충분한 정보를 추출할 수 있도록 해줘야 적절한 action을 취할 수 있음

  • 시스템은 아래와 같이 정의할 수 있음

    • xx와 uu는 robot의 state와 input vector라고 하자.
    • 로봇의 dynamic은 미분방정식으로 표현 x˙=f(x,u)\dot{x}=f(x,u)
    • zz는 perception system의 state vector (e.g., image plane으로의 3D point projection)
    • σ\sigma는 perception 특성 parameter의 vector (e.g., camera의 focal length나 FOV)

  • perception state와 robot state는 robot dynamic를 통해 coupled

z=fp(x,u,σ)z=f_p(x,u,\sigma)
  • 특정 action objective가 주어진 상황에서 action cost La(x,u)\mathcal{L}_a(x,u)와 perception cost Lp(z)\mathcal{L}_p(z)를 정의할 수 있고, 이를 통해 최적화 문제로 이 coupling을 공식화할 수 있음
minu∫t0tfLa(x,u)+Lp(z)dtsubjected  to  r(x,u,z)=0,  h(x,u,z)≤0min_u \int^{t_f}_{t_0} \mathcal{L}_a(x,u)+\mathcal{L}_p(z)dt \\ subjected \; to \; r(x,u,z)=0, \; h(x,u,z) \leq 0
  • rr과 hh는 perception, action이 동시에 만족해야하는 제약 조건을 의미

Methodology

  • robot의 navigation을 위한 computer vision 알고리즘은 두 가지 기본적인 requirements가 존재

    • 알고리즘에 사용될 Point of interest가 이미지에 항상 보여야한다. 예를 들어 visual odometry알고리즘의 pose estimation을 위해서 landmark와 같은 point들이 필요

    • 그런 point of interest들이 image에서 명확하게 recognizable해야함. camera의 motion이나 scene까지의 거리로 인해 motion blur 등이 생길 수 있음

      → 그러므로 camera의 motion은 robust한 visual perception을 보장할 수 있도록 계획되어야 함

  • 이러한 고려를 바탕으로 이 연구에서는 perception object로써 다음의 두 가지를 고려

    • visibility of point of interest
    • minimization of the velocity of their projection onto the image plane

  • 이를 위해 quadrotor에 장착된 카메라의 motion과 3d 공간의 point의 image plane으로 projection 사이의 관계를 파악해야함

    → quadrotor의 state와 input vector의 함수를 통해 image plane으로의 projection dynamic을 표현하여, perception과 action을 하나의 optimization으로 couple

Nomenclature

  • 먼저, world frame WW, quadrotor frame BB, camera frame CC를 정의

  • 벡터의 표현의 경우, WPWB_WP_{WB}는 body frame BB를 world frame에 대한 position을 world frame에서 표현한 것

  • 강체의 orientation을 표현하기 위해서는 quaternion 사용

  • 쿼터니언 q=(qw,qx,qy,qz)Tq=(q_w,q_x,q_y,q_z)^T의 시간 미분은 아래와 같이 정의

q˙=12Λ(w)⋅q\dot{q}=\frac{1}{2}\Lambda(w) \cdot q
  • 벡터 w=(wx,wy,wz)Tw=(w_x,w_y,w_z)^T의 skew-symmetric matrix Λ(w)\Lambda(w)는 아래와 같이 표현
Λ(ω)=[0−ωx−ωy−ωzωx0ωz−ωyωy−ωz0ωxωzωy−ωx0]\Lambda(\omega) = \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix}
  • operator ⊙\odot은 quaternion과 vector의 곱으로 사용. → 이는 벡터 vv를 quaternion qq만큼 회전시키는 것을 의미 즉, 회전한 새 벡터 v′v'는 아래와 같이 구할 수 있음
v′=v⊙q=Qvv'=v \odot q=Qv
Q=[1−2qy2−2qz22(qxqy+qwqz)2(qxqz−qwqy)2(qxqy−qwqz)1−2qx2−2qz22(qyqz+qwqx)2(qxqz+qwqy)2(qyqz−qwqx)1−2qx2−2qy2]Q = \begin{bmatrix} 1 - 2q_y^2 - 2q_z^2 & 2(q_xq_y + q_wq_z) & 2(q_xq_z - q_wq_y) \\ 2(q_xq_y - q_wq_z) & 1 - 2q_x^2 - 2q_z^2 & 2(q_yq_z + q_wq_x) \\ 2(q_xq_z + q_wq_y) & 2(q_yq_z - q_wq_x) & 1 - 2q_x^2 - 2q_y^2 \end{bmatrix}

Quadrotor Dynamics

  • world frame에서 표현된 body의 position과 orientation
pWB=(px,py,pz),  qWB=(qw,qx,qy,qz)Tp_{WB} = (p_x,p_y,p_z), \; q_{WB}=(q_w,q_x,q_y,q_z)^T
  • world frame에서 표현된 body의 linear velocity
vWB=(vx,vy,vz)Tv_{WB}=(v_x,v_y,v_z)^T
  • body frame에서 표현된 angular velocity
ΩB=(wx,wy,wz)T\Omega_B=(w_x,w_y,w_z)^T
  • mass-normalized thrust vector (fif_i는 i번째 모터의 출력)
c=(0,0,c)Tc=(f1+f2+f3+f4)/mc=(0,0,c)^T \\ c= (f_1+f_2+f_3+f_4)/m
  • 그러면 아래와 같이 quadrotor의 dynamic model을 정의할 수 있음
p˙WB=vWBv˙WB=wg+qWB⊙cq˙WB=12Λ(ΩB)⋅qWB\dot{p}_{WB}=v_{WB} \\ \dot{v}_{WB}=wg+q_{WB} \odot c \\ \dot{q}_{WB} = \frac{1}{2} \Lambda(\Omega_B) \cdot q_{WB}
  • state vector와 input vector는 다음과 같음
x=[pWB,vWB,qWB]T,  u=[c,ΩBT]Tx=[p_{WB},v_{WB}, q_{WB}]^T ,\; u=[c, \Omega^T_B]^T

Perception Objectives

  • Point of interest (landmark)의 3D 위치를 world frame에서 표현한 점
Wpf=(Wpfx,Wpfy,Wpfz)_Wp_f=(_Wp_{fx}, _Wp_{fy}, _Wp_{fz})
  • constant rigid body transformation (camera의 extrinsic parameter)
TBC=[pBC,qBC]T_{BC}=[p_{BC},q_{BC}]
  • Wpf_Wp_f는 camera frame에서 표현할 수 있다.
Cpf=(qWBqBC)−1⊙(Wpf−(qWB⊙pBC+pWB))_Cp_f=(q_{WB}q_{BC})^{-1}\odot (_Wp_f-(q_{WB}\odot p_{BC}+p_{WB}))
  • camera frame에서의 점 Cpf_Cp_f은 pinhole camera model에 의해 image coordinate s=(u,v)Ts=(u,v)^T로 projection 가능
u=fxCpfxCpfz,  v=fyCpfyCpfxu=f_x\frac{_Cp_{fx}}{_Cp_{fz}}, \; v=f_y\frac{_Cp_{fy}}{_Cp_{fx}}

  • Robust한 vision-based perception을 위해서는 POI wpf_wp_f의 projection ss가 최대한 이미지의 center에 위치해야한다.

    • 외란에 대해 높은 safety margin을 가질 수 있기 때문
    • 이미지의 외각에서 보통 많은 왜곡이 발생

  • POI가 이미지에서 계속 보이게 하기 위해서, 이들의 image plane으로의 projection들의 velocity가 줄어야 한다.

  • POI가 정적이라고 가정했을 때, projection들의 속도를 quadrotor의 state와 input vector로 표현할 수 있다. 이를 위해서 ss를 시간에 대해 미분한다.

u˙=fxCp˙fxCpfz−CpfxCp˙fzCpfz2\dot{u}=f_x\frac{_C\dot{p}_{fx} {_C}p_{fz}-_Cp_{fx} {_C}\dot{p}_{fz}}{_Cp^2_{fz}}
v˙=fyCp˙fyCpfz−CpfyCp˙fzCpfz2\dot{v}=f_y\frac{_C\dot{p}_{fy} {_C}p_{fz}-_Cp_{fy} {_C}\dot{p}_{fz}}{_Cp^2_{fz}}
  • 정리하면
s˙=[u˙v˙0]=[0−fxCpfz20fyCpfz200000](Cpf×Cp˙f)\dot{s} = \begin{bmatrix} \dot{u} \\ \dot{v} \\ 0 \end{bmatrix} = \begin{bmatrix} 0 & -\frac{f_x}{_Cp^2_{fz}} & 0 \\ \frac{f_y}{_Cp^2_{fz}} & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix} (_Cp_f \times _C\dot{p}_f)
  • Cp˙f_C\dot{p}_f를 계산하기 위해서는 Cpf_Cp_f를 시간에 대해 미분
Cp˙f=−12Λ(ΩC)Cpf−CvWC_C\dot{p}_f = -\frac{1}{2} \Lambda(\Omega_C) _Cp_f - _Cv_{WC}
CvWC=(qWBqBC)−1⊙[12Λ(ΩB)qWB⊙pBC+vWB],ΩC=qBC−1⊙ΩB._Cv_{WC} = (q_{WB} q_{BC})^{-1} \odot \left[ \frac{1}{2} \Lambda(\Omega_B) q_{W B} \odot p_{B C} + v_{W B} \right], \\ \Omega_C = q_{B C}^{-1} \odot \Omega_B.

Action Objectives

  • Quadrotor가 3d space에서 원하는 위치에 도달하게 하기 위해서는 적절한 trajectory 계획이 필요한데, 이를 위해서 두 가지의 objective가 고려되어야 함
    • System의 available bounded inputs : motor의 thrust가 upper & lower bound를 지켜야 한다. → limited input vector uu
    • Underactuated nature of a quadrotor : control inputs (thrust, roll, pitch, yaw)가 독립적으로 모든 degree of freedom을 가지고 control할 수 없다.

Challenges

  • Perception과 Action을 동시에 고려하는 것은 conflict가 발생할 수 있기 때문에 challenging.

  • 예를 들어, quadrotor가 reference trajectory를 track하기 위해서는 원하는 acceleration의 방향으로의 추력을 주기 위해 rotate해야하지만, perception objective가 POI의 visibility를 최대화하기 위해 그러한 rotation을 minimize해야할 수도 있다.

  • 그러므로 system dynamics를 통해 이 둘을 coupling시켜 둘 다 고려할 수 있는 model-based optimization framework가 필요하다.

Model Predictive Control

  • Perception과 action의 coupled된 문제를 optimization 문제로 푸는 것은 기존의 classical control (PID)에 비해서 몇 가지 장점을 가진다.

    1. System dynamics를 고려할 수 있다. (underactuated dynamics)
    2. Input constraint를 고려할 수 있다. (Input bound)
    3. Dynamic한 trajectory를 생성할 수 있다.
    4. Future cost prediction을 고려하여 최소화할 수 있다.

  • 이러한 optimization의 기본적인 공식은 위에서 언급했듯이 아래와 같으며,

minu∫t0tfLa(x,u)+Lp(z)dtsubjected  to  r(x,u,z)=0,  h(x,u,z)≤0min_u \int^{t_f}_{t_0} \mathcal{L}_a(x,u)+\mathcal{L}_p(z)dt \\ subjected \; to \; r(x,u,z)=0, \; h(x,u,z) \leq 0

  • 이러한 quadratic cost를 가지는 non-linear program은 sequential quadratic program (SQP)에 의해 근사될 수 있다. (iterative approximation)

  • 이를 위해서 system dynamics를 time horizon tht_h안에서 time step dtdt로 discretize

xi  ∀i∈[1,N],  ui  ∀i∈[1,N−1]x_i \; \forall i \in [1,N], \; u_i \; \forall i \in [1,N-1]

  • time-varying state cost matrix Qx,i  ∀i∈[1,N]Q_{x,i} \; \forall i \in [1,N] 과 time-varying perception, input cost matrixs를 정의 Qp,i,Ri    ∀i∈[1,N−1]Q_{p,i}, R_i \; \; \forall i \in [1,N-1]. 최종적으로 perception function z=[s,s˙]z=[s,\dot{s}]를 정의

    • 여기서 zz는 quadrotor의 state와 input variable의 함수

  • 그러면 cost function은 아래와 같이 쓸 수 있다.

L=xˉN⊤Qx,NxˉN+∑i=1N−1(xˉi⊤Qx,ixˉi+zˉi⊤Qp,izˉi+uˉi⊤Riuˉi),L = \bar{x}_N^\top Q_{x,N} \bar{x}_N + \sum_{i=1}^{N-1} \left( \bar{x}_i^\top Q_{x,i} \bar{x}_i + \bar{z}_i^\top Q_{p,i} \bar{z}_i + \bar{u}_i^\top R_i \bar{u}_i \right),

  • 여기서 xˉ,zˉ,uˉ\bar{x}, \bar{z}, \bar{u}는 reference와의 차이를 의미.

    • 우리의 경우에는 zz의 reference는 null vector (keep POI center of image와 zero velocity)
    • xx와 uu의 경우에는 precomputed trajectory나 target pose로부터 정해지는 값 (이것이 꼭 필요한 것 같지는 않다.)

  • input uu와 velocty vWBv_{WB}는 아래와 같은 제약조건을 만족해야함

cmin≤c≤cmaxΩmin≤ΩB≤Ωmaxvmin≤vWB≤vmaxc_{min} \le c \le c_{max} \\ \Omega_{min} \le \Omega_B \le \Omega_{max} \\ v_{min} \le v_{WB} \le v_{max}
  • 최적화 문제를 풀 때에는 매 control loop마다 하나의 SQP iteration을 수행하여 최적화 문제의 solution을 approximate하는 식이며, 초기 state로는 onboard에서 따로 돌아가는 Visual-Inertial Odometry pipeline에서 제공되는 가장 최근의 estimate xestx_{est}를 활용한다.
  • 즉, SQP가 full optimization을 수행하지 않고, 적절한 예상치 (외부 알고리즘으로 부터 구한 real state)로부터 최적화를 수행하기 때문에, 훨씬 더 빠르고 정확한 최적화를 수행

Experiments

  • 소형 quadrotart를 통해 실험 진행

  • Qualcomm mvSDK로 VIO 수행

  • ACADO (nonlinear optimization setup framework)와 qpOASES (Quadratic programming solver)를 통해 optimization 수행

  • 예시 실험1. circular flight : 하나의 POI를 정해서 이를 따라 원형 경로를 움직이도록

    • circular reference trajectory를 로봇에게 미리 제공

Code 구현

https://github.com/uzh-rpg/rpg_quadrotor_control

https://github.com/uzh-rpg/rpg_mpc

  • controller의 run()함수를 통해 command input을 얻어낸다.
const quadrotor_common::ControlCommand command = base_controller_.run(
      state_estimate, reference_trajectory_, base_controller_params_);

Control에 필요한 variables

  • State (kStateSize = 10):
    • Position: x,y,z
    • Orientation: Quaternion components (w,x,y,z)
    • Velocity: vx,vy,vz
  • Input (kInputSize = 4):
    • Thrust: T
    • Body rates: p,q,r (angular velocities around the axes)
  • Online data
    • Point of Interest position: (px,py,pz)(p_x, p_y,p_z)
    • Camera-Body translation: (tx,ty,tz)(t_x,t_y,t_z)
    • Camera-Body rotation : (qw,qx,qy,qz)(q_{w},q_x,q_y,q_z)

Quadrotor dynamics

  • 페이퍼에 정의
    p˙WB=vWBv˙WB=wg+qWB⊙cq˙WB=12Λ(ΩB)⋅qWB\dot{p}_{WB}=v_{WB} \\ \dot{v}_{WB}=wg+q_{WB} \odot c \\ \dot{q}_{WB} = \frac{1}{2} \Lambda(\Omega_B) \cdot q_{WB}
  • 코드 구현
    • ⊙\odot 공식에 따라 thrust vector cc를 qWBq_{WB}만큼 회전 시키는 것 확인
    • Λ\Lambda 식도 공식대로
    • << : system에 미분방정식 추가
// Position derivatives
f << dot(p_x) ==  v_x;
f << dot(p_y) ==  v_y;
f << dot(p_z) ==  v_z;

// Quaternion derivatives
f << dot(q_w) ==  0.5 * ( - w_x * q_x - w_y * q_y - w_z * q_z);
f << dot(q_x) ==  0.5 * ( w_x * q_w + w_z * q_y - w_y * q_z);
f << dot(q_y) ==  0.5 * ( w_y * q_w - w_z * q_x + w_x * q_z);
f << dot(q_z) ==  0.5 * ( w_z * q_w + w_y * q_x - w_x * q_y);

// Velocity derivatives (Acceleration)
f << dot(v_x) ==  2 * ( q_w * q_y + q_x * q_z ) * T;
f << dot(v_y) ==  2 * ( q_y * q_z - q_w * q_x ) * T;
f << dot(v_z) ==  ( 1 - 2 * q_x * q_x - 2 * q_y * q_y ) * T - g_z;

Perception Objectives

  • 페이퍼에 따르면 perception objectives에서는 POI의 image로의 projection이 center에 위치해야함

  • 하지만 POI는 world frame에서 정의된 3D 좌표이므로, 아래의 과정이 필요

    • Camera frame에서의 POI 좌표를 계산
    • POI를 image plane으로 projection → 이 projection을 cost function으로 사용해야 함

  • 코드 구현 : 엄청 복잡한 식으로 intermediate state를 정의

// Intermediate states to calculate point of interest projection!
IntermediateState intSx = ((((-q_x)*q_B_C_x+(-q_y)*q_B_C_y+(-q_z)*q_B_C_z+q_B_C_w*q_w)*((-q_x)*q_B_C_x+(-q_y)*q_B_C_y+(-q_z)*q_B_C_z+q_B_C_w*q_w)+((-q_x)*q_B_C_z+q_B_C_w*q_y+q_B_C_x*q_z+q_B_C_y*q_w)*(-(-q_x)*q_B_C_z-q_B_C_w*q_y-q_B_C_x*q_z-q_B_C_y*q_w)+((-q_y)*q_B_C_x+q_B_C_w*q_z+q_B_C_y*q_x+q_B_C_z*q_w)*(-(-q_y)*q_B_C_x-q_B_C_w*q_z-q_B_C_y*q_x-q_B_C_z*q_w)+((-q_z)*q_B_C_y+q_B_C_w*q_x+q_B_C_x*q_w+q_B_C_z*q_y)*((-q_z)*q_B_C_y+q_B_C_w*q_x+q_B_C_x*q_w+q_B_C_z*q_y))*(p_F_x-p_x)+(((-q_x)*q_B_C_x+(-q_y)*q_B_C_y+(-q_z)*q_B_C_z+q_B_C_w*q_w)*((-q_y)*q_B_C_x+q_B_C_w*q_z+q_B_C_y*q_x+q_B_C_z*q_w)+((-q_x)*q_B_C_x+(-q_y)*q_B_C_y+(-q_z)*q_B_C_z+q_B_C_w*q_w)*((-q_y)*q_B_C_x+q_B_C_w*q_z+q_B_C_y*q_x+q_B_C_z*q_w)+((-q_x)*q_B_C_z+q_B_C_w*q_y+q_B_C_x*q_z+q_B_C_y*q_w)*((-q_z)*q_B_C_y+q_B_C_w*q_x+q_B_C_x*q_w+q_B_C_z*q_y)+(-(-q_x)*q_B_C_z-q_B_C_w*q_y-q_B_C_x*q_z-q_B_C_y*q_w)*(-(-q_z)*q_B_C_y-q_B_C_w*q_x-q_B_C_x*q_w-q_B_C_z*q_y))*(p_F_y-p_y)+(((-q_x)*q_B_C_x+(-q_y)*q_B_C_y+(-q_z)*q_B_C_z+q_B_C_w*q_w)*(-(-q_x)*q_B_C_z-q_B_C_w*q_y-q_B_C_x*q_z-q_B_C_y*q_w)+((-q_x)*q_B_C_x+(-q_y)*q_B_C_y+(-q_z)*q_B_C_z+q_B_C_w*q_w)*(-(-q_x)*q_B_C_z-q_B_C_w*q_y-q_B_C_x*q_z-q_B_C_y*q_w)+((-q_y)*q_B_C_x+q_B_C_w*q_z+q_B_C_y*q_x+q_B_C_z*q_w)*((-q_z)*q_B_C_y+q_B_C_w*q_x+q_B_C_x*q_w+q_B_C_z*q_y)+((-q_y)*q_B_C_x+q_B_C_w*q_z+q_B_C_y*q_x+q_B_C_z*q_w)*((-q_z)*q_B_C_y+q_B_C_w*q_x+q_B_C_x*q_w+q_B_C_z*q_y))*(p_F_z-p_z));
IntermediateState intSy = ((((-q_x)*q_B_C_x+(-q_y)*q_B_C_y+(-q_z)*q_B_C_z+q_B_C_w*q_w)*((-q_x)*q_B_C_x+(-q_y)*q_B_C_y+(-q_z)*q_B_C_z+q_B_C_w*q_w)+((-q_x)*q_B_C_z+q_B_C_w*q_y+q_B_C_x*q_z+q_B_C_y*q_w)*((-q_x)*q_B_C_z+q_B_C_w*q_y+q_B_C_x*q_z+q_B_C_y*q_w)+((-q_y)*q_B_C_x+q_B_C_w*q_z+q_B_C_y*q_x+q_B_C_z*q_w)*(-(-q_y)*q_B_C_x-q_B_C_w*q_z-q_B_C_y*q_x-q_B_C_z*q_w)+((-q_z)*q_B_C_y+q_B_C_w*q_x+q_B_C_x*q_w+q_B_C_z*q_y)*(-(-q_z)*q_B_C_y-q_B_C_w*q_x-q_B_C_x*q_w-q_B_C_z*q_y))*(p_F_y-p_y)+(((-q_x)*q_B_C_x+(-q_y)*q_B_C_y+(-q_z)*q_B_C_z+q_B_C_w*q_w)*((-q_z)*q_B_C_y+q_B_C_w*q_x+q_B_C_x*q_w+q_B_C_z*q_y)+((-q_x)*q_B_C_x+(-q_y)*q_B_C_y+(-q_z)*q_B_C_z+q_B_C_w*q_w)*((-q_z)*q_B_C_y+q_B_C_w*q_x+q_B_C_x*q_w+q_B_C_z*q_y)+((-q_x)*q_B_C_z+q_B_C_w*q_y+q_B_C_x*q_z+q_B_C_y*q_w)*((-q_y)*q_B_C_x+q_B_C_w*q_z+q_B_C_y*q_x+q_B_C_z*q_w)+(-(-q_x)*q_B_C_z-q_B_C_w*q_y-q_B_C_x*q_z-q_B_C_y*q_w)*(-(-q_y)*q_B_C_x-q_B_C_w*q_z-q_B_C_y*q_x-q_B_C_z*q_w))*(p_F_z-p_z)+(((-q_x)*q_B_C_x+(-q_y)*q_B_C_y+(-q_z)*q_B_C_z+q_B_C_w*q_w)*(-(-q_y)*q_B_C_x-q_B_C_w*q_z-q_B_C_y*q_x-q_B_C_z*q_w)+((-q_x)*q_B_C_x+(-q_y)*q_B_C_y+(-q_z)*q_B_C_z+q_B_C_w*q_w)*(-(-q_y)*q_B_C_x-q_B_C_w*q_z-q_B_C_y*q_x-q_B_C_z*q_w)+((-q_x)*q_B_C_z+q_B_C_w*q_y+q_B_C_x*q_z+q_B_C_y*q_w)*((-q_z)*q_B_C_y+q_B_C_w*q_x+q_B_C_x*q_w+q_B_C_z*q_y)+((-q_x)*q_B_C_z+q_B_C_w*q_y+q_B_C_x*q_z+q_B_C_y*q_w)*((-q_z)*q_B_C_y+q_B_C_w*q_x+q_B_C_x*q_w+q_B_C_z*q_y))*(p_F_x-p_x));
IntermediateState intSz = ((((-q_x)*q_B_C_x+(-q_y)*q_B_C_y+(-q_z)*q_B_C_z+q_B_C_w*q_w)*((-q_x)*q_B_C_x+(-q_y)*q_B_C_y+(-q_z)*q_B_C_z+q_B_C_w*q_w)+((-q_x)*q_B_C_z+q_B_C_w*q_y+q_B_C_x*q_z+q_B_C_y*q_w)*(-(-q_x)*q_B_C_z-q_B_C_w*q_y-q_B_C_x*q_z-q_B_C_y*q_w)+((-q_y)*q_B_C_x+q_B_C_w*q_z+q_B_C_y*q_x+q_B_C_z*q_w)*((-q_y)*q_B_C_x+q_B_C_w*q_z+q_B_C_y*q_x+q_B_C_z*q_w)+((-q_z)*q_B_C_y+q_B_C_w*q_x+q_B_C_x*q_w+q_B_C_z*q_y)*(-(-q_z)*q_B_C_y-q_B_C_w*q_x-q_B_C_x*q_w-q_B_C_z*q_y))*(p_F_z-p_z)+(((-q_x)*q_B_C_x+(-q_y)*q_B_C_y+(-q_z)*q_B_C_z+q_B_C_w*q_w)*((-q_x)*q_B_C_z+q_B_C_w*q_y+q_B_C_x*q_z+q_B_C_y*q_w)+((-q_x)*q_B_C_x+(-q_y)*q_B_C_y+(-q_z)*q_B_C_z+q_B_C_w*q_w)*((-q_x)*q_B_C_z+q_B_C_w*q_y+q_B_C_x*q_z+q_B_C_y*q_w)+((-q_y)*q_B_C_x+q_B_C_w*q_z+q_B_C_y*q_x+q_B_C_z*q_w)*((-q_z)*q_B_C_y+q_B_C_w*q_x+q_B_C_x*q_w+q_B_C_z*q_y)+(-(-q_y)*q_B_C_x-q_B_C_w*q_z-q_B_C_y*q_x-q_B_C_z*q_w)*(-(-q_z)*q_B_C_y-q_B_C_w*q_x-q_B_C_x*q_w-q_B_C_z*q_y))*(p_F_x-p_x)+(((-q_x)*q_B_C_x+(-q_y)*q_B_C_y+(-q_z)*q_B_C_z+q_B_C_w*q_w)*(-(-q_z)*q_B_C_y-q_B_C_w*q_x-q_B_C_x*q_w-q_B_C_z*q_y)+((-q_x)*q_B_C_x+(-q_y)*q_B_C_y+(-q_z)*q_B_C_z+q_B_C_w*q_w)*(-(-q_z)*q_B_C_y-q_B_C_w*q_x-q_B_C_x*q_w-q_B_C_z*q_y)+((-q_x)*q_B_C_z+q_B_C_w*q_y+q_B_C_x*q_z+q_B_C_y*q_w)*((-q_y)*q_B_C_x+q_B_C_w*q_z+q_B_C_y*q_x+q_B_C_z*q_w)+((-q_x)*q_B_C_z+q_B_C_w*q_y+q_B_C_x*q_z+q_B_C_y*q_w)*((-q_y)*q_B_C_x+q_B_C_w*q_z+q_B_C_y*q_x+q_B_C_z*q_w))*(p_F_y-p_y));
  • 실제로는 아래의 내용을 길게 늘어쓴 것

    • World → Camera frame rotation을 계산 : (qWBqBC)−1(q_{WB}q_{BC})^{-1}

    • World → Camera frame translation을 계산 : qWB⊙pBC+pWBq_{WB}\odot p_{BC}+p_{WB}

    • Camera frame에서의 POI

      Cpf=(qWBqBC)−1⊙(Wpf−(qWB⊙pBC+pWB))_Cp_f=(q_{WB}q_{BC})^{-1}\odot (_Wp_f-(q_{WB}\odot p_{BC}+p_{WB}))

→ 즉, intermediate state는 POI의 camera frame 좌표

  • 다음 image plane으로 projection (normalized image plane)
u=fxCpfxCpfz,  v=fyCpfyCpfxu=f_x\frac{_Cp_{fx}}{_Cp_{fz}}, \; v=f_y\frac{_Cp_{fy}}{_Cp_{fx}}
  • 코드
u = intSx / (intSz + epsilon);
v = intSy / (intSz + epsilon);

Cost function and weights

  • Column cost vector hh에 여러 variable 삽입
h << p_x << p_y << p_z
    << q_w << q_x << q_y << q_z
    << v_x << v_y << v_z
    << intSx/(intSz + epsilon) << intSy/(intSz + epsilon) 
    << T << w_x << w_y << w_z;
  • 마지막 cost vector에는 control input은 없음 (time horizon 이후 control 정의 x)
hN << p_x << p_y << p_z
    << q_w << q_x << q_y << q_z
    << v_x << v_y << v_z
    << intSx/(intSz + epsilon) << intSy/(intSz + epsilon);
  • 각 cost element의 weight도 정의
  DMatrix Q(h.getDim(), h.getDim());
  Q.setIdentity();
  Q(0,0) = 100;   // x
  Q(1,1) = 100;   // y
  Q(2,2) = 100;   // z
  Q(3,3) = 100;   // qw
  Q(4,4) = 100;   // qx
  Q(5,5) = 100;   // qy
  Q(6,6) = 100;   // qz
  Q(7,7) = 10;   // vx
  Q(8,8) = 10;   // vy
  Q(9,9) = 10;   // vz
  Q(10,10) = 0;  // Cost on perception
  Q(11,11) = 0;  // Cost on perception
  Q(12,12) = 1;   // T
  Q(13,13) = 1;   // wx
  Q(14,14) = 1;   // wy
  Q(15,15) = 1;   // wz
  • reference 정의
// Set a reference for the analysis (if CODE_GEN is false).
// Reference is at x = 2.0m in hover (qw = 1).
DVector r(h.getDim());    // Running cost reference
r.setZero();
r(0) = 2.0;
r(3) = 1.0;
r(10) = g_z;

DVector rN(hN.getDim());   // End cost reference
rN.setZero();
rN(0) = r(0);
rN(3) = r(3);
  • Cost function 정의 → Least Squared method 사용 : cost vector와 reference vector 사이 차이를 최소화할 수 있도록 cost function을 정의
Cost=∑k=0N−1(hk−rk)TQ(hk−rk)+(hN−rN)TQN(hN−rN)Cost=\sum^{N-1}_{k=0}(h_k-r_k)^TQ(h_k-r_k)+(h_N-r_N)^TQ_N(h_N-r_N)
  • 페이퍼에서 cost function 정의 → xˉ,zˉ,uˉ\bar{x}, \bar{z}, \bar{u} 는 reference와의 차이를 의미하므로, 위와 같은 식이고, 단지 cost vector component를 나눠서 써놓은 것
L=xˉN⊤Qx,NxˉN+∑i=1N−1(xˉi⊤Qx,ixˉi+zˉi⊤Qp,izˉi+uˉi⊤Riuˉi),L = \bar{x}_N^\top Q_{x,N} \bar{x}_N + \sum_{i=1}^{N-1} \left( \bar{x}_i^\top Q_{x,i} \bar{x}_i + \bar{z}_i^\top Q_{p,i} \bar{z}_i + \bar{u}_i^\top R_i \bar{u}_i \right),
  • 코드 구현
ocp.minimizeLSQ( Q, h, r );
ocp.minimizeLSQEndTerm( QN, hN, rN );

Discussion

  • UAV control을 위해 PID를 사용하다가 더 많은 constraint를 다루고 싶어서 MPC를 공부
  • MPC 구현을 위해 코드가 함께 있는 study를 알게 되어 리뷰
profile
신희준
공부하고 싶은 사람
이전 포스트

Simple online and realtime tracking with a deep association metric 리뷰

다음 포스트

Auto Filmer: Autonomous Aerial Videography Under Human Interaction 논문 리뷰

0개의 댓글