コイルに流れる電流上限を考慮した電磁力上限を設けた2衛星のシミュレーション作った

Author: MASAHIRO KUBOTA

README.md

@@ -9,6 +9,7 @@ coil_FT.mでいろんな相対姿勢でビオサバールとアンペールで
 dynamics2record.mで任意の場所から10^-5Nの上限でレコード盤軌道にもって行く制御
 
 dynamic2record_movie.mで1つの衛星がレコード盤軌道に投入されるシミュレーション動画．dynamics2record.mを改造．フィードバックゲインに注意しろ！
+dynamic2record_movie2.mの方が収束性はいい．
 
 force2current.mで所望の力を電流に変換する．
 
@@ -20,9 +21,10 @@ test8.mで地球の磁場を計算
 test9.mはchatGPTで生成したコード．matlabで動画を生成し，保存するためのコード．
 test10.mでコイルjの電流要素から見た，コイルkの電流要素の位置ベクトルを表示（I_ij確かめるために使った）.plot_magnetic_field_FT2.mと同じ．計算の仕方が違う．逆問題を解くための式のバグ取りのため．
 test12.mはtest10.mを改造して，特定の力を出すために必要な電流を出力するためのコード
-test11.mは特定の力を出すために必要な電流を出力して，その値をplot_magnetic_field_FT2.mで検証している．最大電流も定義してるから，実際に出せる力がこのコードでわかる．
 
+test11.mは特定の力を出すために必要な電流を出力して，その値をplot_magnetic_field_FT2.mで検証している．最大電流も定義してるから，実際に出せる力がこのコードでわかる．
 
+two_satellite_record_movie.mはコイルや距離によって発生できる電磁力の上限が決まることをビオサバールを用いた逆計算をして電磁力を決めている．
 
 
 

dynamic2record_movie.m

@@ -94,8 +94,8 @@
 %R = diag([10^7, 10^3, 10^7]);  エネルギーが少なくて済む=振動が少ない
 %この値はランデブの軌道制御における予見ファジィ制御と最適制御の比較から持ってきてる．
 
-Q = diag([1, 1, 100, 0, 0, 0]);
-R = diag([10^7, 10^3, 10^7]);
+Q = diag([1, 1, 1, 10^5, 10^5, 10^5]);
+R = diag([1, 1, 1]);
 
 %リッカチ方程式を解いて最適ゲインを求める
 [~,K,~] = icare(A,B,Q,R,[],[],[]);

dynamic2record_movie2.m

@@ -168,7 +168,11 @@
 toc
 
 
+%フレームレート
+Fs = 30; 
 
+%表示間引き数　（表示間引き数×フレームレート×サンプリングタイム）＝n倍速となる
+Th = 10;
 
 %各時刻での電磁力の大きさをグラフ化
 figure
@@ -225,11 +229,9 @@
 % 日時をファイル名に追加
 filename = sprintf('movie/dynamics2record_%s.avi', datetime('now','Format', dateformat));
 
-%フレームレート
-Fs = 30; 
 
-%表示間引き数　（表示間引き数×フレームレート×サンプリングタイム）＝n倍速となる
-Th = 10;
+
+
 
 % 動画の準備
 vidObj = VideoWriter(filename); % 動画ファイル名

feedback_system.asv

@@ -0,0 +1,54 @@
+function [X1, x_tilda1_data, u1_data, x_d1_data] = feedback_system(x_d1, v_d1, T, N, x1_0, A_d, B_d, B, K)
+%UNTITLED4 Summary of this function goes here
+%   Detailed explanation goes here
+
+%状態配列
+X1 = zeros(N, 6);
+
+%初期値代入
+X1(1,:) = x1_0;
+
+%変位あり状態方程式の入力変位を設定するための係数
+B_sharp = (B.'*B)\B.';
+
+%グラフを作るためのデータ配列
+u1_data = zeros(N,3);
+x_tilda1_data = zeros(N,1);
+x_d1_data = zeros(N,3);
+
+
+tic
+for i = 1:N %時刻i*T
+    xd = x_d1(i*T);
+
+    %フィードバック系を0に収束させるために入力変位を設定
+    u_d = B_sharp*(v_d1(i*T) - A_d*xd);
+
+    %目標値と現在地のずれ
+    x_tilda = X1(i, :).' - xd;
+
+    %x_tildaを0にするためのフィードバック
+    u_tilda = -K*x_tilda;
+
+    %元の状態方程式の入力(N)
+    u = u_tilda + u_d;
+
+    %所望の力が上限を超えた場合，方向そのままで大きさを小さくする．
+    if norm(u)>10^-5
+        u = u*10^-5/norm(u);
+    end
+
+    [F, ] = force2current(F, a, i_max, x_j, x_k, q_j, q_k)
+
+    %離散状態方程式
+    X1(i+1, :) = A_d*X1(i, :).' + B_d*u;
+
+    %目標値までの距離を格納
+    x_tilda1_data(i) = norm(X1(i, :).' - xd);
+
+    %電磁力ベクトルを格納
+    u1_data(i,:) = u;
+
+    %目標軌道
+    x_d1_data(i,:) =  xd(1:3,1).';
+end

feedback_system.m

@@ -0,0 +1,63 @@
+function [X1, x_tilda1_data, u1_data, x_d1_data] = feedback_system(x_d1, v_d1, T, N, x1_0, A_d, B_d, B, K, q1, q2, a, i_max)
+%UNTITLED4 Summary of this function goes here
+%   Detailed explanation goes here
+
+
+
+
+
+%状態配列
+X1 = zeros(N, 6);
+
+%初期値代入
+X1(1,:) = x1_0;
+
+%変位あり状態方程式の入力変位を設定するための係数
+B_sharp = (B.'*B)\B.';
+
+%グラフを作るためのデータ配列
+u1_data = zeros(N,3);
+x_tilda1_data = zeros(N,1);
+x_d1_data = zeros(N,3);
+
+
+tic
+for i = 1:N %時刻i*T
+    disp(i)
+    xd = x_d1(i*T);
+
+    %フィードバック系を0に収束させるために入力変位を設定
+    u_d = B_sharp*(v_d1(i*T) - A_d*xd);
+
+    %目標値と現在地のずれ
+    x_tilda = X1(i, :).' - xd;
+
+    %x_tildaを0にするためのフィードバック
+    u_tilda = -K*x_tilda;
+
+    %元の状態方程式の入力(N)
+    u1 = u_tilda + u_d;
+
+    %所望の力が上限を超えた場合，方向そのままで大きさを小さくする．
+    %if norm(u)>10^-5
+    %    u = u*10^-5/norm(u);
+    %end
+    
+
+    [u, ~] = force2current(u1.', a, i_max, X1(i, 1:3), -X1(i, 1:3), q1, q2);
+
+    
+
+    %離散状態方程式
+    X1(i+1, :) = A_d*X1(i, :).' + B_d*u;
+
+    %目標値までの距離を格納
+    x_tilda1_data(i) = norm(X1(i, :).' - xd);
+    disp(norm(X1(i, :).' - xd))
+
+    %電磁力ベクトルを格納
+    u1_data(i,:) = u;
+
+    %目標軌道
+    x_d1_data(i,:) =  xd(1:3,1).';
+end

force2current.asv

@@ -1,11 +1,24 @@
-%function i = force2current(f, a, i_max, N, I1)
+function [F, i_k] = force2current(F, a, i_max, x_j, x_k, q_j, q_k)
 %Find the required current from the desired force.
 %   Detailed explanation goes here
 
+
+%disp(F)
+%disp(a)
+%disp(i_max)
+%disp(x_j)
+%disp(x_k)
+%disp(q_j)
+%disp(q_k)
+
+%a = 0.015;
+N = 17;
+
 myu0 = 1.2566*10^(-6);
-i_j = [1 1 1] * 10^-5;
+split = 10;
 
-S = zeros(3,3);
+i_j = i_max * (x_k - x_j)/norm(x_k - x_j) ;
+%i_k = [1 2 3] ;
 
 %E_j = [pi/3,0,pi/4];
 %q_j = quaternion(E_j,'euler','XYZ','point');
@@ -15,27 +28,35 @@ I_j3 = zeros(1,3);
 
 for j = 1:3
     I_j1 =  I_j1 + i_j(j)*calculateI(x_j, x_k, q_j, q_k, split, a, j, 1);
+    %disp(I_j1)
+    disp(calculateI(x_j, x_k, q_j, q_k, split, a, j, 1))
     I_j2 =  I_j2 + i_j(j)*calculateI(x_j, x_k, q_j, q_k, split, a, j, 2);
+    %disp(I_j2)
     I_j3 =  I_j3 + i_j(j)*calculateI(x_j, x_k, q_j, q_k, split, a, j, 3);
+    %disp(I_j3)
 end
 
+
+
 S = N*[I_j1.', I_j2.', I_j3.'];
 
-F = myu0/(4*pi)*S*N
+disp("SSSSSSS")
+disp(S)
+%F = myu0/(4*pi)*N*S*i_k.';
 
+%F = [10^-14, 10^-14, 10^-14].';
+disp(F)
+F = F*1;
 
-%{
-while j = 1:3
-    
-    while k = 1:3
-        
+i_k = 4*pi/(myu0*N)*(S\F.');
+%disp(i_k)
 
-    S = N*i*I1*[]
-    S_inv = inv(S);
-    i = 4*pi/(N*myu0)*S_inv*f;
+if max(abs(i_k)) >= i_max
+    i_k = i_k * i_max/max(abs(i_k));
+    disp('over')
+end
 
-    if i_max > i
 
+%F = myu0/(4*pi)*N*S*i_k;
 
-end
-%}
+end

force2current.m

@@ -1,14 +1,23 @@
-function i_mat = force2current(F, a, i_max, I1, x_j, x_k, q_j, q_k)
+function [F, i_k] = force2current(F, a, i_max, x_j, x_k, q_j, q_k)
 %Find the required current from the desired force.
 %   Detailed explanation goes here
 
+
+%disp(F)
+%disp(a)
+%disp(i_max)
+%disp(x_j)
+%disp(x_k)
+%disp(q_j)
+%disp(q_k)
+
 %a = 0.015;
 N = 17;
 
 myu0 = 1.2566*10^(-6);
 split = 10;
 
-i_j = I1 ;
+i_j = i_max * (x_k - x_j)/norm(x_k - x_j) ;
 %i_k = [1 2 3] ;
 
 %E_j = [pi/3,0,pi/4];
@@ -19,6 +28,8 @@
 
 for j = 1:3
     I_j1 =  I_j1 + i_j(j)*calculateI(x_j, x_k, q_j, q_k, split, a, j, 1);
+    %disp(I_j1)
+    %disp(calculateI(x_j, x_k, q_j, q_k, split, a, j, 1))
     I_j2 =  I_j2 + i_j(j)*calculateI(x_j, x_k, q_j, q_k, split, a, j, 2);
     %disp(I_j2)
     I_j3 =  I_j3 + i_j(j)*calculateI(x_j, x_k, q_j, q_k, split, a, j, 3);
@@ -28,17 +39,24 @@
 
 
 S = N*[I_j1.', I_j2.', I_j3.'];
+
+%disp("SSSSSSS")
 %disp(S)
 %F = myu0/(4*pi)*N*S*i_k.';
 
 %F = [10^-14, 10^-14, 10^-14].';
 %disp(F)
+%F = F*1;
 
-i_mat = 4*pi/(myu0*N)*(S\F.');
+i_k = 4*pi/(myu0*N)*(S\F.');
+%disp(i_k)
 
+if max(abs(i_k)) >= i_max
+    i_k = i_k * i_max/max(abs(i_k));
+    %disp('over')
+end
 
 
+F = myu0/(4*pi)*N*S*i_k;
 
-if any(i_mat >= i_max)
-    i_mat = i_mat * i_max/max(i_mat);
 end