ビジュアルサーボイングによる照準

1. Limelightとドライブトレインだけで、ロボットを正確かつ素早く照準することができます。
2. これらすべてを1時間以内に達成できます。

高フレームレートのビジョントラッキングを使用することで、ビジョンパイプラインをPID制御ループの「センサー」として直接使用し、ロボットやタレットを誘導することが可能になりました。このアイデアをテストするために、2017年のFRCロボットにLimelightを追加し、ドライブトレインとLimelightから報告されるネットワークテーブルデータのみを使用してビジョンターゲットに照準を合わせました。

この例では、テスト対象は6輪ドライブトレインとコルソンホイールを使用した2017年のFRCロボットでした。このテストを行うためにロボットにLimelightを追加している写真がこちらです。

次に、ドライバーがジョイスティックのボタンを押している間に実行されるコードをロボットに追加しました。このロボットは「タンク」スタイルの操縦を使用していたため、OperatorControl関数はドライブトレインの左右を制御するための'left_command'値と'right_command'値を生成していました。通常の制御コードの後に、次のようなコードブロックを追加しました：

float Kp = -0.1f;  // 比例制御定数

std::shared_ptr<NetworkTable> table = NetworkTable::GetTable("limelight");
float tx = table->GetNumber("tx");

if (joystick->GetRawButton(9))
{
	float heading_error = tx;
    steering_adjust = Kp * tx;
    
	left_command+=steering_adjust;
    right_command-=steering_adjust;
}

最初から、これはほぼ動作しました。ボタンを押している間、ロボットは自動的にターゲットの方向に回転します。ターゲットを動かすと、ロボットはターゲットを追従して回転します。しかし、ダッシュボードのライブビデオフィードを使用して、1つの大きな問題があることがわかりました：ロボットが常にターゲットと完全に整列するところまで到達していませんでした。小さなターゲットを使用するゲーム（2016年や2017年など）では、これでは十分ではありません。

これまでに実装したのは、単純な比例制御ループです。方位の誤差を計算し、それに定数を掛けることで、誤差に比例したモーターコマンドを作成しました。誤差がゼロに近づくと、コマンドもゼロに近づきます。問題は、ロボットが回転しようとするときに多くの摩擦が関係していることです。非常に小さなコマンドではロボットはまったく回転しません。小さな角度では、コマンドが小さすぎてロボットを実際に動かすことができなくなります。大きな照準誤差から始めるとロボットはターゲットにうまく到達しますが、最初から非常に近い場合はまったく照準できないことがあるかもしれません。

この問題を解決する方法はいくつかありますが、ここでは非常に簡単な解決策を紹介します。「最小コマンド」という概念を使用しました。誤差がある閾値より大きい場合、ロボットが実際に動くために必要な最小パワーをおおよそ表す定数をモーターコマンドに追加します（実際にはこれより少し小さい値を使用します）。新しいコードは次のようになります：

float Kp = -0.1f;
float min_command = 0.05f;

std::shared_ptr<NetworkTable> table = NetworkTable::GetTable("limelight");
float tx = table->GetNumber("tx");

if (joystick->GetRawButton(9))
{
	float heading_error = -tx;
	float steering_adjust = 0.0f;
	if (Math.abs(heading_error) > 1.0) 
	{
		if (heading_error < 0) 
		{
			steering_adjust = Kp*heading_error + min_command;
		} 
		else 
		{
			steering_adjust = Kp*heading_error - min_command;
		}
	} 
	left_command += steering_adjust;
	right_command -= steering_adjust;
}

注意：Kpまたはmin_commandを高く設定しすぎると、ロボットが不安定になり、ターゲットをオーバーシュートして前後に振動する可能性があります：

Kpとmin_commandを調整することで、ロボットがターゲットに非常に正確かつ素早く照準を合わせるようになるはずです。