二輪モバイルロボットのシミュレーションについて

以下の条件で二輪ロボットのシミュレーション(20秒)を行い、ロボットの軌跡とICCの位置を見つけたいのですが、 今までmatlabを統計やデータ解析で使用したことはあるのですが、ロボットのシミュレーションの経験がなく、いつかサンプルのコードやチュートリアルは見てみたのですが、難易度が高いものが多く、どのように進めていけばいいかわからない状況です。そのため、この内容にあったコード例やチュートリアル、またはこれに対するアドバイス頂けるとさいわいです。 以下条件です Case 1: R1=R2=10cm, Wl = Wr =5sin(3t) (rad/s) Case 2: R1=R2=10cm, Wl =5sin(3t), Wr =4sin(3t+1) (rad/s) B=20cm

[Tohru Kikawada]

もう少し詳しく教えていただけますでしょうか。

ICCとはなんでしょうか。

また、R1,R2,Wl,Wr,Bはそれぞれ何をして示していますか。

参考にしている論文やウェブサイトを教えていただけますか。

二輪ロボットのシミュレーションという意味では下記の例なども参考になるかもしれません。

https://jp.mathworks.com/help/robotics/examples/path-following-for-a-differential-drive-robot.html

[Naoki Ishibashi]

質問が不明確で申し訳ありません。 R1,R2はそれぞれ左右の車輪の直径 Wl,Wrはそれぞれ左右の角速度 Bは2つの車輪間の幅(左車輪の中心から右の車輪中心) ICCは曲率の瞬間中心(Instantaneous Center of Curvature)で、ICC = (x-Rsinθ, y+Rcosθ)でもとまります。(x,yは2車輪間の中間の位置) 前回の投稿に添付した画像をご参考頂ければわかりやすかと思います。 また添付頂いたURLの方法ですとどのように車輪幅と回転速度を指定すればよろしいのでしょうか？

Answer 1

[Tohru Kikawada]

いただいた情報と、下記のサイトを参考に簡単なシミュレーションモデルを作ってみました。

http://www.slideshare.net/adorepump/introduction-to-robotics-presentation

このような構成であっているかはご自身でお確かめください。

今回は固定ステップの離散時間シミュレーションで実現しましたが、 このような動的モデルのシミュレーションには連続ソルバーが使えるSimulinkが適しています。

https://jp.mathworks.com/help/simulink/ug/choosing-a-solver.html

%%初期化
clear; close all;

%%ロボットのパラメータ
Rl = 10e-2; % 10cm
Rr = 10e-2; % 10cm
B = 20e-2; % 20cm

%%シミュレーション条件
dt = 0.01; % サンプル時間
l = 20/dt; % サンプル数

%%変数の初期化
x = zeros(l,3); % 位置 [x, y, theta]
x_dot = zeros(l,3); % 速度 [x', y', theta']
ICC = zeros(l,2); % 曲率瞬間中心 [x, y]

%%シミュレーション
for k=2:l
    t = k*dt;
    Wl = pi/6;
    Wr = pi/4;
    Vl = Wl*Rl;
    Vr = Wr*Rr;
    R = B/2*(Vr+Vl)/(Vr-Vl);
    w = (Vr-Vl)/B;
    v = (Vr+Vl)/2;
    ICC(k,:) = [x(k-1,1)-R*sin(x(k-1,3)), x(k-1,2)+R*cos(x(k-1,3))];
    x_dot(k,:) = [v*cos(x(k-1,3)), v*sin(x(k-1,3)), w];
    x(k,:) = x(k-1,:) + x_dot(k,:)*dt;
end

%%可視化
figure, plot(x(:,1),x(:,2));
hold on;
quiver(x(1:200:end,1),x(1:200:end,2),x_dot(1:200:end,1),x_dot(1:200:end,2));
plot(ICC(:,1),ICC(:,2));
axis equal;
xlabel('X(m)'); ylabel('Y(m)');
legend('位置','速度','ICC');

ロボットの軌跡:

[Naoki Ishibashi]

ご丁寧に回答頂き本当にありがとうございます。 コードにわかりやすコメントありとても助かりました。 ただいくつか自分でも上記のコードをもとに試してみたのですが、うまくいかないことがあり以下再度ご回答頂けると幸いです。

• スタートを(0,0)にY軸に沿うように走行させたいのですが、x = zeros(l,3)を0,0にしてみたら以下のエラーが出てしまいました

インデックスが行列の次元を超えています。
エラー: HW1 (line 24)
    ICC(k,:) = [x(k-1,1)-R*sin(x(k-1,3)), x(k-1,2)+R*cos(x(k-1,3))];

• WlとWrがそれぞれの車輪の角速度になるの思うのですがcase2のWl =5sin(3t), Wr =4sin(3t+1) とすると曲率瞬間中心しかでず、その曲率瞬間中心もおかしな軌跡となりました。
• ロボットの移動に伴い曲率瞬間中心の位置も変わると思うのですがスタート時の曲率瞬間中心しかないように思えるのですが、どのようにすれば各位置での曲率瞬間中心を表示できるでしょうか？

[Tohru Kikawada]

ご質問に文中回答いたします。

• > スタートを(0,0)にY軸に沿うように走行させたいのですが、x = zeros(l,3)を0,0にしてみたら以下のエラーが出てしまいました

インデックスが行列の次元を超えています。
エラー: HW1 (line 24)
    ICC(k,:) = [x(k-1,1)-R*sin(x(k-1,3)), x(k-1,2)+R*cos(x(k-1,3))];

zeros 関数がどのようなものか理解されていますでしょうか。

>> doc zeros

などとして関数の使い方を調べてみてください。スタートを(0,0)にしたいのであれば変更する必要がないことが分かります。

• > WlとWrがそれぞれの車輪の角速度になるの思うのですがcase2のWl =5sin(3t), Wr =4sin(3t+1) とすると曲率瞬間中心しかでず、その曲率瞬間中心もおかしな軌跡となりました。

"曲率瞬間中心しかでず"とはどういう意味でしょうか。 また、おかしな軌跡とは何がどうおかしくなっているのでしょうか。 具体的に教えてください。

表示範囲の問題であれば axis コマンドで範囲を絞れます。

曲率瞬間中心は両輪の速度が同じ場合には R が無限大になるので、同じスケールで表示すると範囲が飛んでしまうと思います。

表示範囲を変更してシミュレーションした結果を貼り付けます。詳細は添付のMATLABファイルを参照してください。

この結果が妥当なのかどうかはご自身でお確かめください。

• > ロボットの移動に伴い曲率瞬間中心の位置も変わると思うのですがスタート時の曲率瞬間中心しかないように思えるのですが、どのようにすれば各位置での曲率瞬間中心を表示できるでしょうか？

上記の例でも各ロボットの位置座標に対応する曲率瞬間中心を表示しているつもりです。一番はじめの例では、両輪の角速度が一定ですので、曲率瞬間中心は常に同じ座標になりますよね。