Fine Alignment State Machine
Fine alignment begins after the platform has moved to the location output by coarse alignment and uses an RFID antenna, mounted to the platform, and two tags, affixed to the underside of the vehicle, to get a better estimation of the vehicle's location. The antenna area is approximately 20cm wide and 30cm long. It is cantilevered off of the top platform. By rotating the platform while stationary four events can be noted, assuming the coarse alignment solution was fairly accurate:
The angles at which these events occur define four lines. The intersections of each pair define the location of each tag, and the tags have a known position relative to the secondary coil. This localization process is illustrated in the video and diagram below.
- The first tag becomes visible
- The first tag is lost
- The second tag becomes visible
- The second tag is lost
The angles at which these events occur define four lines. The intersections of each pair define the location of each tag, and the tags have a known position relative to the secondary coil. This localization process is illustrated in the video and diagram below.
Fine Alignment Math
Once the four angles are known, the position of the car can be calculated as follows.
As measured from the forward driving direction of the car, in the parking spot frame (a vertical line in either diagram above) the four relevant angles are:
Then, if (x0,y0) is the location output from the coarse alignment stage (the location the platform is rotating about for fine alignment), WIDTH is the width of the area in which the antenna will register a tag, RADIUS is the distance of each tag from the center of the secondary (on the car) coil, and TAGANGLE is the angle of each tag from the forward direction of the car, the following series of equations (broken up for readability) yields the estimated actual location of the car, (xCar,yCar,phiCar)
x1=x0+(WIDTH/2)*((1/sin(theta11)+(1/sin(theta12)))*(1/(cot(theta11)-cot(theta12))))
y1=y0+cot(theta11)*(x1-x0)-(WIDTH/2)*(1/sin(theta11))
x2=x0+(WIDTH/2)*((1/sin(theta21)+(1/sin(theta22)))*(1/(cot(theta21)-cot(theta22))))
y2=y0+cot(theta22)*(x2-x0)+(WIDTH/2)*(1/sin(theta22))
phiCar=atan((y1-y2)/(x2-x1))
a=RADIUS*sin(phiCar+TAGANGLE)
b=RADIUS*cos(phiCar+TAGANGLE)
xCar=(x1+x2+b-a)/2
yCar=(y1+y2-a-b)/2
Note that this math generalizes well to other problems in which the goal is to find the center of a square or rectangle given the location of 2 of the corners. A very similar procedure could be used to solve the coarse alignment problem. Note that we started with 4 pieces of information (the 4 angles, or, equivalently, the(x,y) locations of 2 corners), trying to solve for 3 unknowns. Each of the two final equations (xCar and yCar) are actually the average of 2 quantities that should each be equal to the left hand side. This is one way to incorporate the extra information. There are other ways to incorporate the extra information (using some of the redundancy on phi), so this "solution" is not unique.
As measured from the forward driving direction of the car, in the parking spot frame (a vertical line in either diagram above) the four relevant angles are:
- theta11: The angle at which the first tag is found
- theta12: The angle at which the first tag is lost
- theta21: The angle at which the second tag is found
- theta22: The angle at which the second tag is lost
Then, if (x0,y0) is the location output from the coarse alignment stage (the location the platform is rotating about for fine alignment), WIDTH is the width of the area in which the antenna will register a tag, RADIUS is the distance of each tag from the center of the secondary (on the car) coil, and TAGANGLE is the angle of each tag from the forward direction of the car, the following series of equations (broken up for readability) yields the estimated actual location of the car, (xCar,yCar,phiCar)
x1=x0+(WIDTH/2)*((1/sin(theta11)+(1/sin(theta12)))*(1/(cot(theta11)-cot(theta12))))
y1=y0+cot(theta11)*(x1-x0)-(WIDTH/2)*(1/sin(theta11))
x2=x0+(WIDTH/2)*((1/sin(theta21)+(1/sin(theta22)))*(1/(cot(theta21)-cot(theta22))))
y2=y0+cot(theta22)*(x2-x0)+(WIDTH/2)*(1/sin(theta22))
phiCar=atan((y1-y2)/(x2-x1))
a=RADIUS*sin(phiCar+TAGANGLE)
b=RADIUS*cos(phiCar+TAGANGLE)
xCar=(x1+x2+b-a)/2
yCar=(y1+y2-a-b)/2
Note that this math generalizes well to other problems in which the goal is to find the center of a square or rectangle given the location of 2 of the corners. A very similar procedure could be used to solve the coarse alignment problem. Note that we started with 4 pieces of information (the 4 angles, or, equivalently, the(x,y) locations of 2 corners), trying to solve for 3 unknowns. Each of the two final equations (xCar and yCar) are actually the average of 2 quantities that should each be equal to the left hand side. This is one way to incorporate the extra information. There are other ways to incorporate the extra information (using some of the redundancy on phi), so this "solution" is not unique.