### Matlab代写 | CSE 298 – Homework 4

这个作业是用Matlab完成机器人编程

CSE 298 – Homework 4

Sonar Sensor Model:

1. In this problem, you will generate a sensor model for sonar measurements.

2. Assume that your sonar has a maximum range of 10 meters.

3. Write a function

r = DetectWall( robot, a_w, b_w )

that takes as an argument the robot struct and the slope/intercept (a_w,b_w) of the wall in the

world frame, and returns an ARRAY of range measurements from the sonars. In effect, this

function returns “ideal” sonar measurements. Note that the order of sonars should be from

[pi/15:pi/15:2*pi] in the robot frame. Also, only sonars that are within 10 meters of the wall

should return an appropriate value. If the true sonar range would be > 10 meters, you should

return a -1 in the respective array index.

4. We will now perturb the true wall measurements to model both random noise and outlier

measurements.

a. Assume that the range measurements are corrupted with random zeromean Gaussian

noise with standard deviation = 0.01ρ, where ρ is the actual range to the target.

b. Further assume that 25% of the sensor measurements suffer from multipath, resulting

in a range measurement error of +20-50%. You may assume that these measurements

are uniformly distributed.

5. You are to implement a function rHat = FireSonar( r ) that:

a. Take the ARRAY of true ranges from DetectWall as input.

b. Tests to see if each range was affected by multi-path error. If so, the appropriate

random error is added to the actual range measurement and returned from the sensor

model as rHat.

c. Otherwise, the range measurement is corrupted with appropriate random Gaussian

noise and returned as rHat.

d. Under no circumstances should the range returned exceed the maximum detection

range of the sensor. For ranges >10 m, the sonar will return a value of -1 (indicating that

no target was detected).

6. Helpful functions:

a. rand

b. randn

Least Squares Line Fitting:

1. For this problem, we will be using the least-squares line fitter from page 20 of the Robust

Estimation lecture notes.

2. Implement the following functions in Matlab

[ a_r, b_r] = LS_Line( rho, alpha );

[ a_w, b_w ] = Transform_Line( robot, a_r, b_r );

LS_Line takes an array of range measurements rho and an array of bearing angles alpha and

returns the least-squares solution [a_r, b_r] that corresponds to the slope and y-intercept line

parameters in the robot frame. The function Transform_Line takes these parameters and robot

struct as inputs and transforms these to parameters in the world frame. This will be useful for

plotting lines in the world frame. NOTE: You may solve this using the solution in the notes or

pseudo-inverse operation. However, you MAY NOT solve this using a built-in Matlab LS function.

Least-Squares & RANSAC Wallfollowers:

1. You are to implement a least-squares wallfollower LS_WallFollower( kP, kD, y_wall ) based upon

the LS_Line function above. kP and kD correspond to the proportional and derivative gains for

your controller. This will control the relative position and orientation of your robot with respect

to the wall described in Figure 1. y_wall denotes the y-intercept of the wall. You may assume

that it is always parallel to the x-axis.

2. To regulate the position and orientation of the robot, you will implement the PD controller

discussed in class during the feedback control lectures. You will need to choose appropriate

gains for the controller design.

3. You will first estimate the relative position and orientation of the robot with respect to the wall.

These inputs will be fed to the PD Controller which in turn will generate an output (ω) that will

be used to control the orientation of the robot.

4. To estimate the position/orientation with respect to the wall, you will:

a. Determine the range from each sonar to the wall using DetectWall and pass the

appropriate range array to FireSonar where they will be corrupted with noise.

b. From the corrupted range measurements, form an estimate of the position and

orientation for the wall relative to the robot by employing your LS_Line function. From

this, you can infer the relative distance and orientation of the wall for use in your PD

controller. The output of your PD controller will yield ω. Since v is constant, you can

move the robot appropriately. Note that robot motion is “perfect”, and updates occur at

2Hz.

c. The initial robot position will be taken as input from the user mouse (see the ginput

function). The initial orientation will be generated at random from θ [-π/4, π/4].

5. Repeat steps 1-4 by implementing a RANSAC_WallFollower(kP,kV) that will filter outliers from the

sonar measurements.

a. Determine the maximum number of sample trials n_max that you need to run RANSAC to

ensure you have a 99.9% probability of choosing the correct line (you may assume a 25%

outlier rate, although this is technically not quite correct).

b. Use the subset of sonar measurements that yields the largest consensus set after n_max

trials to fit the wall. In your figures, sonar measurements that were used above should be

drawn green, while outliers are drawn red.

6. Also, for both the LS and RANSAC wall followers, redraw the new wall as shown in Figure 2 where

the actual wall (blue), RANSAC wall (green) and LS wall (red) are all shown.

## 发表评论