/// <summary> /// 他のマーカとの距離を返す /// </summary> /// <param name="B"></param> /// <returns></returns> public double GetDistance(MarkPoint B) { double dx = (X - B.X); double dy = (Y - B.Y); return(Math.Sqrt((dx * dx) + (dy * dy))); }
/// <summary> /// 位置補正執行 /// </summary> /// <param name="bRevisionFromGPS">GPSの値をそのまま使う</param> public string LocalizeRevision(bool bRevisionFromGPS) { // 補正ログメッセージ string revisionMsg = ""; // 補正後座標 MarkPoint rev = new MarkPoint(V1); revisionMsg += "LocRevision:Issue / 座標補正を実行\n"; revisionMsg += "LocRevision: beforR1 R1.X" + R1.X.ToString("f2") + "/R1.Y " + R1.Y.ToString("f2") + "/R1.Dir " + R1.Theta.ToString("f2") + "\n"; // GPSが有効なら、GPSの値を元にPF補正 if (bEnableGPS) { // 補正基準 位置,向き double RivLocX = G1.X; double RivLocY = G1.Y; double RivDir = (bActiveCompass ? C1.Theta : R1.Theta); // コンパスが使えるなら、コンパスの値を使う if (bActiveCompass) { // コンパスを使った revisionMsg += "LocRevision:useCompass C1.Dir " + C1.Theta.ToString("f2") + "\n"; } if (bRevisionFromGPS) { // GPSの値をそのまま使う rev.Set(RivLocX, RivLocY, RivDir); // GPSを使った revisionMsg += "LocRevision: useGPS G1.X" + G1.X.ToString("f2") + "/G1.Y " + G1.Y.ToString("f2") + "/G1.Dir " + G1.Theta.ToString("f2") + "\n"; } else { // 補正基準位置からパーティクルフィルター補正 rev.Set(RivLocX, RivLocY, RivDir); // V1ローパスフィルターリセット ResetLPF_V1(rev); // 自己位置推定演算 10回 CalcLocalize(V1, true, 10); // PF revisionMsg += "LocRevision: usePF V1.X" + V1.X.ToString("f2") + "/V1.Y " + V1.Y.ToString("f2") + "/V1.Dir " + V1.Theta.ToString("f2") + "\n"; } } else { // V1の値をそのまま使う revisionMsg += "LocRevision: V1toR1 V1.X" + V1.X.ToString("f2") + "/ V1.Y " + V1.Y.ToString("f2") + "/ V1.Dir " + V1.Theta.ToString("f2") + "\n"; } // 結果を反映 R1.Set(rev); E1.Set(rev); oldE1.Set(rev); return(revisionMsg); }
/// <summary> /// マーカーと同じか比較 /// </summary> /// <param name="B"></param> /// <returns></returns> public bool IsEqual(MarkPoint B) { if (X == B.X && Y == B.Y && Theta == B.Theta) { return(true); } return(false); }
/// <summary> /// 自己位置推定計算 /// </summary> /// <param name="LRF_Data">LRF実取得データ</param> /// <param name="MRF">MAPデータ</param> /// <param name="mkp">想定ロボット位置(計算基点)</param> /// <param name="Particles">パーティクル作業バッファ</param> public void Localize(double[] LRF_Data, MapRangeFinder MRF, MarkPoint mkp, List<Particle> Particles) { double sum = 0; if (null == LRF_Data) { Console.WriteLine("ParticleFilter.cs Localize() : Error! null LrfData"); return; } // パーティクルをばらまく //for (int i = 0; i < Particles.Count; ++i) int numParticles = Particles.Count; Parallel.For(0, numParticles, i => { // 散らばらせる MakeParticle(mkp, PtclRange, PtclDirRange, Particles[i].Location); // 散らばり% = w ? // マップデータとLRFのデータを比べる Particles[i].W = Likefood(LRF_Data, MRF.Sense(Particles[i].Location)); sum += Particles[i].W; }); // W順に並べ替え Particles.Sort((a, b) => (int)((a.W - b.W)*1000.0)); // パーティクルから 整合性の高いデータを取り出す?(リサンプリング) for (int n = 0; n < ResamplingNumber; ++n) { double selectVal = Rand.NextDouble() * sum; double subsum = 0; for (int i = 0; i < Particles.Count; ++i) { subsum += Particles[i].W; if (selectVal < subsum) { ResamplingSet[n] = Particles[i].Location; break; } } } // パーティクルの平均値から、仮想ロボットの位置を補正 double newX = 0; double newY = 0; double newTheta = 0; for (int n = 0; n < ResamplingNumber; ++n) { newX += ResamplingSet[n].X; newY += ResamplingSet[n].Y; newTheta += ResamplingSet[n].Theta; } mkp.X = newX / ResamplingNumber; mkp.Y = newY / ResamplingNumber; mkp.Theta = newTheta / ResamplingNumber; }
/// <summary> /// パーティクル生成 /// </summary> /// <param name="v">基点</param> /// <param name="sigma">散らばりのフレ幅[pixel]</param> /// <param name="sigmaAng">散らばりのフレ幅[角度]</param> /// <param name="p">格納先</param> private void MakeParticle(MarkPoint v, double sigma, double sigmaAng, MarkPoint p) { var dx = NormalDistribution(sigma); // m単位 var dy = NormalDistribution(sigma); var dtheta = NormalDistribution(sigmaAng); // 座標、回転角をふれ幅内で生成 p.X = v.X + dx; p.Y = v.Y + dy; p.Theta = v.Theta + dtheta; }
/// <summary> /// GPS 現在位置取得 /// 差分を計算し現在位置更新 /// </summary> private MarkPoint updateMkp_FromGPS() { MarkPoint resMkp = new MarkPoint(R1); // 自己位置更新 // ロータリーエンコーダの移動量差分を加えて、自己位置を更新 resMkp.X += (G1.X - oldG1.X); resMkp.Y += (G1.Y - oldG1.Y); resMkp.Theta = G1.Theta; // REの向き oldG1.Set(G1); return(resMkp); }
/// <summary> /// 自己位置推定 開始位置セット /// </summary> /// <param name="stWldX">マップ座標X</param> /// <param name="stWldY">マップ座標Y</param> /// <param name="stDir">角度</param> public void SetStartPostion(int stWldX, int stWldY, double stDir) { startMapX = stWldX; startMapY = stWldY; startMapDir = stDir; MRF = new MapRangeFinder(LRF_Ctrl.LRFmaxRange_mm / MapToRealScale); // 仮想Map用 LRFクラス worldMap.UpdateAreaCenter(stWldX, stWldY); AreaBmp = worldMap.AreaGridMap.UpdateBitmap(); //bAreaMapUpdateReqest = false; MRF.SetMap(worldMap.AreaGridMap); // ロボットマーカ設定 R1 = new MarkPoint(stWldX, stWldY, stDir); // 実ロボット位置 R.E,Compass,GPSでの位置+ LRF補正 E1 = new MarkPoint(stWldX, stWldY, stDir); // R.Encoderの位置 oldE1 = new MarkPoint(stWldX, stWldY, stDir); // E2 = new MarkPoint(stWldX, stWldY, stDir); // R.E Pluse & Compus の位置 V1 = new MarkPoint(stWldX, stWldY, stDir); // 推定位置ロボット C1 = new MarkPoint(0, 0, stDir); G1 = new MarkPoint(0, 0, 0); // GPS oldG1 = new MarkPoint(stWldX, stWldY, stDir); // A1 = new MarkPoint(0, 0, 0); // AMCL // パーティクルフィルター初期化 { // サンプル数(ParticleSizeと同じで、すべてのパーティクルを対象とする) // LRFの有効距離を半径に変換(/2.0)、20%の距離で散らばる // +-5度の範囲 ParticleFilter = new ParticleFilter(ParticleSize, (((LRF_Ctrl.LRFmaxRange_mm / 2.0) * 0.20) / MapToRealScale), 30.0); // サンプリング数、パーティクルのレンジ Particles = new List <Particle>(); for (int i = 0; i < ParticleSize; ++i) { // パーティクルマーカー Particles.Add(new Particle(new MarkPoint(0, 0, 0), 0)); } } // ローパスフィルター初期化 ResetLPF_V1(V1); }
private List<Particle> Particles; // �p�[�e�B�N�����X�g #endregion Fields #region Methods /// <summary> /// �C�Ӎ��W�ł̂o�e� /// </summary> /// <param name="numPF"></param> public void CalcLocalize(MarkPoint mkp, bool useLowFilter, int numPF = 1) { // �p�[�e�B�N���t�B���^�[���Z if (useLowFilter) { KalmanFilter.SimpleLPF lpfX = null; KalmanFilter.SimpleLPF lpfY = null; KalmanFilter.SimpleLPF lpTheta = null; for (int i = 0; i < numPF; i++) { MarkPoint locMkp = new MarkPoint(worldMap.GetAreaX(mkp.X), worldMap.GetAreaY(mkp.Y), mkp.Theta); ParticleFilter.Localize(LRF.getData(), MRF, locMkp, Particles); locMkp.X = worldMap.GetWorldX(locMkp.X); locMkp.Y = worldMap.GetWorldY(locMkp.Y); // ���ʂɃ��[�p�X�t�B���^�[������� if (null == lpfX) { // �ŏ��̒l������l�ɂ��� lpfX = new KalmanFilter.SimpleLPF(locMkp.X); lpfY = new KalmanFilter.SimpleLPF(locMkp.Y); lpTheta = new KalmanFilter.SimpleLPF(locMkp.Theta); } else { lpfX.update(locMkp.X); lpfY.update(locMkp.Y); lpTheta.update(locMkp.Theta); } } if (null != lpfX) { mkp.X = lpfX.value(); mkp.Y = lpfY.value(); mkp.Theta = lpTheta.value(); } } else { ParticleFilter.Localize(LRF.getData(), MRF, mkp, Particles); } }
/// <summary> /// 任意座標でのPF補正 /// </summary> /// <param name="numPF"></param> public void CalcLocalize(MarkPoint mkp, bool useLowFilter, int numPF = 1) { // パーティクルフィルター演算 if (useLowFilter) { KalmanFilter.SimpleLPF lpfX = null; KalmanFilter.SimpleLPF lpfY = null; KalmanFilter.SimpleLPF lpTheta = null; for (int i = 0; i < numPF; i++) { MarkPoint locMkp = new MarkPoint(worldMap.GetAreaX(mkp.X), worldMap.GetAreaY(mkp.Y), mkp.Theta); ParticleFilter.Localize(LRF.getData(), MRF, locMkp, Particles); locMkp.X = worldMap.GetWorldX(locMkp.X); locMkp.Y = worldMap.GetWorldY(locMkp.Y); // 結果にローパスフィルターをかける if (null == lpfX) { // 最初の値を初期値にする lpfX = new KalmanFilter.SimpleLPF(locMkp.X); lpfY = new KalmanFilter.SimpleLPF(locMkp.Y); lpTheta = new KalmanFilter.SimpleLPF(locMkp.Theta); } else { lpfX.update(locMkp.X); lpfY.update(locMkp.Y); lpTheta.update(locMkp.Theta); } } if (null != lpfX) { mkp.X = lpfX.value(); mkp.Y = lpfY.value(); mkp.Theta = lpTheta.value(); } } else { ParticleFilter.Localize(LRF.getData(), MRF, mkp, Particles); } }
/// <summary> /// マーカー描画 /// </summary> /// <param name="g"></param> /// <param name="robot"></param> /// <param name="brush"></param> /// <param name="size"></param> private void DrawMaker_Area(Graphics g, float fScale, MarkPoint robot, Brush brush, int size) { double mkX = worldMap.GetAreaX(robot.X) * fScale; double mkY = worldMap.GetAreaY(robot.Y) * fScale; double mkDir = robot.Theta - 90.0; var P1 = new PointF( (float)(mkX + size * Math.Cos(mkDir * Math.PI / 180.0)), (float)(mkY + size * Math.Sin(mkDir * Math.PI / 180.0))); var P2 = new PointF( (float)(mkX + size * Math.Cos((mkDir - 150) * Math.PI / 180.0)), (float)(mkY + size * Math.Sin((mkDir - 150) * Math.PI / 180.0))); var P3 = new PointF( (float)(mkX + size * Math.Cos((mkDir + 150) * Math.PI / 180.0)), (float)(mkY + size * Math.Sin((mkDir + 150) * Math.PI / 180.0))); g.FillPolygon(brush, new PointF[] { P1, P2, P3 }); }
/// <summary> /// マーカー描画 /// </summary> /// <param name="g"></param> /// <param name="fScale"></param> /// <param name="robot"></param> /// <param name="brush"></param> /// <param name="size"></param> static public void DrawMaker(Graphics g, float fScale, MarkPoint robot, Brush brush, int size) { double mkX = robot.X * fScale; double mkY = robot.Y * fScale; double mkDir = robot.Theta - 90.0; var P1 = new PointF( (float)(mkX + size * Math.Cos(mkDir * Math.PI / 180.0)), (float)(mkY + size * Math.Sin(mkDir * Math.PI / 180.0))); var P2 = new PointF( (float)(mkX + size * Math.Cos((mkDir - 150) * Math.PI / 180.0)), (float)(mkY + size * Math.Sin((mkDir - 150) * Math.PI / 180.0))); var P3 = new PointF( (float)(mkX + size * Math.Cos((mkDir + 150) * Math.PI / 180.0)), (float)(mkY + size * Math.Sin((mkDir + 150) * Math.PI / 180.0))); g.FillPolygon(brush, new PointF[] { P1, P2, P3 }); }
/// <summary> /// マーカー描画 /// </summary> /// <param name="g"></param> /// <param name="fScale"></param> /// <param name="robot"></param> /// <param name="brush"></param> /// <param name="size"></param> public static void DrawMaker(Graphics g, float fScale, MarkPoint robot, Brush brush, int size) { double mkX = robot.X * fScale; double mkY = robot.Y * fScale; double mkDir = robot.Theta - 90.0; var P1 = new PointF( (float)(mkX + size * Math.Cos(mkDir * Math.PI / 180.0)), (float)(mkY + size * Math.Sin(mkDir * Math.PI / 180.0))); var P2 = new PointF( (float)(mkX + size * Math.Cos((mkDir - 150) * Math.PI / 180.0)), (float)(mkY + size * Math.Sin((mkDir - 150) * Math.PI / 180.0))); var P3 = new PointF( (float)(mkX + size * Math.Cos((mkDir + 150) * Math.PI / 180.0)), (float)(mkY + size * Math.Sin((mkDir + 150) * Math.PI / 180.0))); g.FillPolygon(brush, new PointF[] { P1, P2, P3 }); }
/// <summary> /// PF自己位置計算 継続 /// </summary> /// <param name="mkp"></param> /// <param name="useLowFilter"></param> /// <param name="numPF"></param> public void ParticleFilterLocalize(bool useLPF) { MarkPoint locMkp = new MarkPoint(worldMap.GetAreaX(V1.X), worldMap.GetAreaY(V1.Y), V1.Theta); // パーティクルフィルター演算 ParticleFilter.Localize(LRF.getData(), MRF, locMkp, Particles); if (useLPF) { // 結果にローパスフィルターをかける V1.X = lpfV1X.update(worldMap.GetWorldX(locMkp.X)); V1.Y = lpfV1Y.update(worldMap.GetWorldY(locMkp.Y)); V1.Theta = lpfV1Theta.update(locMkp.Theta); } else { V1.X = worldMap.GetWorldX(locMkp.X); V1.Y = worldMap.GetWorldY(locMkp.Y); V1.Theta = locMkp.Theta; } }
/// <summary> /// ローパスフィルター初期化 /// </summary> /// <param name="mkp"></param> public void ResetLPF_V1(MarkPoint mkp) { lpfV1X = new KalmanFilter.SimpleLPF(mkp.X); lpfV1Y = new KalmanFilter.SimpleLPF(mkp.Y); lpfV1Theta = new KalmanFilter.SimpleLPF(mkp.Theta); }
/// <summary> /// 他のマーカとの距離を返す /// </summary> /// <param name="B"></param> /// <returns></returns> public double GetDistance(MarkPoint B) { double dx = (X - B.X); double dy = (Y - B.Y); return Math.Sqrt((dx*dx) + (dy*dy)); }
/// <summary> /// ���O�ۑ��p�@���[���h�}�b�v�ւ̋O�Չ摜���� /// </summary> /// <returns></returns> public Bitmap MakeMakerLogBmp(bool bPointOn, MarkPoint marker) { if (R1Log.Count <= 0) return null; // �f�[�^�Ȃ� Bitmap logBmp = new Bitmap(worldMap.mapBmp); Graphics g = Graphics.FromImage(logBmp); // �O�Օ`�� // ���Ȉʒu DrawMakerLogLine_World(g, R1Log, Color.Red.R, Color.Red.G, Color.Red.B); // �p�[�e�B�N���t�B���^�[ ���Ȉʒu���� DrawMakerLogLine_World(g, V1Log, Color.Cyan.R, Color.Cyan.G, Color.Cyan.B); // ���[�^���[�G���R�[�_���W DrawMakerLogLine_World(g, E1Log, Color.Purple.R, Color.Purple.G, Color.Purple.B); // GPS���W DrawMakerLogLine_World(g, G1Log, Color.Green.R, Color.Green.G, Color.Green.B); // �ŏI�n�_�Ƀ}�[�J�\�� if (R1Log.Count > 0) { DrawMaker_Area(g, 1.0f, R1Log[R1Log.Count - 1], Brushes.Red, 4); } if (V1Log.Count > 0) { DrawMaker_Area(g, 1.0f, V1Log[V1Log.Count - 1], Brushes.Cyan, 4); } if (E1Log.Count > 0) { DrawMaker_Area(g, 1.0f, E1Log[E1Log.Count - 1], Brushes.Purple, 4); } if (G1Log.Count > 0) { DrawMaker_Area(g, 1.0f, G1Log[G1Log.Count - 1], Brushes.Green, 4); } if (null != marker) { DrawMaker_Area(g, 1.0f, marker, Brushes.GreenYellow, 4); } // �����Ԃ��Ƃ̈ʒu�ƌ��� if (bPointOn) { // ���Ȉʒu //foreach (var p in R1Log) for (int i = 0; i < R1Log.Count; i++) { if ((i % 30) != 0) continue; DrawMaker_Area(g, 1.0f, R1Log[i], Brushes.Red, 4); } // LRF �p�[�e�B�N���t�B���^�[ //foreach (var p in V1Log) for (int i = 0; i < V1Log.Count; i++) { if ((i % 30) != 0) continue; DrawMaker_Area(g, 1.0f, V1Log[i], Brushes.Cyan, 3); } // ���[�^���[�G���R�[�_���W //foreach (var p in E1Log) for (int i = 0; i < E1Log.Count; i++) { if ((i % 30) != 0) continue; DrawMaker_Area(g, 1.0f, E1Log[i], Brushes.Purple, 4); } } g.Dispose(); return logBmp; }
/// <summary> /// �}�[�J�[�`�� /// </summary> /// <param name="g"></param> /// <param name="robot"></param> /// <param name="brush"></param> /// <param name="size"></param> private void DrawMaker_Area(Graphics g, float fScale, MarkPoint robot, Brush brush, int size) { double mkX = worldMap.GetAreaX(robot.X) * fScale; double mkY = worldMap.GetAreaY(robot.Y) * fScale; double mkDir = robot.Theta - 90.0; var P1 = new PointF( (float)(mkX + size * Math.Cos(mkDir * Math.PI / 180.0)), (float)(mkY + size * Math.Sin(mkDir * Math.PI / 180.0))); var P2 = new PointF( (float)(mkX + size * Math.Cos((mkDir - 150) * Math.PI / 180.0)), (float)(mkY + size * Math.Sin((mkDir - 150) * Math.PI / 180.0))); var P3 = new PointF( (float)(mkX + size * Math.Cos((mkDir + 150) * Math.PI / 180.0)), (float)(mkY + size * Math.Sin((mkDir + 150) * Math.PI / 180.0))); g.FillPolygon(brush, new PointF[] { P1, P2, P3 }); }
/// <summary> /// マップイメージログ出力 /// </summary> /// <param name="BrainCtrl"></param> /// <returns></returns> public bool Output_ImageLog(ref Brain BrainCtrl ) { try { // 軌跡ログ出力 if (!string.IsNullOrEmpty(saveLogFname)) { MarkPoint tgtMaker = null; // 次の目的地取得 { int tgtPosX = 0; int tgtPosY = 0; double dir = 0; BrainCtrl.RTS.getNowTarget(ref tgtPosX, ref tgtPosY); BrainCtrl.RTS.getNowTargetDir(ref dir); tgtMaker = new MarkPoint(tgtPosX, tgtPosY, dir); } { Bitmap bmp = BrainCtrl.LocSys.MakeMakerLogBmp(false, tgtMaker); if (null != bmp) { // 画像ファイル保存 string saveImageLogFname = Path.ChangeExtension(saveLogFname, "png"); bmp.Save(saveImageLogFname, System.Drawing.Imaging.ImageFormat.Png); } } } } catch (Exception ex) { MessageBox.Show(ex.Message + System.Environment.NewLine + ex.StackTrace); return false; } return true; }
/// <summary> /// PF���Ȉʒu�v�Z �p�� /// </summary> /// <param name="mkp"></param> /// <param name="useLowFilter"></param> /// <param name="numPF"></param> public void ParticleFilterLocalize(bool useLPF) { MarkPoint locMkp = new MarkPoint(worldMap.GetAreaX(V1.X), worldMap.GetAreaY(V1.Y), V1.Theta); // �p�[�e�B�N���t�B���^�[���Z ParticleFilter.Localize(LRF.getData(), MRF, locMkp, Particles); if (useLPF) { // ���ʂɃ��[�p�X�t�B���^�[������� V1.X = lpfV1X.update(worldMap.GetWorldX(locMkp.X)); V1.Y = lpfV1Y.update(worldMap.GetWorldY(locMkp.Y)); V1.Theta = lpfV1Theta.update(locMkp.Theta); } else { V1.X = worldMap.GetWorldX(locMkp.X); V1.Y = worldMap.GetWorldY(locMkp.Y); V1.Theta = locMkp.Theta; } }
/// <summary> /// 指定位置のマップ障害物情報をLRFの値として返す /// </summary> /// <param name="mapX"></param> /// <param name="mapY"></param> /// <param name="ang"></param> /// <returns></returns> public double[] GetMapLRF( int mapX, int mapY, double ang ) { MarkPoint mkp = new MarkPoint(worldMap.GetAreaX(mapX), worldMap.GetAreaY(mapY), ang); return MRF.Sense(mkp); }
//static int areaMapDrawCnt = 0; public void AreaMap_Draw_WorldMap(Graphics g, ref CersioCtrl CersioCt, ref Brain BrainCtrl) { LocPreSumpSystem LocSys = BrainCtrl.LocSys; // 全体マップ描画 float viewScale; g.FillRectangle(Brushes.Black, 0, 0, worldMapBmp.Width, worldMapBmp.Height); if (((float)LocSys.worldMap.WorldSize.w / (float)worldMapBmp.Width) < ((float)LocSys.worldMap.WorldSize.h / (float)worldMapBmp.Height)) { viewScale = (float)(1.0 / ((float)LocSys.worldMap.WorldSize.h / (float)worldMapBmp.Height)); } else { viewScale = (float)(1.0 / ((float)LocSys.worldMap.WorldSize.w / (float)worldMapBmp.Width)); } //g.ResetTransform(); //g.TranslateTransform(-ctrX, -ctrY, MatrixOrder.Append); //g.RotateTransform((float)layer.wAng, MatrixOrder.Append); //g.ScaleTransform(viewScale, viewScale, MatrixOrder.Append); if (null != worldMapBmp) { g.DrawImage(worldMapBmp, 0, 0); } //g.ResetTransform(); // 各マーカーの位置を描画 LocSys.DrawWorldMap(g, viewScale); // ターゲット描画 if (null != CersioCt) { int tgtPosX, tgtPosY; double dir = 0; tgtPosX = tgtPosY = 0; BrainCtrl.RTS.getNowTarget(ref tgtPosX, ref tgtPosY); BrainCtrl.RTS.getNowTargetDir(ref dir); MarkPoint tgtMk = new MarkPoint(tgtPosX, tgtPosY, dir + 180); DrawMaker(g, viewScale, tgtMk, Brushes.GreenYellow, 8); // ターゲットまでのライン DrawMakerLine(g, viewScale, LocSys.R1, tgtMk, Pens.Olive, 1); } }
public void CarInit(MarkPoint _mkp) { CarInit(_mkp.X, _mkp.Y, _mkp.Theta); mkp.Set(_mkp); }
public Grid GetMapInfo(MarkPoint mkp) { return(worldMap.GetAreaMapInfo(mkp.X, mkp.Y)); }
/// <summary> /// エリアマップ描画 /// </summary> /// <param name="sender"></param> /// <param name="e"></param> public void AreaMap_Draw_Area(Graphics g, ref CersioCtrl CersioCt, ref Brain BrainCtrl) { // 書き換えBMP(追加障害物)描画 LocPreSumpSystem LocSys = BrainCtrl.LocSys; // エリアマップ描画 //g.FillRectangle(Brushes.Black, 0, 0, picbox_AreaMap.Width, picbox_AreaMap.Height); g.DrawImage(LocSys.AreaOverlayBmp, 0, 0); // ターゲット描画 if (null != CersioCt) { int tgtPosX, tgtPosY; double dir = 0; tgtPosX = tgtPosY = 0; float olScale = (float)LocSys.AreaOverlayBmp.Width / (float)LocSys.AreaBmp.Width; BrainCtrl.RTS.getNowTarget(ref tgtPosX, ref tgtPosY); BrainCtrl.RTS.getNowTargetDir(ref dir); MarkPoint tgtMk = new MarkPoint(LocSys.worldMap.GetAreaX(tgtPosX), LocSys.worldMap.GetAreaY(tgtPosY), dir); DrawMaker(g, olScale, tgtMk, Brushes.GreenYellow, 8); // ターゲットまでのライン DrawMakerLine(g, olScale, new MarkPoint(LocSys.worldMap.GetAreaX(LocSys.R1.X), LocSys.worldMap.GetAreaY(LocSys.R1.Y), 0), tgtMk, Pens.Olive, 1); } }
public double[] GetMapLRF(MarkPoint mkp) { return(GetMapLRF((int)(mkp.X + 0.5), (int)(mkp.Y + 0.5), mkp.Theta)); }
/// <summary> /// 指定位置のマップ障害物情報をLRFの値として返す /// </summary> /// <param name="mapX"></param> /// <param name="mapY"></param> /// <param name="ang"></param> /// <returns></returns> public double[] GetMapLRF(int mapX, int mapY, double ang) { MarkPoint mkp = new MarkPoint(worldMap.GetAreaX(mapX), worldMap.GetAreaY(mapY), ang); return(MRF.Sense(mkp)); }
/// <summary> /// ロータリーエンコーダ 現在位置取得 /// 差分を計算し現在位置更新 /// </summary> private MarkPoint updateMkp_FromREPlot() { MarkPoint resMkp = new MarkPoint(R1); // 自己位置更新 // ロータリーエンコーダの移動量差分を加えて、自己位置を更新 resMkp.X += (E1.X - oldE1.X); resMkp.Y += (E1.Y - oldE1.Y); resMkp.Theta = E1.Theta; // REの向き oldE1.Set(E1); return resMkp; }
/// <summary> /// 自己位置推定計算 /// </summary> /// <param name="LRF_Data">LRF実取得データ</param> /// <param name="MRF">MAPデータ</param> /// <param name="mkp">想定ロボット位置(計算基点)</param> /// <param name="Particles">パーティクル作業バッファ</param> public void Localize(double[] LRF_Data, MapRangeFinder MRF, MarkPoint mkp, List <Particle> Particles) { double sum = 0; if (null == LRF_Data) { Console.WriteLine("ParticleFilter.cs Localize() : Error! null LrfData"); return; } // パーティクルをばらまく //for (int i = 0; i < Particles.Count; ++i) int numParticles = Particles.Count; Parallel.For(0, numParticles, i => { // 散らばらせる MakeParticle(mkp, PtclRange, PtclDirRange, Particles[i].Location); // 散らばり% = w ? // マップデータとLRFのデータを比べる Particles[i].W = Likefood(LRF_Data, MRF.Sense(Particles[i].Location)); sum += Particles[i].W; }); // W順に並べ替え Particles.Sort((a, b) => (int)((a.W - b.W) * 1000.0)); // パーティクルから 整合性の高いデータを取り出す?(リサンプリング) for (int n = 0; n < ResamplingNumber; ++n) { double selectVal = Rand.NextDouble() * sum; double subsum = 0; for (int i = 0; i < Particles.Count; ++i) { subsum += Particles[i].W; if (selectVal < subsum) { ResamplingSet[n] = Particles[i].Location; break; } } } // パーティクルの平均値から、仮想ロボットの位置を補正 double newX = 0; double newY = 0; double newTheta = 0; for (int n = 0; n < ResamplingNumber; ++n) { newX += ResamplingSet[n].X; newY += ResamplingSet[n].Y; newTheta += ResamplingSet[n].Theta; } mkp.X = newX / ResamplingNumber; mkp.Y = newY / ResamplingNumber; mkp.Theta = newTheta / ResamplingNumber; }
public double[] Sense(MarkPoint mkp) { return(Sense(Map, mkp.X, mkp.Y, mkp.Theta)); }
public double[] GetMapLRF(MarkPoint mkp) { return GetMapLRF((int)(mkp.X+0.5), (int)(mkp.Y+0.5), mkp.Theta); }
public Particle(MarkPoint r, double w) { Location = r; W = w; }
public void Set(MarkPoint B) { Set(B.X, B.Y, B.Theta); }
public Grid GetMapInfo(MarkPoint mkp) { return worldMap.GetAreaMapInfo(mkp.X, mkp.Y); }
public MarkPoint(MarkPoint mkp) : this(mkp.X, mkp.Y, mkp.Theta) { }
/// <summary> /// ログ保存用 ワールドマップへの軌跡画像生成 /// </summary> /// <returns></returns> public Bitmap MakeMakerLogBmp(bool bPointOn, MarkPoint marker) { if (R1Log.Count <= 0) { return(null); // データなし } Bitmap logBmp = new Bitmap(worldMap.mapBmp); Graphics g = Graphics.FromImage(logBmp); // 軌跡描画 // 自己位置 DrawMakerLogLine_World(g, R1Log, Color.Red.R, Color.Red.G, Color.Red.B); // パーティクルフィルター 自己位置推定 DrawMakerLogLine_World(g, V1Log, Color.Cyan.R, Color.Cyan.G, Color.Cyan.B); // ロータリーエンコーダ座標 DrawMakerLogLine_World(g, E1Log, Color.Purple.R, Color.Purple.G, Color.Purple.B); // GPS座標 DrawMakerLogLine_World(g, G1Log, Color.Green.R, Color.Green.G, Color.Green.B); // 最終地点にマーカ表示 if (R1Log.Count > 0) { DrawMaker_Area(g, 1.0f, R1Log[R1Log.Count - 1], Brushes.Red, 4); } if (V1Log.Count > 0) { DrawMaker_Area(g, 1.0f, V1Log[V1Log.Count - 1], Brushes.Cyan, 4); } if (E1Log.Count > 0) { DrawMaker_Area(g, 1.0f, E1Log[E1Log.Count - 1], Brushes.Purple, 4); } if (G1Log.Count > 0) { DrawMaker_Area(g, 1.0f, G1Log[G1Log.Count - 1], Brushes.Green, 4); } if (null != marker) { DrawMaker_Area(g, 1.0f, marker, Brushes.GreenYellow, 4); } // 一定期間ごとの位置と向き if (bPointOn) { // 自己位置 //foreach (var p in R1Log) for (int i = 0; i < R1Log.Count; i++) { if ((i % 30) != 0) { continue; } DrawMaker_Area(g, 1.0f, R1Log[i], Brushes.Red, 4); } // LRF パーティクルフィルター //foreach (var p in V1Log) for (int i = 0; i < V1Log.Count; i++) { if ((i % 30) != 0) { continue; } DrawMaker_Area(g, 1.0f, V1Log[i], Brushes.Cyan, 3); } // ロータリーエンコーダ座標 //foreach (var p in E1Log) for (int i = 0; i < E1Log.Count; i++) { if ((i % 30) != 0) { continue; } DrawMaker_Area(g, 1.0f, E1Log[i], Brushes.Purple, 4); } } g.Dispose(); return(logBmp); }
/// <summary> /// 自己位置推定 開始位置セット /// </summary> /// <param name="stWldX">マップ座標X</param> /// <param name="stWldY">マップ座標Y</param> /// <param name="stDir">角度</param> public void SetStartPostion(int stWldX, int stWldY, double stDir) { MRF = new MapRangeFinder(LRF_Ctrl.LRFmaxRange_mm / MapToRealScale); // 仮想Map用 LRFクラス worldMap.UpdateAreaCenter(stWldX, stWldY); AreaBmp = worldMap.AreaGridMap.UpdateBitmap(); //bAreaMapUpdateReqest = false; MRF.SetMap(worldMap.AreaGridMap); // ロボットマーカ設定 R1 = new MarkPoint(stWldX, stWldY, stDir); // 実ロボット位置 R.E,Compass,GPSでの位置+ LRF補正 E1 = new MarkPoint(stWldX, stWldY, stDir); // R.Encoderの位置 oldE1 = new MarkPoint(stWldX, stWldY, stDir); // E2 = new MarkPoint(stWldX, stWldY, stDir); // R.E Pluse & Compus の位置 V1 = new MarkPoint(stWldX, stWldY, stDir); // 推定位置ロボット C1 = new MarkPoint(0, 0, stDir); G1 = new MarkPoint(0, 0, 0); // GPS oldG1 = new MarkPoint(stWldX, stWldY, stDir); // // パーティクルフィルター初期化 { // サンプル数(ParticleSizeと同じで、すべてのパーティクルを対象とする) // LRFの有効距離を半径に変換(/2.0)、20%の距離で散らばる // +-5度の範囲 ParticleFilter = new ParticleFilter(ParticleSize, (((LRF_Ctrl.LRFmaxRange_mm / 2.0) * 0.20) / MapToRealScale), 30.0 ); // サンプリング数、パーティクルのレンジ Particles = new List<Particle>(); for (int i = 0; i < ParticleSize; ++i) { // パーティクルマーカー Particles.Add(new Particle(new MarkPoint(0,0,0), 0)); } } // ローパスフィルター初期化 ResetLPF_V1(V1); }
/// <summary> /// マーカーと同じか比較 /// </summary> /// <param name="B"></param> /// <returns></returns> public bool IsEqual(MarkPoint B) { if (X == B.X && Y == B.Y && Theta == B.Theta) return true; return false; }
/// <summary> /// 位置補正執行 /// </summary> /// <param name="bRevisionFromGPS">GPSの値をそのまま使う</param> public string LocalizeRevision( bool bRevisionFromGPS ) { // 補正ログメッセージ string revisionMsg = ""; // 補正後座標 MarkPoint rev = new MarkPoint(V1); revisionMsg += "LocRevision:Issue / 座標補正を実行\n"; revisionMsg += "LocRevision: beforR1 R1.X" + R1.X.ToString("f2") + "/R1.Y " + R1.Y.ToString("f2") + "/R1.Dir " + R1.Theta.ToString("f2") + "\n"; // GPSが有効なら、GPSの値を元にPF補正 if (bEnableGPS) { // 補正基準 位置,向き double RivLocX = G1.X; double RivLocY = G1.Y; double RivDir = (bActiveCompass ? C1.Theta : R1.Theta); // コンパスが使えるなら、コンパスの値を使う if (bActiveCompass) { // コンパスを使った revisionMsg += "LocRevision:useCompass C1.Dir " + C1.Theta.ToString("f2") + "\n"; } if (bRevisionFromGPS) { // GPSの値をそのまま使う rev.Set(RivLocX, RivLocY, RivDir); // GPSを使った revisionMsg += "LocRevision: useGPS G1.X" + G1.X.ToString("f2") + "/G1.Y " + G1.Y.ToString("f2") + "/G1.Dir " + G1.Theta.ToString("f2") + "\n"; } else { // 補正基準位置からパーティクルフィルター補正 rev.Set(RivLocX, RivLocY, RivDir); // V1ローパスフィルターリセット ResetLPF_V1(rev); // 自己位置推定演算 10回 CalcLocalize(V1, true,10); // PF revisionMsg += "LocRevision: usePF V1.X" + V1.X.ToString("f2") + "/V1.Y " + V1.Y.ToString("f2") + "/V1.Dir " + V1.Theta.ToString("f2") + "\n"; } } else { // V1の値をそのまま使う revisionMsg += "LocRevision: V1toR1 V1.X" + V1.X.ToString("f2") + "/ V1.Y " + V1.Y.ToString("f2") + "/ V1.Dir " + V1.Theta.ToString("f2") + "\n"; } // 結果を反映 R1.Set(rev); E1.Set(rev); oldE1.Set(rev); return revisionMsg; }
private void DrawMakerLine(Graphics g, float fScale, MarkPoint robotA, MarkPoint robotB, Pen pen, int size) { var P1 = new PointF( (float)(robotA.X * fScale), (float)(robotA.Y * fScale)); var P2 = new PointF( (float)(robotB.X * fScale), (float)(robotB.Y * fScale)); g.DrawLine(pen, P1, P2); }
/// <summary> /// 現在位置更新 /// 各センサー情報から、座標、向きを選択 /// </summary> public void update_NowLocation() { // R.E.Plotの座標を計算・取得 MarkPoint RePlotMkp = updateMkp_FromREPlot(); // GPS座標を計算・取得 MarkPoint GpsMkp = updateMkp_FromGPS(); // 移動情報 更新 if (Setting.bMoveSrcRePlot) { // R.E Plot R1.X = RePlotMkp.X; R1.Y = RePlotMkp.Y; } else if (Setting.bMoveSrcGPS) { // GPS R1.X = GpsMkp.X; R1.Y = GpsMkp.Y; } else if (Setting.bMoveSrcSVO) { // Semi-direct Monocular Visual Odometry // ※未実装 } else if (Setting.bMoveSrcReCompus) { // RE Pulse値(移動量) & Compusの向きでの座標 R1.X = E2.X; R1.Y = E2.Y; } else if (Setting.bMoveSrcPF) { // RE Pulse値(移動量) & PFの向きでの座標 // ※未実装 } else if (Setting.bMoveAMCL) { R1.X = A1.X; R1.Y = A1.Y; } // 向き情報 更新 if (Setting.bDirSrcRePlot) { R1.Theta = RePlotMkp.Theta; } else if (Setting.bDirSrcGPS) { R1.Theta = GpsMkp.Theta; } else if (Setting.bDirSrcSVO) { // Semi-direct Monocular Visual Odometry // ※未実装 } else if (Setting.bDirSrcCompus) { if (bActiveCompass) { R1.Theta = C1.Theta; } } else if (Setting.bDirAMCL) { R1.Theta = A1.Theta; } // 更新結果ログ保存 UpdateLogData(); }
public double[] Sense(MarkPoint mkp ) { return Sense(Map, mkp.X, mkp.Y, mkp.Theta); }
/// <summary> /// /// </summary> private void SimCarInit() { // クルマの配置の初期状態 MarkPoint carInitPos = new MarkPoint(mapFileData.startPosition.x * ScalePixelToReal, mapFileData.startPosition.y * ScalePixelToReal, mapFileData.startDir); // シムカー生成 carPos = new MarkPoint(carInitPos.X, carInitPos.Y, carInitPos.Theta); carSim = new CarSim(ScalePixelToReal); carSim.CarInit(carInitPos); carSim.MapInit(MapBmp); }