【検討】NervesでQuadCopterの姿勢制御できるか

課題

姿勢制御では、yaw,roll,pitchの3次元での計算が必要です。
行列での記述ができると何かと都合がよいです。
姿勢制御をPid制御で行うと想定して、ターゲットのRaspi Zero Wでこの演算にどれくらいの時間がかかる調査してみる。

姿勢制御を安定して行うには、最低でも毎秒100回、できれば、毎秒500回程度実行する必要があります。
余裕もみて、一回の演算処理を1m秒程度におさえたいところです。

姿勢制御プログラム

pid.ex

alias Nx.Tensor

defmodule Pid do
  import Nx.Defn

  defstruct kp: nil, ki: nil, kd: nil, guard: nil, integration: nil, error_last: nil, u: nil

  def new([kp, ki, kd, guard]) do
    kp = Nx.tensor(kp)
    ki = Nx.tensor(ki)
    kd = Nx.tensor(kd)
    guard = Nx.tensor(guard)
    guard = if !Nx.equal(ki, Nx.tensor(0.0)), do: Nx.multiply(guard, Nx.divide(kp, ki)), else: guard
    %Pid{
      kp: kp,
      ki: ki,
      kd: kd,
      guard: guard,
      integration: Nx.tensor([0,0,0]),
      error_last: Nx.tensor([0,0,0]),
      u: Nx.tensor([0,0,0])
    }
  end

  def out(%__MODULE__{kp: kp, ki: ki, kd: kd, guard: guard, integration: integration, error_last: error_last} = pid, setpoint, y_value, dt) do
    {integration, error, u} = out_nx(kp,ki,kd,guard, integration,error_last, setpoint, y_value, dt)
    %Pid{pid| integration: integration, error_last: error, u: u}
  end

  defn out_nx(kp,ki,kd,guard, integration,error_last, setpoint, y_value, dt) do
    error = setpoint-y_value
    integration = min(integration+error*dt, guard)
    d_error = error - error_last
    u = kp * error + ki * integration + kd * d_error / dt
    {integration, error, u}
  end

end

controller.ex

defmodule Controller do
  defstruct pid_stable: %{}, pid_angle: %{}, dt: 0.0

  def new(dt, pid_stable_const, pid_angle_const) do
    pid_stable = Pid.new(pid_stable_const)
    pid_angle = Pid.new(pid_angle_const)
    %Controller{pid_stable: pid_stable, pid_angle: pid_angle, dt: dt}
  end

  def next(%Controller{pid_stable: pid_stable, pid_angle: pid_angle, dt: dt}, angle, angular_velocity, setpoint) do
    # PID for angles
    pid_angle = Pid.out(pid_angle, setpoint, angle, dt)

    # PID for stabilization
    pid_stable = Pid.out(pid_stable, pid_angle.u, angular_velocity, dt)
    %Controller{pid_stable: pid_stable, pid_angle: pid_angle, dt: dt}
  end

  def get_u(%Controller{pid_stable: pid_stable}) do
    pid_stable.u
  end
end

測定プログラム

適当な値を用意して、Pidの演算処理を10回実行して実行時間を表示する。
単位はマイクロ秒。

test.ex

defmodule Main do

  def main do
    pid_stable_const = [[0,0,0], [0,0,0], [0,0,0], [0,0,0]]
    pid_anble_const = [[0,0,0], [0,0,0], [0,0,0], [0,0,0]]

    controller = Controller.new(10, pid_stable_const, pid_anble_const)
    angle = Nx.tensor([0, 0, 0])
    angular_velocity = Nx.tensor([0, 0, 0])
    set_point = Nx.tensor([0, 0, 0])

    for x <- 1..10 do
      {time, controller} = :timer.tc(fn -> Controller.next(controller, angle, angular_velocity, set_point)end)
      u = Controller.get_u(controller)
      time
    end
  end

end

結果

No	CPU	Nx	defn	EXLA	実行時間概算(μs)
1	RaspiZero	使用	使用	未使用	8000
2	RaspiZero	使用	未使用	未使用	3600
3	RaspiZero	未使用	未使用	未使用	100
4	IntelCPU	使用	使用	未使用	300
5	Raspi4	使用	使用	未使用	3300
6	Raspi4	使用	使用	使用	5800

2024/9/5
Raspi4の結果追加しました

まとめ

• 思っていたより、Nxを使うと遅い
• defnを使わない記述の3m秒だったら、なんとか使えるかも。でももう一桁速くなってほしい
• Nxを使わない（1次元の処理を３回実行）記述なら余裕
• Intel CPUは速い

EXLAが使えると状況はかわるかもしれません。Raspi ZeroではEXLAのコンパイルができてないので、一旦 Raspi 4で比較してみたい。

結果めも

結果１

iex(1)> Main.main()
[10881, 9542, 9326, 9430, 8546, 8083, 7798, 8698, 7986, 8534]
iex(2)>

結果２

defnを使わないで、Nxの関数を呼び出した場合

pid.ex

  def out(%__MODULE__{kp: kp, ki: ki, kd: kd, guard: guard, integration: integration, error_last: error_last} = pid, setpoint, y_value, dt) do
    # {integration, error, u} = out_nx(kp,ki,kd,guard, integration,error_last, setpoint, y_value, dt)
    error = Nx.subtract(setpoint, y_value)
    integration = Nx.min(Nx.add(integration, Nx.multiply(error, dt)), guard)
    d_error = Nx.subtract(error, error_last)
    u = Nx.add(Nx.add(Nx.multiply(kp, error), Nx.multiply(ki, integration)), Nx.divide(Nx.multiply(kd, d_error), dt))

    %Pid{pid| integration: integration, error_last: error, u: u}
  end

iex(1)> Main.main()
[4320, 4158, 3477, 4261, 3670, 3232, 4136, 3068, 3771, 3911]
iex(2)>

結果３

Nxを使わないで、1次元の処理を3回行った場合

iex(1)> Main.main()
[171, 106, 100, 99, 100, 97, 205, 227, 105, 99]
iex(2)>

結果４

Nx版をIntel CPU環境で実行した場合

iex(1)> Main.main()
[6792, 282, 344, 249, 181, 304, 194, 254, 356, 212]
iex(2)>