ロボトレーサのシミュレーション

次のような座標系を考える。

この時、

\frac{dx(t)}{dt}=v(t)cosθ(t) \\
\frac{dy(t)}{dt}=v(t)sinθ(t) \\
\frac{dθ(t)}{dt}=ω(t) \\

これを解析的に解く事はできない
ここで、オドメトリ、デッドレコニングと呼ばれる自己位置推定の手法がある。
ここで連続時間系なので、離散時間にするときには、これを用いてシミュレーションする
ここで直線近似と円弧近似が考えられる
直線近似では、Δt秒間直線を移動すると考える
円弧近似では、Δt秒間円弧を移動すると考える

• 直線近似

\frac{x_{t+1}-x_{t}}{Δt}=v_tcosθ_t \\
\frac{y_{t+1}-x_{t}}{Δt}=v_tsinθ_t \\
\frac{θ_{t+1}-θ{t}}{Δt}=ω_t \\

よって、

x_{t+1}=x_t+Δtv_tcosθ_t \\
y_{t+1}=y_t+Δtv_tsinθ_t \\
θ_{t+1}=θ_t+Δtω_t

• 円弧近似

移動している時の平均角度は$θ_t+\frac{Δθ}{2}$
よって、ΔxはΔLの直線のcos成分
つまり、

x_{t+1}=x_t+Δx=x_t+ΔLcos(θ_t+\frac{Δθ}{2}) \\
y_{t+1}=y_t+Δx=y_t+ΔLsin(θ_t+\frac{Δθ}{2}) \\

結局直線近似してるね。。。

これでは、$x_t$から$x_{t+1}$までの直線の距離をΔL（弧の長さ）としている
ここで、厳密にこの弧の長さを求めると、

$$ΔL'=2ρsin(\frac{Δθ}{2})$$

代入して、

x_{t+1}=x_t+2ρsin(\frac{Δθ}{2})cos(θ_t+\frac{Δθ}{2}) \\
       =x_t+\frac{Δθρsin(\frac{Δθ}{2})cos(θ_t+\frac{Δθ}{2})}{\frac{Δθ}{2}} \\
       =x_t+Δθρcos(θ_t+\frac{Δθ}{2})\frac{sin(\frac{Δθ}{2})}{\frac{Δθ}{2}} \\
       =x_t+Δθρcos(θ_t+\frac{Δθ}{2})sinc(\frac{Δθ}{2})

sinc(θ)はマクローリン展開すると、
$$sinc(θ)=1+\frac{θ^2}{3!}+\frac{θ^4}{5!}+・・・$$
ここで、１次近似してみると、

x_{t+1}=x_t+Δθρcos(θ_t+\frac{Δθ}{2}) \\
       =x_t+ΔLcos(θ_t+\frac{Δθ}{2}) \\

同じ事になる。つまり、円弧を直線に近似する事は、ΔL'を１次近似することに対応する

tracer_kinematics.m

function obj = simu(obj,v,omega)
          D_theta=omega*obj.DT;
          obj.theta=obj.theta+D_theta;
          if omega==0
            obj.D_L=v*obj.DT;
            DX=obj.D_L*cos(obj.theta);
            DY=obj.D_L*sin(obj.theta);
          else
            r=v/omega;
            obj.D_L=2*r*sin(D_theta/2);
            DX=obj.D_L*cos(obj.theta+D_theta/2);
            DY=obj.D_L*sin(obj.theta+D_theta/2);
          end
          obj.x=obj.x+DX;
          obj.y=obj.y+DY;
      end

加速度と角加速度まで考慮すると

tracer_kinematics.m

        function obj = simu2(obj,a,alpha)
            obj.v=a*obj.DT;
            obj.omega=alpha*obj.DT;

            D_theta=obj.omega*obj.DT;
            obj.theta=obj.theta+D_theta;
            if obj.omega==0
                obj.D_L=obj.v*obj.DT;
                DX=obj.D_L*cos(obj.theta);
                DY=obj.D_L*sin(obj.theta);
            else
                r=obj.v/obj.omega;
                obj.D_L=2*r*sin(D_theta/2);
                DX=obj.D_L*cos(obj.theta+D_theta/2);
                DY=obj.D_L*sin(obj.theta+D_theta/2);
            end
            obj.x=obj.x+DX;
            obj.y=obj.y+DY;
        end

この時、作用反作用の法則により、モータには慣性力が働く