使用 Sphero 映射环境，如何获得更好的精度？

Posted 2023-02-24

【中文标题】使用 Sphero 映射环境，如何获得更好的精度？

【英文标题】：Using Sphero to map an environnement, how to get a better accuracy?

【发布时间】：2014-12-09 17:20:34

【问题描述】：

我为 sphero 开发了一个应用程序，让它可以在房子等环境中旅行。目的是绘制环境图。我设法恢复位置和碰撞点，但似乎这些碰撞会破坏位置。我认为 Sphero 在遇到障碍物时会偏离其路径。有没有人知道适当的设置以获得最佳精度？低速？基于方向等其他参数的校正？

这是我的代码，可以在https://github.com/scenaristeur/sphero-carto 下载应用程序：

package orbotix.uisample;
import java.io.File;
import java.io.FileWriter;
import java.io.IOException;  
import java.util.ArrayList;
import java.util.Random;
import android.content.BroadcastReceiver;
import android.content.Context;
import android.content.Intent;
import android.content.IntentFilter;
import android.os.Bundle;
import android.os.Environment;
import android.provider.Settings;
import android.util.Log;
import android.view.MotionEvent;
import android.view.View;
import android.widget.CheckBox;
import android.widget.TextView;
import android.widget.Toast;
import orbotix.robot.app.ColorPickerActivity;
import orbotix.robot.base.CollisionDetectedAsyncData;
import orbotix.robot.base.Robot;
import orbotix.robot.base.CollisionDetectedAsyncData.CollisionPower;
import orbotix.robot.sensor.Acceleration;
import orbotix.robot.sensor.LocatorData;
import orbotix.robot.widgets.CalibrationImageButtonView;
import orbotix.robot.widgets.NoSpheroConnectedView;
import orbotix.robot.widgets.NoSpheroConnectedView.OnConnectButtonClickListener;
import orbotix.robot.widgets.SlideToSleepView;
import orbotix.robot.widgets.joystick.JoystickView;
import orbotix.sphero.CollisionListener;
import orbotix.sphero.ConnectionListener;
import orbotix.sphero.LocatorListener;
import orbotix.sphero.Sphero;
import orbotix.view.calibration.CalibrationView;
import orbotix.view.calibration.ControllerActivity;
import orbotix.view.connection.SpheroConnectionView;


public class UiSampleActivity extends ControllerActivity 
/** ID to start the StartupActivity for result to connect the Robot */
private final static int STARTUP_ACTIVITY = 0;
private static final int BLUETOOTH_ENABLE_REQUEST = 11;
private static final int BLUETOOTH_SETTINGS_REQUEST = 12;

/** ID to start the ColorPickerActivity for result to select a color */
private final static int COLOR_PICKER_ACTIVITY = 1;
private boolean mColorPickerShowing = false;

/** The Robot to control */
private Sphero mRobot;

/** One-Touch Calibration Button */
private CalibrationImageButtonView mCalibrationImageButtonView;

/** Calibration View widget */
private CalibrationView mCalibrationView;

/** Slide to sleep view */
private SlideToSleepView mSlideToSleepView;

/** No Sphero Connected Pop-Up View */
private NoSpheroConnectedView mNoSpheroConnectedView;

/** Sphero Connection View */
private SpheroConnectionView mSpheroConnectionView;

//Colors
private int mRed = 0xff;
private int mGreen = 0xff;
private int mBlue = 0xff;
private float positionX;
private float positionY;
private float velocityX;
private float velocityY;
private String couleur="vert";
private float angle=0f;
private float vitesse=0.6f;
private boolean stop;
private boolean auto;
private int countCollision=0;
private int maxCollision=10;
private float accelX;
private float accelY;
private float powerX;
private float powerY;
private static final String TAG = "OBX-LocatorSmag";
private Point point=new Point();
private ArrayList<Point> points=new ArrayList<Point>();
File repertoire= new File (Environment.getExternalStorageDirectory()+"/smag-sphero");

private LocatorListener mLocatorListener = new LocatorListener() 
    @Override
    public void onLocatorChanged(LocatorData locatorData) 
        Log.d(TAG, locatorData.toString());
        if (locatorData != null) 
            positionX=locatorData.getPositionX();
            positionY=locatorData.getPositionY();
            velocityX=locatorData.getVelocityX();
            velocityY=locatorData.getVelocityY();
            point=new Point(positionX,positionY,velocityX,velocityY,couleur);
            points.add(point);

            if ((auto)&&(Math.abs((velocityX))<.01)&&(Math.abs(velocityY)<.01))

                chercheDirection();
            
            //Toast.makeText(UiSampleActivity.this,"X:"+positionX, Toast.LENGTH_SHORT).show();
           // ((TextView) findViewById(R.id.txt_locator_x)).setText(locatorData.getPositionX() + " cm");
           // ((TextView) findViewById(R.id.txt_locator_y)).setText(locatorData.getPositionY() + " cm");
           // ((TextView) findViewById(R.id.txt_locator_vx)).setText(locatorData.getVelocityX() + " cm/s");
           // ((TextView) findViewById(R.id.txt_locator_vy)).setText(locatorData.getVelocityY() + " cm/s");
        
    
    ;

private BroadcastReceiver mColorChangeReceiver = new BroadcastReceiver() 
    @Override
    public void onReceive(Context context, Intent intent) 
        // update colors
        int red = intent.getIntExtra(ColorPickerActivity.EXTRA_COLOR_RED, 0);
        int green = intent.getIntExtra(ColorPickerActivity.EXTRA_COLOR_GREEN, 0);
        int blue = intent.getIntExtra(ColorPickerActivity.EXTRA_COLOR_BLUE, 0);

        // change the color on the ball
        mRobot.setColor(red, green, blue);
    
;

/** Called when the activity is first created. */
@Override
public void onCreate(Bundle savedInstanceState) 
    super.onCreate(savedInstanceState);
    setContentView(R.layout.main);
    if (!repertoire.exists()) 
          repertoire.mkdir();
        

    // Set up the Sphero Connection View
    mSpheroConnectionView = (SpheroConnectionView) findViewById(R.id.sphero_connection_view);

    mSpheroConnectionView.addConnectionListener(new ConnectionListener() 

        @Override
        public void onConnected(Robot robot) 
            // Set Robot
            mRobot = (Sphero) robot; // safe to cast for now
            //Set connected Robot to the Controllers
            setRobot(mRobot);
            //active LocatorListener;
            mRobot.getSensorControl().addLocatorListener(mLocatorListener);
            mRobot.getSensorControl().setRate(5);
            mRobot.getCollisionControl().addCollisionListener(mCollisionListener);
            mRobot.getCollisionControl().startDetection(45, 45, 45, 45, 100);
            // Make sure you let the calibration view knows the robot it should control
            mCalibrationView.setRobot(mRobot);

            // Make connect sphero pop-up invisible if it was previously up
            mNoSpheroConnectedView.setVisibility(View.GONE);
            mNoSpheroConnectedView.switchToConnectButton();
            Toast.makeText(UiSampleActivity.this,"Déplacez Sphéro, cliquez ensuite sur Sleep pour enregistrer le parcours dans /smag-sphero", Toast.LENGTH_SHORT).show();

            if (repertoire.isDirectory())
                Toast.makeText(UiSampleActivity.this,"positions enregistrées dans /smag-sphero", Toast.LENGTH_SHORT).show();

            else
                Toast.makeText(UiSampleActivity.this,"création du repertoire /smag-sphero", Toast.LENGTH_SHORT).show();
            
        

        @Override
        public void onConnectionFailed(Robot sphero) 
            // let the SpheroConnectionView handle or hide it and do something here...
        

        @Override
        public void onDisconnected(Robot sphero) 
            if (points!=null)ecrireFichier();
            mSpheroConnectionView.startDiscovery();

        
    );

    //Add the JoystickView as a Controller
    addController((JoystickView) findViewById(R.id.joystick));

    // Add the calibration view
    mCalibrationView = (CalibrationView) findViewById(R.id.calibration_view);

    // Set up sleep view
    mSlideToSleepView = (SlideToSleepView) findViewById(R.id.slide_to_sleep_view);
    mSlideToSleepView.hide();
    // Send ball to sleep after completed widget movement
    mSlideToSleepView.setOnSleepListener(new SlideToSleepView.OnSleepListener() 
        @Override
        public void onSleep() 
            mRobot.sleep(0);
        
    );

    // Initialize calibrate button view where the calibration circle shows above button
    // This is the default behavior
    mCalibrationImageButtonView = (CalibrationImageButtonView) findViewById(R.id.calibration_image_button);
    mCalibrationImageButtonView.setCalibrationView(mCalibrationView);
    // You can also change the size and location of the calibration views (or you can set it in XML)
    mCalibrationImageButtonView.setRadius(100);
    mCalibrationImageButtonView.setOrientation(CalibrationView.CalibrationCircleLocation.ABOVE);

    // Grab the No Sphero Connected View
    mNoSpheroConnectedView = (NoSpheroConnectedView) findViewById(R.id.no_sphero_connected_view);
    mNoSpheroConnectedView.setOnConnectButtonClickListener(new OnConnectButtonClickListener() 

        @Override
        public void onConnectClick() 
            mSpheroConnectionView.setVisibility(View.VISIBLE);
            mSpheroConnectionView.startDiscovery();
        

        @Override
        public void onSettingsClick() 
            // Open the Bluetooth Settings Intent
            Intent settingsIntent = new Intent(Settings.ACTION_BLUETOOTH_SETTINGS);
            UiSampleActivity.this.startActivityForResult(settingsIntent, BLUETOOTH_SETTINGS_REQUEST);
        
    );





/** Called when the user comes back to this app */
@Override
protected void onResume() 
    super.onResume();
    if (mColorPickerShowing) 
        mColorPickerShowing = false;
        return;
    

    Log.d("", "registering Color Change Listener");
    IntentFilter filter = new IntentFilter(ColorPickerActivity.ACTION_COLOR_CHANGE);
    registerReceiver(mColorChangeReceiver, filter);


/** Called when the user presses the back or home button */
@Override
protected void onPause() 
    super.onPause();
    if (mColorPickerShowing) return;
ecrireFichier();
    // Disconnect Robot properly
    if (mRobot != null) 
        mRobot.disconnect();
    
    try 
        unregisterReceiver(mColorChangeReceiver); // many times throws exception on leak
     catch (Exception e) 
    


@Override
protected void onActivityResult(int requestCode, int resultCode, Intent data) 
    super.onActivityResult(requestCode, resultCode, data);

    if (resultCode == RESULT_OK) 
        if (requestCode == COLOR_PICKER_ACTIVITY) 
            //Get the colors
            mRed = data.getIntExtra(ColorPickerActivity.EXTRA_COLOR_RED, 0xff);
            mGreen = data.getIntExtra(ColorPickerActivity.EXTRA_COLOR_GREEN, 0xff);
            mBlue = data.getIntExtra(ColorPickerActivity.EXTRA_COLOR_BLUE, 0xff);

            //Set the color
            mRobot.setColor(mRed, mGreen, mBlue);
         else if (requestCode == BLUETOOTH_ENABLE_REQUEST) 
            // User enabled bluetooth, so refresh Sphero list
            mSpheroConnectionView.setVisibility(View.VISIBLE);
            mSpheroConnectionView.startDiscovery();
        
     else 
        if (requestCode == STARTUP_ACTIVITY) 
            // Failed to return any robot, so we bring up the no robot connected view
            mNoSpheroConnectedView.setVisibility(View.VISIBLE);
         else if (requestCode == BLUETOOTH_ENABLE_REQUEST) 

            // User clicked "NO" on bluetooth enable settings screen
            Toast.makeText(UiSampleActivity.this,
                    "Enable Bluetooth to Connect to Sphero", Toast.LENGTH_LONG).show();
         else if (requestCode == BLUETOOTH_SETTINGS_REQUEST) 
            // User enabled bluetooth, so refresh Sphero list
            mSpheroConnectionView.setVisibility(View.VISIBLE);
            mSpheroConnectionView.startDiscovery();
        
    


/**
 * When the user clicks the "Color" button, show the ColorPickerActivity
 *
 * @param v The Button clicked
 */
public void onColorClick(View v) 

    mColorPickerShowing = true;
    Intent i = new Intent(this, ColorPickerActivity.class);

    //Tell the ColorPickerActivity which color to have the cursor on.
    i.putExtra(ColorPickerActivity.EXTRA_COLOR_RED, mRed);
    i.putExtra(ColorPickerActivity.EXTRA_COLOR_GREEN, mGreen);
    i.putExtra(ColorPickerActivity.EXTRA_COLOR_BLUE, mBlue);

    startActivityForResult(i, COLOR_PICKER_ACTIVITY);


public void onAutoClick(View v) 
    mRobot.setColor(150, 150, 0);
     Toast.makeText(UiSampleActivity.this,"Auto", Toast.LENGTH_SHORT).show();
    // for (int i=0;i<10;i++)
     auto=true;
     mRobot.drive(angle, vitesse);



/**
 * When the user clicks the "Sleep" button, show the SlideToSleepView shows
 *
 * @param v The Button clicked
 */
public void onSleepClick(View v) 
    ecrireFichier();
    mSlideToSleepView.show();


public void onWriteClick(View v)
    String message= "X :"+positionX+" Y : "+positionY;
     Toast.makeText(UiSampleActivity.this,"Ecriture fichier"+message, Toast.LENGTH_LONG).show();

    /* String NOTES ="info.txt";
     File file = new File(Environment.getExternalStorageDirectory(), NOTES);

     String Info = "Test Text";
     try 
        file.createNewFile();
         FileWriter filewriter = new FileWriter(file,false);
         filewriter.write(Info);
         filewriter.close();
         Toast.makeText(UiSampleActivity.this,"OK", Toast.LENGTH_LONG).show();

     catch (IOException e) 
        // TODO Auto-generated catch block
        e.printStackTrace();
         Toast.makeText(UiSampleActivity.this,"Ecriture plantée"+message, Toast.LENGTH_LONG).show();

    */


public void onStopClick(View v) 
    if (mRobot != null) 
        // Stop robot
        stop=true;
        auto=false;
        mRobot.stop();
    



@Override
public boolean dispatchTouchEvent(MotionEvent event) 
    mCalibrationView.interpretMotionEvent(event);
    mSlideToSleepView.interpretMotionEvent(event);
    return super.dispatchTouchEvent(event);

private final CollisionListener mCollisionListener = new CollisionListener() 
    public void collisionDetected(CollisionDetectedAsyncData collisionData) 
        mRobot.setColor(200, 0, 0);
        Acceleration acceleration = collisionData.getImpactAcceleration();
        CollisionPower power = collisionData.getImpactPower();
        stop=true;
        accelX=(float) acceleration.x;
        accelY=(float) acceleration.y;
        powerX=power.x;
        powerY=power.y;
        couleur="rouge";
        point=new Point(positionX,positionY,velocityX,velocityY,couleur);
        points.add(point);
        couleur="vert";
        countCollision++;

         Toast.makeText(UiSampleActivity.this,countCollision+" Collision "+angle, Toast.LENGTH_SHORT).show();
         if (auto)
             mRobot.setColor(20,0,0);
chercheDirection();

//mRobot.drive(angle,vitesse);
         
    /*    // Update the UI with the collision data
        Acceleration acceleration = collisionData.getImpactAcceleration();
        mAccelXValueLabel = (TextView) findViewById(R.id.accel_x_value);
        mAccelXValueLabel.setText("" + acceleration.x);

        mAccelYValueLabel = (TextView) findViewById(R.id.accel_y_value);
        mAccelYValueLabel.setText("" + acceleration.y);

        mAccelZValueLabel = (TextView) findViewById(R.id.accel_z_value);
        mAccelZValueLabel.setText("" + acceleration.z);

        mXAxisCheckBox = (CheckBox) findViewById(R.id.axis_x_checkbox);
        mXAxisCheckBox.setChecked(collisionData.hasImpactXAxis());

        mYAxisCheckBox = (CheckBox) findViewById(R.id.axis_y_checkbox);
        mYAxisCheckBox.setChecked(collisionData.hasImpactYAxis());

        CollisionPower power = collisionData.getImpactPower();
        mPowerXValueLabel = (TextView) findViewById(R.id.power_x_value);
        mPowerXValueLabel.setText("" + power.x);

        mPowerYValueLabel = (TextView) findViewById(R.id.power_y_value);
        mPowerYValueLabel.setText("" + power.y);

        mSpeedValueLabel = (TextView) findViewById(R.id.speed_value);
        mSpeedValueLabel.setText("" + collisionData.getImpactSpeed());

        mTimestampLabel = (TextView) findViewById(R.id.time_stamp_value);
        mTimestampLabel.setText(collisionData.getImpactTimeStamp() + " ms");*/
    
;


public void chercheDirection()
    //mRobot.drive(angle, vitesse);

    if((velocityX==0)&&(velocityY==0))

        mRobot.setColor(0, 0, 20);

    Random rand = new Random();
             float variation = rand.nextFloat();
        /* float sens=rand.nextFloat();
         if (sens>.5)
        angle=angle+(180f*variation);
         else
             angle=angle-(180f*variation); 
         */
        angle=angle+180f*variation;
        if(angle>=360f)
            angle-=360;
        
        mRobot.setColor(20,0,0);
        //vitesse=0.2f;
        // Toast.makeText(UiSampleActivity.this,"demi tour "+angle+" "+vitesse, Toast.LENGTH_SHORT).show();

        mRobot.drive(angle,vitesse);
        //vitesse=0.6f;
        try 
            Thread.sleep(250);
         catch (InterruptedException e) 
            // TODO Auto-generated catch block
            e.printStackTrace();
        
        mRobot.setColor(20,20,0);
        mRobot.drive(angle,vitesse);
    


public void ecrireFichier()
    String message= ""; 

    //ecriture
     String cheminTime ="smag-sphero/Smag-Sphero"+System.currentTimeMillis()+".txt";
    String cheminLast ="smag-sphero/Smag-Sphero-last.txt";
     File file = new File(Environment.getExternalStorageDirectory(), cheminTime);
     File fileLast = new File(Environment.getExternalStorageDirectory(), cheminLast);
     Point dernier=null;
      for (int i=0;i<points.size();i++)
          Point p=points.get(i);
          dernier=p;
          message=message+p.getX()+","+p.getY()+","+p.getVelX()+","+p.getVelY()+","+p.getCouleur()+","+p.getAccelX()+","+p.getAccelY()+","+p.getPowerX()+","+p.getPowerY()+"\n";
      
      message=message+dernier.getX()+","+dernier.getY()+","+dernier.getVelX()+","+dernier.getVelY()+",bleu,"+dernier.getAccelX()+","+dernier.getAccelY()+","+dernier.getPowerX()+","+dernier.getPowerY()+"\n";
     String Info = message;
     try 
        file.createNewFile();
     FileWriter filewriter = new FileWriter(file,true);
     filewriter.write(Info);
     filewriter.close();

        //pour last
     if (fileLast.exists())
         fileLast.delete();
         
        fileLast.createNewFile();
         FileWriter filewriterLast = new FileWriter(fileLast,true);
         filewriterLast.write(Info);
         filewriterLast.close();
         Toast.makeText(UiSampleActivity.this,"OK", Toast.LENGTH_LONG).show();
     catch (IOException e) 
        // TODO Auto-generated catch block
        e.printStackTrace();
     Toast.makeText(UiSampleActivity.this,"Ecriture plantée"+message, Toast.LENGTH_LONG).show();

    

【参考方案1】：

如果您只想绘制环境边界（平面图），您可以沿着墙壁行驶。

您可以分析 imu 传感器的滚动值，并在以 0.15 的功率行驶时将其与左侧墙壁保持 35° 左右。

当球体击中凹角时，速度会降至零，您必须向右转一点，直到他再次移动。

当球体沿凸角移动时，滚动值将下降到 0° 或更小。您必须保存该位置，停止 sphero 并计算返回该保存点的方向，以便从那里继续进行映射。