アルゴリズム

直線は次のRの符号と同じにする
円弧で、前回の経路と符号が違う時、法線ベクトルの方向にトレッド幅の２倍ずれる

saiteki_keiro.m

        function obj = write_saiteki_keiro(obj,keiro)%keiro[回転角度[rad],R[m]]直線の時:rad=距離
            keiro = optimize_keiro(obj,keiro)
            for gyou = 1:size(keiro,1)
                gyou
                %符号が違う(積がマイナス)時、ずらす
                if (abs(keiro(gyou,2))~=inf)
                    if keiro(gyou-1,2)*keiro(gyou,2)<0
                        if keiro(gyou,2)>0
                            housen_theta = obj.theta + pi/2;
                        else
                            housen_theta = obj.theta - pi/2;
                        end
                        obj.x = obj.x + 2*obj.tred*cos(housen_theta);
                        obj.y = obj.y + 2*obj.tred*sin(housen_theta);
                    end
                end
                obj = write_one_keiro(obj,keiro(gyou,1),keiro(gyou,2));
            end
        end

後は、

経路をずれるところをぼかす
タコつぼ、スラローム対策
次に速度、角速度を決定してみる

次に加速度、角加速度を決定してみる
角加速度のトレードオフ
$$\boldsymbol{a}(t)=a(t)\boldsymbol{t}+\frac{v^2}{r}\boldsymbol{n}=a(t)\boldsymbol{t}+rω^2\boldsymbol{n}=a(t)\boldsymbol{t}+vω\boldsymbol{n}$$
なので、速度一定、角加速度一定では、加速度は大きくなるばかりだ
とりあえず、角加速度だけ設定してみる