みなさん，こんにちは
おかしょです．
ロボットの姿勢角の算出をするプロジェクトの完成編です．
以前までのプログラムではカメラの映像表示ができていませんでしたが，ようやくwebカメラの使い方もわかり，基板も作成しました．
この記事を読むと以下のようなことがわかる・できるようになります．
- 姿勢角の算出方法によって異なる精度
- Processingでリアルタイムグラフの作成方法
- Processingのアニメーション例
この記事を読む前に
この記事は以下の記事の続きになるので，そちらを先に読んでおくことをおすすめします．
Arduino側のプログラム
姿勢角の算出にはArduinoを使用しています．
Arduinoに書き込むプログラムは以前のものと変わらないので，以下の記事内のプログラムを参考にしてください．
Processing側のプログラム
Processingのプログラムは以前のプログラムにwebカメラを使用できるプログラムを追加しただけです．
Processingでwebカメラを使用する方法は以下で解説しているので，そちらを参照してください．
これを組み込んだプログラムの全容は以下になります．
import processing.opengl.*;
import processing.serial.*;
import processing.video.*;
Serial port;
Capture cam;
int[] data = new int[21];
int time, phiA, thetaA, phiG, thetaG, phi, theta; // リアルタイムデータ
// cameraの位置を座標で指定
int cameraX = 0;
int cameraY = -300;
int cameraZ = 1000;
int positionX = 380;
int positionY = -450;
int[] PlotData1 = new int[300];
int[] PlotData2 = new int[300];
int[] PlotData3 = new int[300];
int[] PlotData4 = new int[300];
int[] PlotData5 = new int[300];
int[] PlotData6 = new int[300];
void setup() {
size(displayWidth,displayHeight, OPENGL); // windowのサイズを画面いっぱいにする
String[] cameras = Capture.list();
for(int i=0; i<cameras.length; i++){
println("[" + i + "] " + cameras[i]);
}
cam = new Capture(this, 640, 360, cameras[0]);
cam.start();
smooth();
port = new Serial(this, Serial.list()[0], 9600); // Serial port設定
background(0, 0, 0); // 背景を黒にする
frameRate(10); // frame rateを5fpsに設定
translate(width/2, height/2, 0); // 原点を画面の中心に
camera(cameraX,cameraY,cameraZ,0,0,0,0,1,0); // 原点を向く
for(int i=0; i<300; i++) // plotするデータの初期化
{
PlotData1[i] = 0;
PlotData2[i] = 0;
PlotData3[i] = 0;
PlotData4[i] = 0;
PlotData5[i] = 0;
PlotData6[i] = 0;
}
time = 0;
}
void draw() {
background(0); // 背景の色指定 黒に指定
// 3次元描画
pushMatrix(); // 座標系の保存
RotateBox(0, height/4, 0, 50, 255, 50, phi, theta); // 真ん中の直方体の描画
RotateBox(-width/3, height/4, 0, 255, 50, 50, phiA, thetaA); // 左の直方体の描画
RotateBox(width/3, height/4, 0, 50, 50, 255, phiG, thetaG); // 右の直方体の描画
popMatrix(); // 保存した座標系の出力
// 2次元描画
rotateY(atan2(cameraX, cameraZ)); // カメラの向きから2次元に描画できるように座標軸を回転
rotateX(-atan2(cameraY, sqrt(sq(cameraX)+sq(cameraZ))));
hint(DISABLE_DEPTH_TEST); // z軸を無効化
TimeHistory(-width/2, -height/2+height/2/5, 0, "φ[deg]", 'r', phiA, phi, phiG); // 時間履歴の表示
TimeHistory(-width/6+width/3/10, -height/2+height/2/5, 0, "θ[deg]", 'p', thetaA, theta, thetaG); // 時間履歴の表示
textSize(height*0.04); // 文字の大きさ
textAlign(CENTER); // 文字の座標指定位置
text("Accel", -width/3, height/2*0.95); // 使用センサ描画
text("Fusion", 0, height/2*0.95); // 使用センサ描画
text("Gyro", width/3, height/2*0.95); // 使用センサ描画
if(cam.available() == true){
cam.read();
}
translate(positionX, positionY);
image(cam, 0, 0);
hint(ENABLE_DEPTH_TEST); // z軸を有効化
}
void RotateBox(int positionX, int positionY, int positionZ, int ColorR, int ColorG, int ColorB, int roll, int pitch)
{
pushMatrix(); // 座標系の保存
fill(ColorR, ColorG, ColorB); // 直方体の色
translate(positionX, positionY, positionZ); // 直方体の位置
rotateZ(-radians(pitch));
rotateX(-radians(roll));
box(450, 20, 300); // 直方体の描画
popMatrix(); // 保存した座標系を出力
}
void TimeHistory(int positionX, int positionY, int positionZ, String name, char ch, int data1, int data2, int data3)
{
// 引数説明 //
// positionX, positionY, positionZはグラフを表示する枠の左上の点
// nameはグラフの縦軸のラベル名
// data0はリアルタイム，data0Pは1ステップ前の時間
// data1はリアルタイムのデータ，data1Pは1ステップ前のデータ
// 枠線の描画
float Width = width/3*0.85; // 枠の幅
float Height = height/2*0.6; // 枠の高さ
pushMatrix(); // 座標系の保存
noFill(); // 塗りつぶしなし
stroke(100); // 枠線の色
strokeWeight(3); // 枠線の幅
translate(positionX, positionY, positionZ); // 枠の左上の点を指定
rect(0, 0, Width, Height); // 枠の描画
// メモリ線の描画
strokeWeight(1); // メモリ線の幅
for (int i=1; i<6; i++) // メモリ線の描画
{
line(0, Height/6*i, Width, Height/6*i); // 縦軸
line(Width/6*i, 0, Width/6*i, Height); // 横軸
}
// メモリ値の描画
stroke(0);
fill(100); // メモリ値の文字色
// 縦軸のメモリ値
textSize(Height*0.06); // 文字の大きさ
textAlign(RIGHT); // 文字の座標指定位置
int j = -90; // 縦軸の目盛りの最低値
for (int i=6; i>=0;i--) // メモリ戦の描画
{
text(j, -width/3*0.02, Height/6*i); // 文字の描画
j = j + 30; // 縦軸の目盛りの更新
}
pushMatrix(); // 座標系の保存
rotate(radians(-90)); // 座標軸を90度回転
textAlign(CENTER); // 文字の座標指定位置
text(name, -Height/2, -width/3*0.1); // 縦軸ラベルの描画
popMatrix(); // 保存した座標系の出力
// 横軸のメモリ値
textAlign(CENTER); // 文字の座標指定位置
text("now", Width, Height*1.08); // 文字の描画
j = -30; // 横軸の目盛りの最低値
for(int i=0; i<=5; i++)
{
text(j, Width/6*i, Height*1.08); // 文字の描画
j = j + 5; // 横軸の目盛りの更新
}
text("time [s]", Width/2, Height*1.2); // 横軸ラベルの描画
// グラフの描画
pushMatrix();
translate(0, Height/2, 0); // 座標の原点を縦軸の中心に移動
if(data1>90) // 枠からはみ出ないようにデータを修正
{
data1 = 90;
}
else if(data1<-90)
{
data1 = -90;
}
if(data2>90)
{
data2 = 90;
}
else if(data2<-90)
{
data2 = -90;
}
if(data3>90)
{
data3 = 90;
}
else if(data3<-90)
{
data3 = -90;
}
if(ch=='r') // ロール角の場合
{
PlotData1[299] = data1; // データの更新
PlotData2[299] = data2;
PlotData3[299] = data3;
for(int i=0; i<300-1; i++) // plotするデータ処理
{
PlotData1[i] = PlotData1[i+1];
PlotData2[i] = PlotData2[i+1];
PlotData3[i] = PlotData3[i+1];
}
for(int i=0; i<300-1; i++)
{
stroke(255, 0, 0); // 線の色
line(Width/300*i, Height/6*PlotData1[i]/30, Width/300*(i+1), Height/6*PlotData1[i+1]/30);
stroke(0, 255, 0); // 線の色
line(Width/300*i, Height/6*PlotData2[i]/30, Width/300*(i+1), Height/6*PlotData2[i+1]/30);
stroke(0, 0, 255); // 線の色
line(Width/300*i, Height/6*PlotData3[i]/30, Width/300*(i+1), Height/6*PlotData3[i+1]/30);
}
}
if(ch=='p') // ピッチ角の場合
{
PlotData4[299] = data1; // データの更新
PlotData5[299] = data2;
PlotData6[299] = data3;
for(int i=0; i<300-1; i++) // plotするデータ処理
{
PlotData4[i] = PlotData4[i+1];
PlotData5[i] = PlotData5[i+1];
PlotData6[i] = PlotData6[i+1];
}
for(int i=0; i<300-1; i++)
{
stroke(255, 0, 0); // 線の色
line(Width/300*i, Height/6*PlotData4[i]/30, Width/300*(i+1), Height/6*PlotData4[i+1]/30);
stroke(0, 255, 0); // 線の色
line(Width/300*i, Height/6*PlotData5[i]/30, Width/300*(i+1), Height/6*PlotData5[i+1]/30);
stroke(0, 0, 255); // 線の色
line(Width/300*i, Height/6*PlotData6[i]/30, Width/300*(i+1), Height/6*PlotData6[i+1]/30);
}
}
popMatrix();
stroke(0); // 線の色を戻す
popMatrix(); // 保存した座標系の出力
}
void serialEvent(Serial port)
{
// シリアルポートからデータを受け取ったら
if (port.available() >=23)
{
// シリアルデータ受信
if(port.read()==1)
{
if(port.read()==200)
{
// 時間
data[0] = port.read();
data[1] = port.read();
data[2] = port.read();
// 加速度 φ
data[3] = port.read();
data[4] = port.read();
data[5] = port.read();
// 加速度 θ
data[6] = port.read();
data[7] = port.read();
data[8] = port.read();
// 角速度 φ
data[9] = port.read();
data[10] = port.read();
data[11] = port.read();
// 角速度 θ
data[12] = port.read();
data[13] = port.read();
data[14] = port.read();
// センサフュージョン φ
data[15] = port.read();
data[16] = port.read();
data[17] = port.read();
// センサフュージョン θ
data[18] = port.read();
data[19] = port.read();
data[20] = port.read();
time = data[0]*65536+data[1]*256+data[2];
if(data[3]==0)
{
phiA = data[4]*256+data[5];
}
else
{
phiA = -1*(data[4]*256+data[5]);
}
if(data[6]==0)
{
thetaA = data[7]*256+data[8];
}
else
{
thetaA = -1*(data[7]*256+data[8]);
}
if(data[9]==0)
{
phiG = data[10]*256+data[11];
}
else
{
phiG = -1*(data[10]*256+data[11]);
}
if(data[12]==0)
{
thetaG = data[13]*256+data[14];
}
else
{
thetaG = -1*(data[13]*256+data[14]);
}
if(data[15]==0)
{
phi = data[16]*256+data[17];
}
else
{
phi = -1*(data[16]*256+data[17]);
}
if(data[18]==0)
{
theta = data[19]*256+data[20];
}
else
{
theta = -1*(data[19]*256+data[20]);
}
}
}
}
}
実行結果
このプログラムは以前，Twitterでつぶやいたのですが以下のようになります．
ロボットの姿勢角の求め方の比較をしました．加速度センサーのみとジャイロセンサーのみ，それらを組み合わせたセンサーフュージョンの場合で姿勢角を求めました．グラフなどは #Processing で表示できるようにしました．#Arduino #電子工作 pic.twitter.com/xjj8UC2pzT
— おかしょ (@okasho_engineer) August 29, 2020
姿勢角算出方法の比較
姿勢角の算出方法を比較した結果，加速度センサーのみの場合はゆっくりとした姿勢角の変化には対応できていますが，機体に加速度が生じると大きく姿勢が乱れてしまっています．
ジャイロセンサーのみの場合は，ただ積分しているだけなので積分誤差がたまってしまい，時間が経てば経つほど姿勢角の誤差が増していきます．
センサーフュージョンをしている場合は，2つの算出方法の良いところどりをしているので加速度が生じても姿勢が大きく乱れることはなく，ジャイロセンサーのように誤差がたまることもないことが動画から確認できます．
結論として，センサーフュージョンが最も良い精度で姿勢角の算出ができていることがわかりました．
まとめ
この記事では姿勢角の算出方法の比較をするプロジェクトの完成編を解説しました．
このプロジェクトによって，センサーフュージョンがなぜ必要なのかが確認できました．
続けて読む
この記事ではそれぞれのセンサーで姿勢角の算出を行っています．
それぞれの姿勢角の算出方法が知りたい方は，以下の記事を参考にしてください．
Twitterでは記事の更新情報や活動の進捗などをつぶやいているので気が向いたらフォローしてください．
それでは最後まで読んでいただきありがとうございました．
コメント