Suppose a grid is defined by a set of grid parameters: its origin (x0, y0), an angel between one side and x axis, and increments and - please see the figure below.
There are scattered points with known coordinates on the grid but they don’t exactly fall on grid intersections. Is there an algorithm to find a set of grid parameters to define the grid so that the points are best fit to grid intersections?
Suppose the known coordinates are:
(2 , 5.464), (3.732, 6.464), (5.464, 7.464)
(3 , 3.732), (4.732, 4.732), (6.464, 5.732)
(4 , 2 ), (5.732, 3 ), (7.464, 4 ).
I expect the algorithm to find the origin (4, 2), the angle 30 degree, and the increments both 2.
You can solve the problem by finding a matrix that transforms points from positions (0, 0), (0, 1), ... (2, 2) onto the given points.
Although the grid has only 5 degrees of freedom (position of the origin + angle + scale), it is easier to define the transformation using 2x3 matrix A, because the problem can be made linear in this case.
Let a point with index (x0, y0) to be transformed into point (x0', y0') on the grid, for example (0, 0) -> (2, 5.464) and let a_ij be coefficients of matrix A. Then this pair of points results in 2 equations:
a_00 * x0 + a_01 * y0 + a_02 = x0'
a_10 * x0 + a_11 * y0 + a_12 = y0'
The unknowns are a_ij, so these equations can be written in form
a_00 * x0 + a_01 * y0 + a_02 + a_10 * 0 + a_11 * 0 + a_12 * 0 = x0'
a_00 * 0 + a_01 * 0 + a_02 * 0 + a_10 * x0 + a_11 * y0 + a_12 = y0'
or in matrix form
K0 * (a_00, a_01, a_02, a_10, a_11, a_12)^T = (x0', y0')^T
where
K0 = (
x0, y0, 1, 0, 0, 0
0, 0, 0, x0, y0, 1
)
These equations for each pair of points can be combined in a single equation
K * (a_00, a_01, a_02, a_10, a_11, a_12)^T = (x0', y0', x1', y1', ..., xn', yn')^T
or K * a = b
where
K = (
x0, y0, 1, 0, 0, 0
0, 0, 0, x0, y0, 1
x1, y1, 1, 0, 0, 0
0, 0, 0, x1, y1, 1
...
xn, yn, 1, 0, 0, 0
0, 0, 0, xn, yn, 1
)
and (xi, yi), (xi', yi') are pairs of corresponding points
This can be solved as a non-homogeneous system of linear equations. In this case the solution will minimize sum of squares of distances from each point to nearest grid intersection. This transform can be also considered to maximize overall likelihood given the assumption that points are shifted from grid intersections with normally distributed noise.
a = (K^T * K)^-1 * K^T * b
This algorithm can be easily implemented if there is a linear algebra library is available. Below is an example in Python:
import numpy as np
n_points = 9
aligned_points = [(0, 0), (0, 1), (0, 2), (1, 0), (1, 1), (1, 2), (2, 0), (2, 1), (2, 2)]
grid_points = [(2, 5.464), (3.732, 6.464), (5.464, 7.464), (3, 3.732), (4.732, 4.732), (6.464, 5.732), (4, 2), (5.732, 3), (7.464, 4)]
K = np.zeros((n_points * 2, 6))
b = np.zeros(n_points * 2)
for i in range(n_points):
K[i * 2, 0] = aligned_points[i, 0]
K[i * 2, 1] = aligned_points[i, 1]
K[i * 2, 2] = 1
K[i * 2 + 1, 3] = aligned_points[i, 0]
K[i * 2 + 1, 4] = aligned_points[i, 1]
K[i * 2 + 1, 5] = 1
b[i * 2] = grid_points[i, 0]
b[i * 2 + 1] = grid_points[i, 1]
# operator '#' is matrix multiplication
a = np.linalg.inv(np.transpose(K) # K) # np.transpose(K) # b
A = a.reshape(2, 3)
print(A)
[[ 1. 1.732 2. ]
[-1.732 1. 5.464]]
Then the parameters can be extracted from this matrix:
theta = math.degrees(math.atan2(A[1, 0], A[0, 0]))
scale_x = math.sqrt(A[1, 0] ** 2 + A[0, 0] ** 2)
scale_y = math.sqrt(A[1, 1] ** 2 + A[0, 1] ** 2)
origin_x = A[0, 2]
origin_y = A[1, 2]
theta = -59.99927221917264
scale_x = 1.99995599951599
scale_y = 1.9999559995159895
origin_x = 1.9999999999999993
origin_y = 5.464
However there remains a minor issue: matrix A corresponds to an affine transform. This means that grid axes are not guaranteed to be perpendicular. If this is a problem, then the first two columns of the matrix can be modified in a such way that the transform preserves angles.
Update: I fixed the mistakes and resolved sign ambiguities, so now this algorithm produces the expected result. However it should be tested to see if all cases are handled correctly.
Here is another attempt to solve this problem. The idea is to decompose transformation into non-uniform scaling matrix and rotation matrix A = R * S and then solve for coefficients sx, sy, r1, r2 of these matrices given restriction that r1^2 + r2^2 = 1. The minimization problem is described here: How to find a transformation (non-uniform scaling and similarity) that maps one set of points to another?
def shift_points(points):
n_points = len(points)
shift = tuple(sum(coords) / n_points for coords in zip(*points))
shifted_points = [(point[0] - shift[0], point[1] - shift[1]) for point in points]
return shifted_points, shift
n_points = 9
aligned_points = [(0, 0), (0, 1), (0, 2), (1, 0), (1, 1), (1, 2), (2, 0), (2, 1), (2, 2)]
grid_points = [(2, 5.464), (3.732, 6.464), (5.464, 7.464), (3, 3.732), (4.732, 4.732), (6.464, 5.732), (4, 2), (5.732, 3), (7.464, 4)]
aligned_points, aligned_shift = shift_points(aligned_points)
grid_points, grid_shift = shift_points(grid_points)
c1, c2 = 0, 0
b11, b12, b21, b22 = 0, 0, 0, 0
for i in range(n_points):
c1 += aligned_points[i][0] ** 2
c2 += aligned_points[i][0] ** 2
b11 -= 2 * aligned_points[i][0] * grid_points[i][0]
b12 -= 2 * aligned_points[i][1] * grid_points[i][0]
b21 -= 2 * aligned_points[i][0] * grid_points[i][1]
b22 -= 2 * aligned_points[i][1] * grid_points[i][1]
k = (b11 ** 2 * c2 + b22 ** 2 * c1 - b21 ** 2 * c2 - b12 ** 2 * c1) / \
(b21 * b11 * c2 - b12 * b22 * c1)
# r1_sqr and r2_sqr might need to be swapped
r1_sqr = 2 / (k ** 2 + 4 + k * math.sqrt(k ** 2 + 4))
r2_sqr = 2 / (k ** 2 + 4 - k * math.sqrt(k ** 2 + 4))
for sign1, sign2 in [(1, 1), (-1, 1), (1, -1), (-1, -1)]:
r1 = sign1 * math.sqrt(r1_sqr)
r2 = sign2 * math.sqrt(r2_sqr)
scale_x = -b11 / (2 * c1) * r1 - b21 / (2 * c1) * r2
scale_y = b12 / (2 * c2) * r2 - b22 / (2 * c2) * r1
if scale_x >= 0 and scale_y >= 0:
break
theta = math.degrees(math.atan2(r2, r1))
There might be ambiguities in choosing r1_sqr and r2_sqr. Origin point can be estimated from aligned_shift and grid_shift, but I didn't implement it yet.
theta = -59.99927221917264
scale_x = 1.9999559995159895
scale_y = 1.9999559995159895
I'm trying to figure out an algorithm for setting up a rendezvous between two spaceships.
There is no gravity or drag. Both spaceships have a position and a velocity at the start. Spaceship B continues on its course with no acceleration, so spaceship A needs to accelerate to close the distance between them and then match velocities when it arrives at the position of spaceship B.
The spaceship can instantly change its direction of thrust, but can only use maximum acceleration or no acceleration at all. I also want a limit on the velocity difference between the spaceships during the maneuver.
I would like the output to be in the form of a number of trajectory legs, i.e: leg1: accelerate direction x for t1 seconds,
leg2: coast for t2 seconds,
leg3: accelerate direction y for t3 seconds.
I don't need an optimal solution, but I would like it to "look right".
I tried to make an impulse to equalize the velocities and add it to an impulse for moving towards spaceship B, but even though spaceship A ends up with the correct velocity, it fails to reach the targets position. I've tried the impulses by themselves and they seem to perform as expected, so I'm guessing it's the way I'm adding them together that is the problem. I don't know if I am implementing it incorrectly or if this approach simply won't work. I'm hoping someone with stronger math and physics skills can enlighten me.
Here is the code I am using:
// var velocityAdjustmentTime = (int)Math.Sqrt(2 * velocityDelta.Length / tp.Acceleration);
var velocityAdjustmentTime = (int)(velocityDelta.Length / tp.Acceleration);
var velocityAdjustVector = velocityDelta;
velocityAdjustVector.Normalize();
velocityAdjustVector *= tp.Acceleration;
var targetAccelerationDisplacement = new Vector3D(0, 0, 0); // TODO: Replace this with proper values.
Vector3D newPosition;
Vector3D newVelocity;
Vector3D targetNewPosition;
// Check if the formation and the target already have a paralell course with the same velocity.
if (velocityAdjustmentTime > 0)
{
// If not, calculate the position and velocity after the velocity has been aligned.
newPosition = tp.StartPosition + (tp.StartVelocity * velocityAdjustmentTime) + ((velocityAdjustVector * velocityAdjustmentTime * velocityAdjustmentTime) / 2);
newVelocity = tp.StartVelocity + velocityAdjustVector * velocityAdjustmentTime;
targetNewPosition = tp.TargetStartPosition + (tp.TargetStartVelocity * velocityAdjustmentTime) + targetAccelerationDisplacement;
}
else
{
// Else, new and old is the same.
newPosition = tp.StartPosition;
newVelocity = tp.StartVelocity;
targetNewPosition = tp.TargetStartPosition;
}
// Get the new direction from the position after velocity change.
var newDirection = targetNewPosition - newPosition;
// Changing this value moves the end position closer to the target. Thought it would be newdirection length, but then it doesn't reach the target.
var length = newDirection.Length;
// I don't think this value matters.
var speed = (int)(cruiseSpeed);
var legTimes = CalculateAccIdleDecLegs(tp.Acceleration, length, speed);
// Sets how much of the velocity change happens on the acceleration or deceleration legs.
var velFactorAcc = 1;
var velFactorDec = 1 - velFactorAcc;
// Make the acceleration vector.
accelerationVector = newDirection;
accelerationVector.Normalize();
accelerationVector *= legTimes[0] * tp.Acceleration;
accelerationVector += velocityDelta * velFactorAcc;
accelerationTime = (int)(accelerationVector.Length / tp.Acceleration);
accelerationVector.Normalize();
accelerationVector *= tp.Acceleration;
// Make the acceleration order.
accelerationLeg.Acceleration = accelerationVector;
accelerationLeg.Duration = accelerationTime;
// Make the deceleration vector.
decelerationVector = newDirection;
decelerationVector.Negate();
decelerationVector.Normalize();
decelerationVector *= legTimes[2] * tp.Acceleration;
decelerationVector += velocityDelta * velFactorDec;
decelerationTime = (int)(decelerationVector.Length / tp.Acceleration);
decelerationVector.Normalize();
decelerationVector *= tp.Acceleration;
// And deceleration order.
decelerationLeg.Acceleration = decelerationVector;
decelerationLeg.Duration = decelerationTime;
// Add the orders to the list.
trajectory.Add(accelerationLeg);
// Check if there is an idle leg in the middle...
if (legTimes[1] > 0)
{
// ... if so, make the order and add it to the list.
idleLeg.Duration = legTimes[1];
trajectory.Add(idleLeg);
}
// Add the deceleration order.
trajectory.Add(decelerationLeg);
And the function for calculating the approach legs:
private static int[] CalculateAccIdleDecLegs(double acceleration, double distance, int cruiseSpeed)
{
int[] legDurations = new int[3];
int accelerationTime;
int idleTime;
int decelerationTime;
// Calculate the max speed it's possible to accelerate before deceleration needs to begin.
var topSpeed = Math.Sqrt(acceleration * distance);
// If the cruise speed is higher than or equal to the possible top speed, the formation should accelerate to top speed and then decelerate.
if (cruiseSpeed >= topSpeed)
{
// Get the time to accelerate to the max velocity.
accelerationTime = (int)((topSpeed) / acceleration);
// Idle time is zero.
idleTime = 0;
// Get the deceleration time.
decelerationTime = (int)(topSpeed / acceleration);
}
// Else, the formation should accelerate to max velocity and then coast until it starts decelerating.
else
{
// Find the acceleration time.
accelerationTime = (int)((cruiseSpeed) / acceleration);
// Get the deceleration time.
decelerationTime = (int)(cruiseSpeed / acceleration);
// Calculate the distance traveled while accelerating.
var accelerationDistance = 0.5 * acceleration * accelerationTime * accelerationTime;
// Calculate the distance traveled while decelerating.
var decelerationDistance = 0.5 * acceleration * decelerationTime * decelerationTime;
// Add them together.
var thrustDistance = accelerationDistance + decelerationDistance;
// Find the idle distance.
var idleDistance = distance - thrustDistance;
// And the time to idle.
idleTime = (int)(idleDistance / cruiseSpeed);
}
legDurations[0] = accelerationTime;
legDurations[1] = idleTime;
legDurations[2] = decelerationTime;
return legDurations;
}
NEW VERSION:
Assume you have the initial positions and velocities xA0, vA0 and xB0, vB0 of spaceships A and B respectively. As you said, B moves with no acceleration and with constant velocity vB0. Therefore, it travels uniformly along a straight line. Its motion is described as: xB = xB0 + t*vB0. Spaceship A can turn on and off an acceleration of constant magnitude a0 but can change its direction as it sees fit. The velocity of A should not exceed certain value v_max > 0.
Since spaceship B travels uniformly, along a straight line with constant velocity vB0, it actually defines an inertial coordinate system. In other words, if we translate the original coordinate system and attach it to B, the new system travels with constant velocity along a straight line and is therefore also inertial. The transformation is Galilean, so one can define the following change of coordinates (in both directions)
y = x - xB0 - t*vB0
u = v - vB0
x = y + xB0 + t*vB0
v = u + vB0
in particular, for B for any moment of time t we get
yB = xB - xB0 - t*vB0 = xB0 + t*vB0 - xB0 - t*vB0 = 0``
At time t=0,
yA0 = xA0 - xB0
uA0 = vA0 - vB0
We are aiming to design the control in this new coordinate system and them move it back into the original one. So let us switch coordinates:
y = x - xB
u = v - vB0
So in this new inertial coordinate system we are solving a problem of control theory and to engineer a good control, we would use as a Lyapunov function (a function that allows us to guarantee certain stable behavior and design the proper expression for the acceleration a) the magnitude of the velocity squared L = norm(u)^2. We want to design acceleration a so that the Lyapunov function in the initial phase of the motion monotonically and steadily decreases while the velocity reorients appropriately.
Define the unit vector
L_unit = cross(x0A - xB0, v0A - vB0) / norm(cross(x0A - xB0, v0A - vB0))
Let in the coordinate system attached to B the motion of A satisfy the system of ordinary differential equations (these equations in both systems are Newton's, because both systems are inertial):
dy/dt = u
du/dt = - a0 * (u - cross(L_unit, u)) / norm(u - cross(L_unit, u))
In other words, the acceleration is set to
a = - a0 * (u - cross(L_unit, u)) / norm(u - cross(L_unit, u))
Observe that by design norm(a) = a0. Because the vectors u and cross(L_unit, u) are orthogonal and of equal magnitude (simply cross(L_unit, u) is the ninety degree rotation of vector u), the denominator simplifies to
norm(u - cross(L_unit, u)) = sqrt( norm(u - cross(L_unit, u))^2 )
= sqrt( norm(u)^2 + norm(L_unit, u)^2 )
= sqrt( norm(u)^2 + norm(L_unit)^2*norm(u)^2 )
= sqrt( norm(u)^2 + norm(u)^2)
= sqrt(2) * norm(u)
So the system of differential equations simplifies to
dy/dt = u
du/dt = -(a0/sqrt(2)) * u/norm(u) + (a0/sqrt(2)) * cross(L_unit, u)) / norm(u)
The system is designed so that A always moves in the plane passing thorugh the origin B and perpendicular to the vector L_unit.
Becauseu and cross(L_unit, u) are perpendicular, their dot product is 0, which allows us to calculate the time-derivative of the lyapunov function along the solutions to the system above (u' means transpose of column-vector u):
d/dt( L ) = d/dt( norm(u)^2 ) = d/dt( u' * u ) = u' * du/dt
= u' * ( -(a0/sqrt(2)) * u/norm(u)
+ (a0/sqrt(2)) * cross(L_unit, u)) / norm(u) )
= -(a0/sqrt(2)) * u'*u / norm(u)
+ (a0/sqrt(2)) * u'*cross(L_unit, u)) / norm(u)
= -(a0/sqrt(2)) * norm(u)^2 / norm(u)
= -(a0/sqrt(2)) * norm(u)
= - (a0/sqrt(2)) * sqrt(L)
d/dt( L ) = -(a0/sqrt(2)) * sqrt(L) < 0
which means that norm(u) decreases with time to 0, as desired.
The system of differential equations, that governs the motion, looks initially non-linear but can be linearized and explicitly solvaed. However, for simplicity, I have decided to integrate it numerically.
The system of differential equations, that governs the motion, looks initially non-linear but can be linearized and explicitly solved. However, for simplicity, I have decided to integrate it numerically. To do that, I have chosen a geometric integrator method, where the system is split into two explicitly solvable systems, whose solutions are combined together to give (a very good approximation of) the solution to the original system. The systems are:
dy/dt = u / 2
du/dt = -(a0/sqrt(2)) u / norm(u)
and
dy/dt = u / 2
du/dt = (a0/sqrt(2)) cross(L_unit, u) / norm(u)
Initially, the second system is nonlinear, however after we calculate:
d/dt(norm(u)*2) = d/dt (dot(u, u)) = 2 * dot(u, du/dt)
= 2 * dot(u, (a0/sqrt(2)) * cross(L_unit , u))
= 2 * (a0/sqrt(2)) * dot(u, cross(L_unit , u))
= 0
we conclude that during the motion defined by this system, the magnitude of the
velocity is constant, i.e. norm(u) = norm(u0) where u0 = u(0). Thus, the systems, together with their solutions, now look like:
First system:
dy/dt = u / 2
du/dt = -(a0/sqrt(2)) u / norm(u)
Solution:
y(t) = y0 + h * u0/2 - t^2 * a0 * u0 / (4*sqrt(2)*norm(u0));
u(t) = u - t * a0 * u0 / (sqrt(2)*norm(u0));
and
Second system:
dy/dt = u / 2
du/dt = (a0/(sqrt(2)*norm(u0))) cross(L_unit, u)
Solution:
y(t) = y0 + (sqrt(2)*norm(u0)/a0) *( cross(L_unit, u0)
+ sin( t * a0/(sqrt(2)*norm(u0)) ) * u0
- cos( t *a0/(sqrt(2)*norm(u0)) ) * cross(L_unit, u0) )
u(t) = cos( t *a0/(sqrt(2)*norm(u0)) ) * u0
+ sin( t *a0/(sqrt(2)*norm(u0)) ) * cross(L_unit, u0)
The solution to the original system can be approximated as follows. Select a time step h. Then if at time t the spaceship's position and velocity have been calculated to be y, u, the updated spaceship's position and velocity at time t + h can be calculated by first letting the ship move along the solution of the second system starting from y, u for time h/2, then move along the solution of the first system for time h and then move along the solution of the second system for time h/2.
function main()
h = 0.3;
a0 = 0.1;
u_max = .8; % velocity threshold
xB0 = [0; 0; 0];
vB0 = [-1; 2; 0];
xA0 = [ 7; 12; 0] + xB0;
vA0 = [1; 5; 0]/7;
%vA0 = [2; -1; 0];
L_unit = cross(xA0 - xB0, vA0 - vB0);
L_unit = L_unit / norm(L_unit);
t = 0;
xB = xB0;
x = xA0;
v = vA0;
hold on
grid on
%axis([-200 20 -100 350])
plot(0, 0, 'bo')
% STEP 0 (the motion as described in the text above):
n = floor(sqrt(2)*norm(vA0 - vB0)/(a0*h));
for i=1:n
[t, x, v, xB] = E(t, x, v, xB, vB0, a0, L_unit, h);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
u = v - vB0;
norm_u = norm(u);
% short additional decceleration so that A attains velocity v = vB0
t0 = t + norm_u/a0;
n = floor((t0 - t)/h);
a = - a0 * u / norm_u;
for i=1:n
[t, x, v, xB] = ET(t, x, v, xB, vB0, a, h);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
[t, x, v, xB] = ET(t, x, v, xB, vB0, a, t0-t);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
% STEP 1 (uniform acceleration of magnitude a0):
v = vB0;
a = x-xB;
norm_y0 = norm(a);
a = - a0 * a / norm_y0;
%t2 = t1 + sqrt( norm_y/a0 );
accel_time = min( u_max/a0, sqrt( norm_y0/a0 ) );
t1 = t0 + accel_time;
n = floor((t1 - t0)/h);
for i=1:n
[t, x, v, xB] = ET(t, x, v, xB, vB0, a, h);
plot(x(1), x(2), 'bo');
plot(xB(1), xB(2), 'ro');
pause(0.1)
end
[t, x, v, xB] = ET(t, x, v, xB, vB0, a, t1-t);
plot(x(1), x(2), 'bo');
plot(xB(1), xB(2), 'ro');
pause(0.1)
% STEP 2 (uniform straight-line motion):
norm_y1 = norm(x-xB);
norm_y12 = max(0, norm_y0 - 2*(norm_y0 - norm_y1));
t12 = norm_y12 / norm(v-vB0)
t = t + t12
n12 = floor(t12/h)
for i=1:n12
x = x + h*v;
xB = xB + h*vB0;
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
x = x + (t12-n12*h)*v;
xB = xB + (t12-n12*h)*vB0;
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
% STEP 3 (uniform deceleration of magnitude a0, symmetric to STEP 1):
a = -a;
for i=1:n % t2 + (t2-t1)
[t, x, v, xB] = ET(t, x, v, xB, vB0, a, h);
plot(x(1), x(2), 'bo');
plot(xB(1), xB(2), 'ro');
pause(0.1)
end
[t, x, v, xB] = ET(t, x, v, xB, vB0, a, t0+t12+2*accel_time-t);
plot(x(1), x(2), 'bo');
plot(xB(1), xB(2), 'ro');
pause(0.1)
norm(x-xB)
norm(v-vB0)
end
Here are the additional functions that are used in the main code above:
% change of coordinates from world coordinates x, v
% to coordinates y, u from spaship B's point of view:
function [y, u] = change(x, v, xB, vB0)
y = x - xB;
u = v - vB0;
end
% inverse chage of coordinates from y, u to x, v
function [x, v] = inv_change(y, u, xB, vB0)
x = y + xB;
v = u + vB0;
end
% solution to the second system of differential equations for a step h:
function [y_out, u_out] = R(y, u, a0, L_unit, h)
omega = a0 / (sqrt(2) * norm(u));
L_x_u = cross(L_unit, u);
cos_omega_h = cos(omega*h);
sin_omega_h = sin(omega*h);
omega = 2*omega;
y_out = y + (L_x_u ...
+ sin_omega_h * u - cos_omega_h * L_x_u) / omega;
u_out = cos_omega_h * u + sin_omega_h * L_x_u;
end
% solution to the first system of differential equations for a step h:
function [y_out, u_out] = T(y, u, a0, h)
sqrt_2 = sqrt(2);
u_unit = u / norm(u);
y_out = y + h * u/2 - h^2 * a0 * u_unit/ (4*sqrt_2);
u_out = u - h * a0 * u_unit / sqrt_2;
end
% approximate solution of the original system of differential equations for step h
% i.e. the sum of furst and second systems of differential equations:
function [t_out, x_out, v_out, xB_out] = E(t, x, v, xB, vB0, a0, L_unit, h)
t_out = t + h;
[y, u] = change(x, v, xB, vB0);
[y, u] = R(y, u, a0, L_unit, h/2);
[y, u] = T(y, u, a0, h);
[y, u] = R(y, u, a0, L_unit, h/2);
xB_out = xB + h*vB0;
[x_out, v_out] = inv_change(y, u, xB_out, vB0);
end
% straight-line motion with constant acceleration:
function [t_out, x_out, v_out, xB_out] = ET(t, x, v, xB, vB0, a, h)
t_out = t + h;
[y, u] = change(x, v, xB, vB0);
y = y + h * u + h^2 * a / 2;
u = u + h * a;
xB_out = xB + h*vB0;
[x_out, v_out] = inv_change(y, u, xB_out, vB0);
end
OLDER VERSION:
I developed two models. Both models are initially described in the moving with B inertial frame of reference y, u (see my previous answers) and then the coordinates are transformed into the original ones x, v. I designed the control based on the function norm(u)^2 as a Lyapunov function, so that in the first step of the algorithm, the acceleration is designed so that the Lyapunov function norm(u)^2 decreases steadily. In the first version, the speed of decrease is quadratic, but the model is easier to integrate, while in the second version, the speed of decrease is exponential, but the model requires Runge-Kutta integration. And I haven't quite tuned it well. I think Version 1 should looks good.
Take L_unit = cross(y0, u0) / norm(cross(y0, u0)).
Version 1: The model is:
dy/dt = y
du/dt = - a0 * (u + cross(L_unit, u)) / norm(u + cross(L_unit, u))
= - a0 * (u + cross(L_unit, u)) / (norm(u)*sqrt(1 + norm(L_unit)^2))
= - a0 * (u + cross(L_unit, u)) / (sqrt(2) * norm(u))
To integrate it, split it into a pair of systems:
dy/dt = y
du/dt = - a0 * u / norm(u)
dy/dt = y
du/dt = - a0 * cross(L_unit, u) / norm(u0) (see previous answers)
and integrate them one after the other for small increments of h time intervals, and then go back and forth between these two systems consecutively. I experimented with some Matlab code:
function main()
h = 0.3;
a0 = 0.1;
xB0 = [0; 0; 0];
vB0 = [-1; 2; 0];
xA0 = [ 7; 12; 0] + xB0;
vA0 = [2; -1; 0];
L_unit = cross(xA0 - xB0, vA0 - vB0);
L_unit = L_unit / norm(L_unit);
t = 0;
xB = xB0;
x = xA0;
v = vA0;
hold on
grid on
%axis([-200 20 -100 350])
plot(0, 0, 'bo')
n = floor(2*norm(v - vB0)/(h*a0));
for i=1:n
[t, x, v, xB] = R(t, x, v, xB, vB0, a0, L_unit, h/2);
a = - a0 * (v - vB0) / norm(v - vB0);
[t, x, v, xB] = T(t, x, v, xB, vB0, a, h/2);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
t1 = t + norm(v - vB0)/a0;
n = floor((t1 - t)/h);
a = - a0 * (v - vB0) / norm(v - vB0);
for i=1:n
[t, x, v, xB] = T(t, x, v, xB, vB0, a, h);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
[t, x, v, xB] = T(t, x, v, xB, vB0, a, t1-t);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
t2 = t1 + sqrt( norm(x - xB)/a0 );
n = floor((t2 - t1)/h);
a = - a0 * (x - xB) / norm(x - xB);
v = vB0;
for i=1:n
[t, x, v, xB] = T(t, x, v, xB, vB0, a, h);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
[t, x, v, xB] = T(t, x, v, xB, vB0, a, t2-t);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
for i=1:n % t2 + (t2-t1)
[t, x, v, xB] = T(t, x, v, xB, vB0, -a, h);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
[t, x, v, xB] = T(t, x, v, xB, vB0, -a, 2*t2 - t1 -t);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
where the relevant functions are:
function [t_out, y_out, u_out] = R1(t, y, u, a0, L_unit, h)
t_out = t + h;
norm_u = norm(u);
R = norm_u^2 / a0;
cos_omega_h = cos(a0 * h / norm_u);
sin_omega_h = sin(a0 * h / norm_u);
u_unit = u / norm_u;
y_out = y + R * cross(L_unit, u_unit) ...
+ R * sin_omega_h * u_unit ...
- R * cos_omega_h * cross(L_unit, u_unit);
u_out = norm_u * sin_omega_h * cross(L_unit, u_unit) ...
+ norm_u * cos_omega_h * u_unit;
end
function [t_out, x_out, v_out, xB_out] = R(t, x, v, xB, vB0, a0, L_unit, h)
[t_out, y_out, u_out] = R1(t, x - xB, v - vB0, a0, L_unit, h);
xB_out = xB + h * vB0;
x_out = y_out + xB_out;
v_out = u_out + vB0;
end
function [t_out, y_out, u_out] = T1(t, y, u, a, h)
t_out = t + h;
u_out = u + h * a;
y_out = y + h * u + h^2 * a / 2;
end
function [t_out, x_out, v_out, xB_out] = T(t, x, v, xB, vB0, a, h)
[t_out, y_out, u_out] = T1(t, x - xB, v - vB0, a, h);
xB_out = xB + h * vB0;
x_out = y_out + xB_out;
v_out = u_out + vB0;
end
Version 2: The model is:
0 < k0 < 2 * a0 / norm(u0)
dy/dt = y
du/dt = - k0 * u / 2 + sqrt(a0^2 - k0^2 * norm_u^2 / 4) * cross(L_unit, u/norm_u);
Matlab code:
function main()
h = 0.3;
a0 = 0.1;
xB0 = [0; 0; 0];
vB0 = [-1; 2; 0];
xA0 = [ 7; 12; 0] + xB0;
vA0 = [2; -1; 0];
k0 = a0/norm(vA0-vB0);
L_unit = cross(xA0 - xB0, vA0 - vB0);
L_unit = L_unit / norm(L_unit);
t = 0;
xB = xB0;
x = xA0;
v = vA0;
hold on
grid on
%axis([-200 20 -100 350])
plot(0, 0, 'bo')
n = floor(2*norm(v - vB0)/(h*a0)); % this needs to be improved
for i=1:n
[t, x, v, xB] = F_step(t, x, v, xB, vB0, a0, L_unit, k0, h);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
t1 = t + norm(v - vB0)/a0;
n = floor((t1 - t)/h);
a = - a0 * (v - vB0) / norm(v - vB0);
for i=1:n
[t, x, v, xB] = T(t, x, v, xB, vB0, a, h);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
[t, x, v, xB] = T(t, x, v, xB, vB0, a, t1-t);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
t2 = t1 + sqrt( norm(x - xB)/a0 );
n = floor((t2 - t1)/h);
a = - a0 * (x - xB) / norm(x - xB);
v = vB0;
for i=1:n
[t, x, v, xB] = T(t, x, v, xB, vB0, a, h);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
[t, x, v, xB] = T(t, x, v, xB, vB0, a, t2-t);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
for i=1:n % t2 + (t2-t1)
[t, x, v, xB] = T(t, x, v, xB, vB0, -a, h);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
[t, x, v, xB] = T(t, x, v, xB, vB0, -a, 2*t2 - t1 -t);
plot(x(1), x(2), 'ro');
plot(xB(1), xB(2), 'bo');
pause(0.1)
end
where the relevant functions are:
function [dydt, dudt] = F1(u, a0, L_unit, k0)
norm_u = norm(u);
dydt = u;
dudt = - k0 * u / 2 + sqrt(a0^2 - k0^2 * norm_u^2/4) * cross(L_unit, u/norm_u);
end
function [t_out, y_out, u_out] = F1_step(t, y, u, a0, L_unit, k0, h)
t_out = t + h;
[z1, w1] = F1(u, a0, L_unit, k0);
[z2, w2] = F1(u + h * w1/2, a0, L_unit, k0);
[z3, w3] = F1(u + h * w2/2, a0, L_unit, k0);
[z4, w4] = F1(u + h * w3, a0, L_unit, k0);
y_out = y + h*(z1 + 2*z2 + 2*z3 + z4)/6;
u_out = u + h*(w1 + 2*w2 + 2*w3 + w4)/6;
end
function [t_out, x_out, v_out, xB_out] = F_step(t, x, v, xB, vB0, a0, L_unit, k0, h)
[t_out, x_out, v_out] = F1_step(t, x-xB, v-vB0, a0, L_unit, k0, h);
xB_out = xB + h * vB0;
x_out = x_out + xB_out;
v_out = v_out + vB0;
end
function [t_out, y_out, u_out] = T1(t, y, u, a, h)
t_out = t + h;
u_out = u + h * a;
y_out = y + h * u + h^2 * a / 2;
end
function [t_out, x_out, v_out, xB_out] = T(t, x, v, xB, vB0, a, h)
[t_out, y_out, u_out] = T1(t, x - xB, v - vB0, a, h);
xB_out = xB + h * vB0;
x_out = y_out + xB_out;
v_out = u_out + vB0;
end
I have tried to outline a somewhat simple approach, back of the envelope so to say, divided into four simple steps.
Assume you have the initial positions and velocitiesxA0, vA0 and xB0, vB0 of spaceship A and B respectively. As you said, B moves with no acceleration and with constant velocity vB0. Therefore, it travels uniformly along a straight line. Its motion is described as:
xB = xB0 + t*vB0
Spaceship A can turn on and off an acceleration of constant magnitude a0 but can change its direction as it sees fit.
I really hope that your velocitiy limit satisfies norm(vA0 - vB0) < v_max otherwise, the acceleration control you have to construct becomes more complex.
Step 1: Kill the difference between the velocities of A and B. Apply constant acceleration
a = a0 *(vB0 - vA0) / norm(vB0 - vA0)
to spaceship A. Then, the positions and the velocities of A and B change with time as follows:
xA = xA0 + t*vA0 + t^2*a0*(vB0 - vA0)/(2*norm(vB0 - vA0))
vA = vA0 + t*a0*(vB0 - vA0)/norm(vB0 - vA0)
xB = xB0 + t*vB0
vB = vB0
At time t1 = norm(vB0 - vA0)/a0 the velocity of spaceship A is vB0 which is equal in magnitude and direction to the velocity of spaceship B. At t1 if A turns off its acceleration and keeps it off, it will travel parallel to B, just with an offset in space.
Explanation: (not needed for the algorithm, but explains the calculations used in the next steps)
Since spaceship B travels uniformly, along a straight line with constant velocity vB0, it actually defines an inertial coordinate system. In other words, if we translate the original coordinate system and attach it to B, the new system travels with constant velocity along a straight line and is therefore also inertial. The transformation is Galilean, so one can define the following change of coordinates (in both directions)
y = x - xB0 - t*vB0
u = v - vB0
x = y + xB0 + t*vB0
v = u + vB0
At time t1 from step 1, the positions of the two spaceships are
xA1 = xA0 + t1*vA0 + t1^2*a0*(vB0 - vA0)/(2*norm(vB0 - vA0))
xB1 = xB0 + t*vB0
and their velocities are vA1 = vB1 = vB0. Thus
yA1 = xA1 - xB0 - t1*vB0
yB1 = xB1 - xB0 - t1*vB0 = xB0 + t1*vB0 - xB0 - t1*vB0 = 0
In this coordinate system, if at time t1 A turns off its acceleration and keeps it off, it will be just stationary, i.e. its position yA1 will not change with time. Now, all we have to do is move A from point yA1 to 0 along the straight-line segment AB, defined by the vector - yA1 = vector(AB) (pointing from A to the origin B). The idea is that now A can simply move with constant acceleration along AB for some time (t2-t1), gaining some velocity uA2 which does not exceed your velocity limit morm(uA2 + vB0) < v_max, then turn off the acceleration and fly for some period of time (t3-t2), which is to be determined, with velocity uA2, and finally turn on decceleration along AB for time (t4-t3) = (t2-t1), and at time t4 the A and B meet and the velocity of A is 0 (in the new coordinate system, the one flying with B). Which means the two ships are at the same location and have the same velocity (as a vector) in the original coordinate system.
Now,
yA = yA1 - (t-t1)^2*a0*yA1/(2*norm(yA1))
uA = (t-t1)*a0*yA1/norm(yA1)
so at t2 (all points yA1, yA2, yA3 and 0 are collinear):
yA2 = yA1 - (t2-t1)^2*a0*yA1/(2*norm(yA1)) = (norm(yA1)-(t2-t1)^2*a0/(2*norm(yA1))) * yA1
uA2 = (t2-t1)*a0*yA1/norm(yA1)
norm(yA2 - yA1) = norm( yA1 - (t2-t1)^2*a0*yA1/(2*norm(yA1)) - yA1 )
= norm(- (t2-t1)^2*a0*yA1/(2*norm(yA1)))
= (t2-t1)^2*(a0/2)*norm(yA1/norm(yA1))
= (t2-t1)^2*a0/2
norm(yA1) = norm(yA2 - yA1) + norm(yA3 - yA2) + norm(0 - yA3)
norm(yA3 - yA2) = norm(yA1) - norm(yA2 - yA1) - norm(0 - yA3)
= norm(yA1) - (t2-t1)^2*a0
(t3-t2) = norm(yA3 - yA2) / norm(uA2) = ( norm(yA1) - (t2-t1)^2*a0 )/norm(uA2)
Now, let us return to the original coordinate system.
yA1 = xA1 - xB1
uA2 = vA2 - vB0
(t3-t2) = ( norm(xA1 - xB1) - (t2-t1)^2*a0 )/norm(vA2 - vB0)
so the important calculation here is: as soon as you choose your t2, you get to calculate
t3 = t2 + ( norm(xA1 - xB1) - (t2-t1)^2*a0 )/norm(vA2 - vB0)
Step 2: As it was mentioned already, at time t1 from step 1, the positions of the two spaceships are
xA1 = xA0 + t1*vA0 + t1^2*a0*(vB0 - vA0)/(2*norm(vB0 - vA0))
xB1 = xB0 + t*vB0
and their velocities are vA1 = vB1 = vB0.
At time t1 apply acceleration a = a0*(xB1 - xA1)/norm(xB1 - xA1). Then, the positions and the velocities of A and B change with time as follows:
xA = xA1 + (t-t1)*vB0 + (t-t1)^2*a0*(xB1 - xA1)/(2*norm(xB1 - xA1))
vA = vB0 + (t-t1)*a0*(xB1 - xA1)/norm(xB1 - xA1)
xB = xB1 + (t-t1)*vB0 or if you prefer xB = xB0 + t*vB0
vB = vB0
Pick any t2 that satisfies
t2 <= t1 + sqrt( norm(xA1 - xB1)/a0 ) (the time to get to the middle of ``AB`` accelerating)
and such that it satisfies
norm( vB0 - (t2 - t1)*a0*(xA1 - xB1)/norm(xA1 - xB1) ) < v_max
Then at time t2 you get the positions an velocities
xA2 = xA1 + (t2-t1)*vB0 + (t2-t1)^2*a0*(xB1 - xA1)/(2*norm(xB1 - xA1))
vA2 = vB0 + (t2-t1)*a0*(xB1 - xA1)/norm(xB1 - xA1)
xB2 = xB1 + (t2-t1)*vB0 or if you prefer xB2 = xB0 + t2*vB0
vB2 = vB0
Step 3: Calculate the next time-moment
t3 = t2 + ( norm(xA1 - xB1) - (t2-t1)^2*a0 )/norm(vA2 - vB0)
and since A moves with constant velocity vA2 along a straight line:
xA3 = xA2 + (t3-t2)*vA2
vA3 = vA2
xB3 = xB2 + (t3-t2)*vB0 or if you prefer xB3 = xB0 + t3*vB0
vB3 = vB0
Step 4: This is the final stretch, when A deccelerates to meet with B:
t4 = t3 + (t2-t1)
At time t3 apply acceleration a = a0*(xA1 - xB1)/norm(xA1 - XB1), exactly opposite to the one from step 2. Then, the positions and the velocities of A and B change with time as follows:
xA = xA3 + (t-t3)*vB3 + (t-t3)^2*a0*(xA1 - xB1)/(2*norm(xA1 - xB1))
vA = vB3 + (t-t3)*a0*(xA1 - xB1)/norm(xA1 - xB1)
xB = xB3 + (t-t3)*vB0 or if you prefer xB = xB0 + t*vB0
vB = vB0
and for t4 we should have
xA4 = xB4 and vA4 = vB0
Now I realize there is a fair amount of details, so it possible I have some typos and possibly errors. However, the idea looks sound to me but I advise you to redo some of the calculations, just to be sure.
Assume you have the initial positions and velocities xA0, vA0 and xB0, vB0 of spaceships A and B respectively. As you said, B moves with no acceleration and with constant velocity vB0. Therefore, it travels uniformly along a straight line. Its motion is described as: xB = xB0 + t*vB0. Spaceship A can turn on and off an acceleration of constant magnitude a0 but can change its direction as it sees fit.
Since spaceship B travels uniformly, along a straight line with constant velocity vB0, it actually defines an inertial coordinate system. In other words, if we translate the original coordinate system and attach it to B, the new system travels with constant velocity along a straight line and is therefore also inertial. The transformation is Galilean, so one can define the following change of coordinates (in both directions)
y = x - xB0 - t*vB0
u = v - vB0
x = y + xB0 + t*vB0
v = u + vB0
in particular, for B for any moment of time t we get
yB = xB - xB0 - t*vB0 = xB0 + t*vB0 - xB0 - t*vB0 = 0``
At time t=0,
yA0 = xA0 - xB0
uA0 = vA0 - vB0
So we are going to design the control in this new coordinate system and them move it back into the original one. First, spaceship A is going move so that its velocity has always the same magnitude norm(uA) = norm(uA0) but its direction changes uniformly. To achieve that, one can simply take the cross-product vector
L0 = cross(yA0, uA0) / ( norm( cross(yA0, uA0) ) * norm(uA0) )
and at each moment of time t apply acceleration
a = a0 * cross(L0, uA)
This means that the law of motion of A satisfies the differential equations
dyA/dt = uA
duA/dt = a0 * cross(L0 , uA)
then
d/dt (dot(uA, uA)) = 2 * dot(uA, duA/dt) = 2 * dot(uA, a0 * cross(L0 , uA))
= 2 * a0 * dot(uA, cross(L0 , uA))
= 0
which is possible only when norm(uA)^2 = dot(uA, uA) = norm(uA0), i.e. the magnitude norm(uA) = norm(uA0) for all t is constant.
Let us check the norm of the acceleration's magnitude:
norm(a) = a0 * norm( cross(L0, uA)) = a0 * norm(L0) * norm(uA)
= a0 * norm( cross(yA0, uA0)/( norm( cross(yA0, uA0) )*norm(uA0) ) )*norm(uA0)
= a0 * norm( cross(yA0, uA0) )/( norm( cross(yA0, uA0) )*norm(uA0) ) )*norm(uA0)
= a0
Since norm(uA) = norm(uA0) = const the tip of the velocity of A, drawn as a vector uA from the origin B, always lies on the sphere norm(uA) = norm(uA0) centered at the origin. At the same time
d/dt ( dot(L0, uA) ) = dot(L0, duA/dt) = a0 * dot(L0, cross(L0, uA)) = 0
which means that
dot(L0, uA) = const = dot(L0, uA0) = 0
hence uA always lies on a plane perpendicular to vector L0 and passing through the origin. Thus, uA points to the intersection of the said plane with the sphere norm(uA) = norm(uA0), i.e. uA traverses a circle. In other words, the equation
duA/dt = a0 cross(L0, uA)
defines a rotation around the origin of vector uA in a plane through the origin and perpendicular to L0. Next, take
dyA/dt - a0*cross(L0, yA) = uA - a0*cross(L0, yA)
and differentiate it with respect to t:
duA/dt - a0*cross(L0, dyA/dt) = duA/dt - a0*cross(L0, uA) = 0
which means that there exists a constant vector such that dyA/dt - a0*cross(L0, yA) = const_vect and we can rewrite this last equation as
dyA/dt = a0*cross(L0, yA - cA)
and even like
d/dt( yA - cA ) = a0*cross(L0, yA - cA)
which just by the same arguments as the ones for uA implies that yA - cA traverses a circle centered at the origin and in a plane perpendicular to L0. Consequently, yA traverses a circle in the plane through the origin, perpendicular to L0 and centered at cA. One just needs to find the radius and the center of the circle. Then the motion of A under the equations
dyA/dt = uA
duA/dt = a0* cross(L0, uA)
reduces to the equation
dyA/dt = a0 * cross(L0, yA - cA)
yA(0) = yA0
In order to find the radius R, we set time t=0:
uA0 = a0 * cross(L0, yA0 - cA)
so
norm(uA0) = a0 * norm(cross(L0, yA0 - cA)) = a0 * norm(L0) * norm(yA0 - cA)
= a0 * norm(L0) * R
norm(L0) = 1 / norm(uA0)
R = norm(uA0)^2 / a0
The center is then along the vector perpendicular to both uA0 and L0 , so
cA = yA0 + R * cross(L0, uA0) / (norm(L0)*norm(uA0))
Then, we can set up a 2D coordinate system in the plane in which the motion occurs by choosing origin yA0 and unit perpendicular vectors uA0/norm(uA0) and -cross(L0, uA0) / (norm(L0)*norm(uA0)). So the motion of A in the coordinate system moving uniformly in a straight line with B can be described as
yA(t) = yA0 + R * sin(a0 * t / norm(L0)) * uA0 / norm(uA0)
- R * cos(a0 * t / norm(L0)) * cross(L0, uA0) / (norm(L0)*norm(uA0))
which is the solution to the initial value problem:
dyA/dt = uA0
duA/dt = a0 * cross(L0, uA)
yA(0) = yA0
uA(0) = uA0
So my new suggestion is to incorporate
Step 0: For a time period t from 0 to t0 apply the following acceleration and motion, which rotates the direction of the velocity vector of A:
yA0 = xA0 - xB0
uA0 = vA0 - vB0
L0 = cross(yA0, uA0) / ( norm( cross(yA0, uA0) ) * norm(uA0) )
a = a0 * cross(L0, uA0)
R = norm(uA0)^2 / a0
yA(t) = yA0 + R * cos(a0*t/norm(uA0)) / (norm(L0)*norm(uA0))
+ R * sin(a0*t/norm(uA0)) * uA0/norm(uA0)
- R * cos(a0*t/norm(uA0)) * cross(L0, uA0) / (norm(L0)*norm(uA0))
xA(t) = yA(t) + xB0 + t * vB0 =
= xA0 + t * vB0 + R * cos(a0*t/norm(uA0)) / (norm(L0)*norm(uA0))
+ R * sin(a0*t/norm(uA0)) * uA0/norm(uA0)
- R * cos(a0*t/norm(uA0)) * cross(L0, uA0) / (norm(L0)*norm(uA0))
until a moment of time t0 chosen so that the velocity's direction vA(t) is at a better position relative to vB0 so that from moment t0 on, you can apply the four steps outlined in my previous answer. Of course you can also use this new circular motion control to make your own combination that you like better.