現在、Simulink で 2 台の車のオーバーパス操作をシミュレートするモデルを作成しています。パス フォローイングでは、Simulink で組み込みの非線形 MPC を使用しています。入力として基準点を取得します。
基準点を作成するために、MATLAB 関数を開発しました。コードは次のとおりです。
function bezierPoints = buildBezier(egoX, egoY, egoV, targetX, targetY, targetV)
bezierPoints = zeros(3,1);
pts = [];
yaw = [0];
timeGap = (abs(egoX - targetX))/(egoV - targetV);
counter = 0;
if (egoX - targetX) < -1.5
%counter = 1;
P0 = [egoX ; egoY];
P1 = P0;
P2 = [egoX + 2 ; egoY];
P3 = P2;
t = linspace(0,1,80);
yaw = linspace(0,0,80);
pts = [kron((1-t).^3,P0) + kron(3*(1-t).^2.*t,P1) + kron(3*(1-t).*t.^2,P2) + kron(t.^3,P3), yaw'];
counter = counter + 1;
elseif (egoX - targetX) > -1.5 && (egoX - targetX) <= 1
%counter = 1;
finalX = egoX + egoV*timeGap;
intermediateX = (egoX + finalX)/2;
P0 = [egoX ; egoY];
P1 = [intermediateX ; -0.5];
P2 = [intermediateX ; 0.5];
P3 = [finalX ; 0.5];
t = linspace(0,1,80);
pts = [kron((1-t).^3,P0) + kron(3*(1-t).^2.*t,P1) + kron(3*(1-t).*t.^2,P2) + kron(t.^3,P3)];
for i=1:(size(pts,2)-1)
yaw = [ yaw , atan((pts(2,i+1)-pts(2,i))/(pts(1,i+1)-pts(1,i)))];
end
pts = [pts, yaw'];
counter = counter + 1;
elseif (egoX - targetX) > 1
%counter = 1;
finalX = egoX + egoV*timeGap;
intermediateX = (egoX + finalX)/2;
P0 = [egoX ; egoY];
P1 = [intermediateX ; 0.5];
P2 = [intermediateX ; -0.5];
P3 = [finalX ; -0.5];
t = linspace(0,1,80);
pts = [kron((1-t).^3,P0) + kron(3*(1-t).^2.*t,P1) + kron(3*(1-t).*t.^2,P2) + kron(t.^3,P3)];
for i=1:(size(pts,2)-1)
yaw = [yaw , atan((pts(2,i+1)-pts(2,i))/(pts(1,i+1)-pts(1,i)))];
end
pts = [pts, yaw'];
counter = counter + 1;
end
bezierPoints = pts(:,counter);
end
コードにいくつかの問題があり、システム自体はかなり「ばかげている」ことは知っていますが、現在の主な問題は、非線形 MPC が可変サイズの信号を取得できないことです (MATLAB は可変サイズであると言いますが、実際にはサイズは常に同じです。この行のポイント数を定義しますt = linspace(0,1,80);)。Simulink は次のエラーを示します。
私はいくつかのことを試しました。変数をbezierPoints永続化して、ここからベジェ曲線を構築する他の方法を試しました:二次ベジェ曲線コードを三次ベジェ曲線に変換する方法は? . 上記のコードでは、次のメソッドを使用しました: Bézier Curves and Kronecker's Tensor Product . 変数の代入をbezierPoints関数の最後に移動しました (複数の代入により、ベクトル サイズが異なるためにこのエラーが発生するのではないかと考えました)。しかし、何も機能しませんでした。
そこで、この問題をベジエ曲線の構築に使用する方法に関連付けます。シミュレーション中にこの曲線を固定サイズの配列として作成する方法はありますか? この関数の前に、シミュレーションの前に曲線を事前定義し、シミュレーションの時間参照を使用してブロック "ワークスペースから" を使用しましたが、今では両方の車の位置と速度に応じてシミュレーション中に作成する必要があります。
ありがとう！