ロボットローダープロジェクト：自分の場所を見つける

私の長年のイギリスのパートナー（2年前に彼に宛てた「郵送先住所の認識」 ）は、ビジネスプロセスを最適化するための新しいアイデアを持っていました。 もちろん、ポイントは、ローダーがすべてのロボットの後ろに続き、ロボットが停止するとすぐにロードとアンロードを開始することではありませんが、ローダーよりもはるかに多くのロボットがあり、ほとんどの場合ロボットはエンドポイントにありますルート、ロード待ち。 ローダーは、あるロボットから次のロボットに移動し、それぞれをロードするだけで、商品の輸送に時間を浪費しません。

背景

昨年、ルンバの自走式掃除機プラットフォームを実験しました。 新品の掃除機の費用は約300ポンド（使用済みのものは100ポンドまたはそれ以下）であり、車輪に2台の電気駆動装置、前面に2台のタッチセンサー、下部に（手順を検出するため）および上部に（爆弾ステーションを見つけるために）赤外線センサーが含まれています。 センサーの正確なリストはモデルによって異なります。プロトコルは、下から最大4つの赤外線センサーを提供し、それぞれが1ビットを返します（「床が見える/見えない」）。 いずれにせよ、距離計はありません。利用可能なセンサーはすべてシングルビットです。 さらに、Roombaには「プログラム可能なarduins」はありません。それを制御するには、ラップトップ（またはarduino）を上部にインストールし、RS-232経由でロボットと通信する必要があります。 掃除機で遊んだ後、倉庫の棚の1つにほこりをつけたままにしました。

今年、 Microsoft Robotics Development Studio（MRDS）を試してみることにしました。これを促進するために、Microsoftは「標準」ロボットを制御する機器とプロトコルのセットである「MRDSリファレンスプラットフォーム」の仕様を策定しました。 この仕様により、ロボット愛好家は互換性のあるロボットを作成し、ロボット間でプログラムを転送できます。 掃除機のハードウェアと比較して、リファレンスプラットフォームはより複雑で強力です。仕様には、Kinect、3つの赤外線距離計、2つの超音波距離計、ホイール回転センサー（エンコーダー）が含まれます。 これまでのMRDS RPの実装は、 Eddie （Kinectを含まない約1000ポンド）と呼ばれるParallaxによってのみ提供されています。 EddieとMRDS RP仕様のプロトタイプロボットの写真との異常な類似性は、仕様がParallaxとの密接なコラボレーションで作成されたことを示しています。言い換えると、ParallaxはMicrosoftにプラットフォームを参考にさせることに成功しました。

さまざまなセンサーに加えて、Eddieには機械的に印象的なプラットフォーム（20 kgの負荷容量が宣言されており、それ自体の前に倉庫ローダーを押すのに十分なモーター電力があります）と、プログラマブルコントローラーParallax Propeller、つまり 重要なコードは、コンピューターから直接命令するだけでなく、ロボットに直接縫い付けることができます。

British Channel 4 は 、Gadget Manプログラムのリリースの1つに、ショッピングカートに統合されたEddieのデモを含む2分間の断片を含めました。 残念ながら、英国のIPアドレスの所有者のみがチャンネル4のWebサイトでプログラムを見ることができますが、私はそれをつかんでリロードすることができませんでした（おそらく読者の一部は成功するでしょうか）。

挑戦する

ロボットを「前後左右」だけで制御するのは難しいことではありません。そのような「リモート制御」のプログラムはエディの配信に直接含まれています。 しかし、ロボットローダーのパックを操縦する人はいません。各自が倉庫のどのポイントにいるかを認識し、次のポイントに進む必要があります。 この位置の定義で、主な問題が生じました。 GPSのような技術は一掃されています。衛星からの信号は倉庫の屋根を通過しません。 Kinectはジェスチャー認識に適していますが、ストレージシェルフをどのように認識しますか？ 棚自体には、メーターごとに一意のバーコードが貼り付けられていますが、まず、外出先で棚から20 cm離れた場所ではあまり信頼性の高いバーコードが読み取れません（近づきません-サポートにぶつかったり、棚から突き出ている箱にぶつかる危険があります）。 第二に、20cmの距離で棚に移動してバーコードを読み取るには、現在の位置を大まかに理解する必要があります。さもなければ、棚から横に移動するロボットは完全に取り返しのつかない方向を失います。

最初のナビゲーションの試みは、距離計に基づいていました。ロボットは棚を「模索」し、それに沿って移動し、階段から壁に寄りかかって驚異的な酔っぱらいが歩くように、所定の距離を維持します。 類似性は非常に大きかった：距離計からの信号の受信、MRDSでの信号の処理、モーターのチームの形成、慣性の克服の遅延は10分の1秒でした。 この間、ロボットは冷静に横に「引き離された」ので、そのたびに「コース修正」は数十度になり、軌道は広いジグザグになりました。 これに加えて、エディの距離計は非常に正確ではありませんでした-エラーは最大±5cmです-と非常に狭い焦点、すなわち 棚は、床から所定の高さの1つだけで認識されました。

より有望なのは、ホイールセンサーのナビゲーションで、各ホイールの走行距離を約±7mmの精度で与えました。 Eddieファームウェアには「ロボットの走行距離」（両車輪の走行の半分）と「ロボットの方向」（車輪の走行距離の差、正規化係数）の計算が既に含まれていますが、現在の座標（x、y）は2つのカウンターの値から計算できません ロボットの軌道を小さな単純なセクションに分割し、各セクションの動きを要約する必要があります。 一見、破線ロボットの経路を近づけたいと思います。 しかし実際には、「静止」-モーターの一定速度で-ロボットは円の弧に沿って移動します。半径が大きいほど、モーターの速度の差は小さくなります。 それどころか、ブレークポイントは、ロボットがスポットから移動せずに方向を変更する場合、実際には存在し、存在することはできません。 したがって、ロボットの軌道を一連の円弧で近似します。

計算

それはすべて、夜遅くにナプキンに絵を描くことから始まりました。



アークの開始時、ロボットは点A （既知）にあり、最後に点B （未知）になり、左車輪は既知の経路S ₁ = φr1を、右車輪は既知の経路S ₂ = φr2を移動しました 。 トラック間の距離wは一定であり、ロボットの設計によって決まります。 取得するもの：
r ₂ / r ₁ = S ₂ / S ₁およびr ₂ = r ₁ + w
（ r ₁ + w ）/ r ₁ = S ₂ / S ₁
r ₁ S ₁ + wS ₁ = r ₁ S ₂
r ₁ = wS ₁ /（ S ₂ - S ₁ ）

したがって、ロボットが回転した角度はφ = S ₁ / r ₁ =（ S ₂ - S ₁ ）/ wであり、距離| AB | = 2 | AM | = 2（ r ₁ + w / 2）sin（ φ / 2）=（2 wS ₁ /（ S ₂ - S ₁ ）+ w ）sin（ φ / 2）= w sin（ φ / 2）（ S ₂ + S ₁ ）/（ S ₂ - S ₁ ）

ここで、概して、位置を決定するためのすべての数学：計算されたφをロボットの方向の角度に追加し、現在の座標を距離だけシフトするたびに| AB |。 実用的な問題は残っています：そのような位置の更新を実行する頻度は？ 余りにも頻繁に、私達は離散的な「刻み目」で走行距離を測定する車輪センサーの不正確さに出くわします。 あまりにまれな場合、円の弧による経路セグメントの近似は十分に正確ではありません。

ホイールセンサーの読み取り値は「ティック」の時点で最も正確であると判断したため、ホイールのティックごとにロボットの位置を再計算します。ただし、約80ミリ秒前後に記録されたロボットの調査時間を超えません。 少なくとも1秒間に1回、停止したホイール（たとえば、回転中）も記録されるようにします。 登録されたすべての「ティック」はリストに保存されます。これは、位置を再計算するために、両方のホイールを同じ時間実行する必要があるためです。 両方のホイールで以前の「ティック」 の早い方から間隔を取り、「ティックのリスト」に従って同じ瞬間に2番目のホイールの走行距離を線形補間します。 ギャップの終わりについては、同様に、目盛り付きホイールのセンサーが読み取った走行距離を取得し、2番目の走行距離を直線的に外挿します。 したがって、精度は可能な限り達成可能でなければなりません。

ロボットの位置が再計算されるたびに、ロボットを指定されたターゲットに向けるために、車輪の速度を新しい方法で設定する必要があります。 ロボットを所定の「方位角」に正確に向けて定位置で回転させるのは非現実的です。前述の±7 mmのホイールマイレージエラーは、ロボットが回転すると数度のエラーになります。 ロボットは故意に「ランダムに」移動し、道路に沿って再び旋回する必要があります。 したがって、ターゲットへの方向が現在の方向とあまり違わない場合は、事前に「方位角に向ける」ことなく、アークに沿ってロボットをターゲットに向けることにしました。 （現在の方向が希望の方向とπ / 2異なる場合、そのような円弧は半円より大きくなります。これは明らかに非現実的です。この場合、ロボットは正しい方向に回転する以外に選択肢がありません。）

アークを計算するには、上の図に戻りましょう。 これで、ポイントAとBがわかったため、角度φがわかりました。 比率S ₂ / S ₁ = r ₂ / r ₁を見つける必要があります。これは、アークに必要な車輪速度の比率を設定します。 上記の計算を反対方向に繰り返します。
| AB | = 2 | AM | = 2（ r ₁ + w / 2）sin（ φ / 2）
r ₁ = | AB | /（2sin（ φ / 2））- w / 2
r ₂ = | AB | /（2sin（ φ / 2））+ w / 2
r ₂ / r ₁ = 1 + w /（| AB | /（2sin（ φ / 2））- w / 2）= 1 + 2 /（| AB | /（ w・sin（ φ / 2））-1 ）

このようなアークは、ラインABからどの程度離れますか？ ロボットが円弧を完成させるために両側に2メートルの空きスペースを必要とする場合、倉庫にはそのような機会はありません。棚間の通路の幅は1.5メートルしかありません。



距離を計算する| Mh | 円弧と線の間：| OH |-| OM | = r - r cos（ φ / 2）=（ r ₁ + w / 2）（1-cos（ φ / 2））

この「ハンプ」の大きさが所定の制限を超える場合、弧を描くように移動することを拒否し、所定の位置で回転して、ターゲットに向かって近づこうとします。

判明したように、実際には、モーターのストレスの比率はまだ車輪速度の比率を決定していません。 それらの間で正しい変換を選択することは可能ですが、私は「ブルートフォース」で問題を解決しました：私は比率を9度に上げました。 片側へのわずかな偏差は、すぐにロボットを反対方向に強く押します。 実際、ロボットの動きが本当に滑らかになったのはこの指数でした。角度が小さくなると、ロボットは横に大きく外れ、ターゲット自体でのみ正しいルートに戻りました。 大きなものでは、補正が強すぎることが判明し、ロボットは狭いジグザグで左右に「揺れ」ました。

実装

ReferencePlatform2011配信からMarkRobotサービスに新しい機能を追加することを計画していました。PositionOperationsの精神で新しいポートレットを実装しますPositionOperations : PortSet<GetPosition, SetPosition, SetDestination> 。 実際、この実装には至りませんでした。サービス間で責任を分離せずに準備できたのは泥だらけのプロトタイプだけでした。 しかし、私がやったことを見せようとします。

最初に、座標を操作するために何かが必要です。それらを保存するためにSystem.Drawing.PointFはありません。

 public struct Position { public readonly double x, y, heading; public Position(double x, double y, double heading) { this.x = x; this.y = y; this.heading = heading; } private static double Sqr(double d) { return d * d; } public double DistanceTo(double x, double y) { return Math.Sqrt(Sqr(this.y - y) + Sqr(this.x - x)); } public static double NormalizeHeading(double heading) { while (heading < -Math.PI) heading += 2 * Math.PI; while (heading > Math.PI) heading -= 2 * Math.PI; return heading; } // turn angle/2, go ahead distance, turn angle/2 again public Position advance(double distance, double angle) { double newHeading = heading + angle / 2, newX = x + distance * Math.Cos(newHeading), newY = y + distance * Math.Sin(newHeading); return new Position(newX, newY, NormalizeHeading(heading + angle)); } } 

advanceメソッドは、指定された回転角φと移動距離|の座標の再計算を実装します。 AB |（記事の中央の図のように）。

（今、ここにコードをコピーして、私は正確に「二重角度式」を取得し、角度φ / 2で| AB |だけシフトする代わりに、角度φで r ₁ + w / 2だけ移動できることに気付きました。まあ。）

さらに、サービスにはPositionKeepingインスタンスが含まれており、その内部にはEncoderLog 2つのインスタンスがあります。「 EncoderLogリスト」、各ホイールの個別のインスタンスです。 （理論的には）センサーデータは時系列で到着しない可能性があるため、リストはSortedListにSortedListれます。 EncoderLog唯一の重要な方法は、「ティック」の間または最後の「ティック」の後の任意の時点で値を取得するためのパスの線形近似です。

 private class PositionKeeping { private static readonly DateTime initialized = DateTime.Now; public class EncoderLog { private SortedList<DateTime, double> log = new SortedList<DateTime, double> { { initialized, 0 } }; public void Register(DateTime at, double reading) { log[at] = reading; } public void Reset(DateTime at, double reading) { log.Clear(); log.Add(at, reading); } public DateTime LastTick { get { return log.Last().Key; } } public double LastReading { get { return log.Last().Value; }} public double ReadingAt(DateTime at) { int index = log.Count - 1; while(index>=0 && log.Keys[index] > at) index--; if(index<0) return double.NaN; // before first reading; impossible // now, log.Keys[index] <= at, and log.Keys[index+1] > at DateTime preceding = log.Keys[index], following = index<log.Count-1 ? log.Keys[index+1] : DateTime.MaxValue; if(following == DateTime.MaxValue) { // last reading precedes at; extrapolate if(index==0) // there's only one reading return log[preceding]; else { DateTime nextPreceding = log.Keys[index-1]; return log[nextPreceding] + (at-nextPreceding).TotalSeconds * (log[preceding]-log[nextPreceding]) / (preceding-nextPreceding).TotalSeconds; } } else // both readings are available; interpolate return log[preceding] + (at-preceding).TotalSeconds * (log[following]-log[preceding]) / (following - preceding).TotalSeconds; } } public EncoderLog leftEnc = new EncoderLog(), rightEnc = new EncoderLog(); 

「ティックリスト」に加えて、各「ティック」時の位置のリストを PositionKeepingに保存する必要があります。この保存された位置から、移動した距離を測定して新しい位置を取得します。

  private SortedList<DateTime, Position> position = new SortedList<DateTime, Position> { { initialized, new Position(0, 0, Math.PI / 2) } }; public void Register(DateTime at, Position pos) { position.Add(at, pos); } public void Reset(DateTime at, Position pos) { // the position has changed => old ticks logs become obsolete leftEnc.Reset(at, leftEnc.ReadingAt(at)); rightEnc.Reset(at, rightEnc.ReadingAt(at)); position.Clear(); position.Add(at, pos); } public Position Current { get { return position.Last().Value; } } 

導出された公式に従って、両方のホイールセンサーのデータに従って新しい位置を計算することにより、 PositionKeepingクラスを補完します。 Updateはセンサーチェックサイクル（ ServiceHandlerBehavior.Concurrent ）から呼び出すことができ、サービスポートを引数として受け取り、ティックが登録されると新しい座標を送信します。 このメッセージはサービスの状態を変更するため、 ServiceHandlerBehavior.Exclusiveとして扱う必要があります。

  private static readonly TimeSpan RegisterDelay = TimeSpan.FromSeconds(1); // register null-tick if no actual ticks for this long public void Update(DateTime at, double left, double right, PositionOperations mainPort) { DateTime prevRef = Min(leftEnc.LastTick, rightEnc.LastTick); SetPosition set = new SetPosition { Timestamp = at, LeftEncUpdated = left != leftEnc.LastReading || at > leftEnc.LastTick + RegisterDelay, LeftEncReading = left, RightEncUpdated = right != rightEnc.LastReading || at > rightEnc.LastTick + RegisterDelay, RightEncReading = right }; if(set.LeftEncUpdated || set.RightEncUpdated) { set.Position = Recalculate(prevRef, left, right); mainPort.Post(set); } } private Position Recalculate(DateTime prevRef, double left, double right) { double sLeft = left - leftEnc.ReadingAt(prevRef), sRight = right - rightEnc.ReadingAt(prevRef); Position refPos = position[prevRef]; // has to exist if the encoder reference exists if (Math.Abs(sRight - sLeft) < .5) // less then half-tick difference: go straight return refPos.advance(Constants.CmPerTick * (sRight + sLeft) / 2, 0); else { double angle = Constants.CmPerTick * (sRight - sLeft) / Constants.WheelsDist, distance = Constants.WheelsDist * Math.Sin(angle / 2) * (sRight + sLeft) / (sRight - sLeft); return refPos.advance(distance, angle); } } } 

目的のコードに残っているのはSetPositionハンドラーだけです。これは、ロボットがターゲットに向かって移動するようにモーターの電圧を設定します。

 [ServiceHandler(ServiceHandlerBehavior.Exclusive)] public void SetPositionHandler(SetPosition set) { if (!set.LeftEncUpdated && !set.RightEncUpdated) { // position updated by an absolute reference. positionKeeping.Reset(set.Timestamp, set.Position); } else { if (set.LeftEncUpdated) positionKeeping.leftEnc.Register(set.Timestamp, set.LeftEncReading); if (set.RightEncUpdated) positionKeeping.rightEnc.Register(set.Timestamp, set.RightEncReading); positionKeeping.Register(set.Timestamp, set.Position); } // the navigator Destination dest = state.dest; double distance = set.Position.DistanceTo(dest.x, dest.y); if (distance < 5) // reached { drivePort.SetDrivePower(0, 0); SendNotification(submgrPort, new DriveDistance()); return; } double heading = Position.NormalizeHeading(Math.Atan2(dest.y - set.Position.y, dest.x - set.Position.x)), power = (distance < 50) ? .2 : .4; // a few magic numbers if (Math.Abs(heading) < .05) { // straight ahead drivePort.SetDrivePower(power, power); return; } double r = distance / (2 * Math.Sin(heading / 2)), hump = r * (1 - Math.Cos(heading / 2)); if (Math.Abs(heading) > Math.PI / 2 || Math.Abs(hump) > Constants.MaxHump) { // not reachable by an arc; rotate if (heading > 0) // rotate left drivePort.SetDrivePower(-.3, .3); else // rotate right drivePort.SetDrivePower(.3, -.3); } else { // go in arc double rLeft = Math.Abs(r - Constants.WheelsDist / 2), rRight = Math.Abs(r + Constants.WheelsDist / 2), rMax = Math.Max(rLeft, rRight); // <Patrician|Away> what does your robot do, sam // <bovril> it collects data about the surrounding environment, then discards it and drives into walls drivePort.SetDrivePower(power * Math.Pow(rLeft / rMax, 9), power * Math.Pow(rRight / rMax, 9)); } } 

次は？

このロボットはかなりうまく運転しましたが、累積エラーのために問題が発生しました。 座標を修正するには、バーコードの少なくとも20％を正常に読み取り、 LeftEncUpdated = RightEncUpdated = falseでLeftEncUpdated = RightEncUpdated = false呼び出してPositionKeepingを「再起動」 PositionKeeping SetPositionでLeftEncUpdated = RightEncUpdated = falseです。 バーコードの読み取り間隔が数メートルの場合、座標を決定する際のエラーは20 cmを超えませんでした-ロボットと棚の間にあまりにも多く残しました。

方向を修正するものが何もないため、ヘディングエラーの状況はさらに悪化しました。バーコードはどの角度からでも読み取ることができます（そして神に感謝します）。 数度の累積誤差は、ロボットがランダムに乗って全力で連隊に入るにはすでに十分です。 正面の解決策は、ジャイロスコープを設置することです。 しかし、Eddie用の既製のジャイロはありません。はんだごてとファームウェアの書き込みで謎を回避したいと思います。

ジャイロスコープや他のエキゾチックなセンサーなしで方向を修正するには、次の記事に専念します。