规划两艘宇宙飞船交会的算法
Algorithm for planning a rendezvous between two spaceships
我正在尝试找出一种在两艘宇宙飞船之间建立会合点的算法。
没有重力或阻力。两艘宇宙飞船在开始时都有一个位置和一个速度。飞船B继续前进，没有加速，所以飞船A需要加速来拉近它们之间的距离，然后在到达飞船B的位置时匹配速度。
飞船可以瞬间改变推力方向，但只能使用最大加速度或者不加加速度。我还想限制飞船在机动过程中的速度差。
我希望输出是多条轨迹腿的形式，即：leg1：在 t1 秒内加速方向 x，
leg2：滑行 t2 秒，
leg3: y 方向加速 t3 秒。
我不需要最优解，但我想要 "look right"。
我试图制造一个冲量来平衡速度，并将其添加到向飞船 B 移动的冲量中，但即使飞船 A 以正确的速度结束，它也无法到达目标位置。我自己尝试了脉冲，它们似乎按预期执行，所以我猜这是我将它们加在一起的方式，这就是问题所在。我不知道我是不是没有正确地实施它，或者这种方法是否根本行不通。希望数学和物理能力强的人能赐教。
这是我使用的代码：
// 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);
以及计算进近航段的函数：
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;
}
我试图概述一个有点简单的方法，信封后面这么说，分为四个简单的步骤。
假设您分别拥有space飞船A和B的初始位置和速度xA0, vA0和xB0, vB0。如您所说，B 没有加速度，并且以恒定速度 vB0 移动。因此，它均匀地沿直线行进。其运动描述为：
xB = xB0 + t*vB0
宇宙飞船 A 可以打开和关闭恒定大小的加速度 a0，但可以根据需要改变方向。
我真的希望你的速度限制满足norm(vA0 - vB0) < v_max否则，你要构建的加速度控制会变得更加复杂。
第 1 步：消除 A 和 B 之间的速度差异。施加恒定加速度
a = a0 *(vB0 - vA0) / norm(vB0 - vA0)
到space船A，那么A船和B船的位置和速度随时间变化如下：
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
在时间 t1 = norm(vB0 - vA0)/a0 spaceship A 的速度是 vB0，其大小和方向等于 spaceship B 的速度。在 t1 如果 A 关闭其加速度并保持关闭状态，它将平行于 B 行进，只是在 space 中有一个偏移量。
解释：（算法不需要，但解释了后续步骤中使用的计算）
由于spaceB船做匀速运动，沿直线以恒定速度vB0运动，实际上定义了一个惯性坐标系。换句话说，如果我们将原来的坐标系平移并附在 B 上，新的坐标系沿直线匀速运动，因此也是惯性的。变换是伽利略变换，因此可以定义以下坐标变化（在两个方向上）
y = x - xB0 - t*vB0
u = v - vB0
x = y + xB0 + t*vB0
v = u + vB0
在第 1 步 t1 时，两艘 space 飞船的位置是
xA1 = xA0 + t1*vA0 + t1^2*a0*(vB0 - vA0)/(2*norm(vB0 - vA0))
xB1 = xB0 + t*vB0
他们的速度是vA1 = vB1 = vB0。于是
yA1 = xA1 - xB0 - t1*vB0
yB1 = xB1 - xB0 - t1*vB0 = xB0 + t1*vB0 - xB0 - t1*vB0 = 0
在这个坐标系中，如果在t1时刻A关闭加速度并保持关闭状态，它只是静止的，即它的位置yA1不会随时间变化。现在，我们所要做的就是沿着直线段 AB 将 A 从点 yA1 移动到 0，由向量 - yA1 = vector(AB) 定义（从 A 指向来源 B).这个想法是，现在 A 可以简单地沿 AB 以恒定加速度移动一段时间 (t2-t1)，获得一些不超过速度限制 morm(uA2 + vB0) < v_max 的速度 uA2，然后转向关闭加速飞行一段时间(t3-t2)，待定，速度uA2，最后开启减速AB时间(t4-t3) = (t2-t1)，以及在t4时刻A和B相遇，A的速度为0（在新的坐标系中，与B一起飞行的那个）。这意味着两艘船在相同的位置并且在原始坐标系中具有相同的速度（作为矢量）。
现在，
yA = yA1 - (t-t1)^2*a0*yA1/(2*norm(yA1))
uA = (t-t1)*a0*yA1/norm(yA1)
所以在 t2（所有点 yA1, yA2, yA3 和 0 共线）：
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)
现在，让我们return回到原来的坐标系。
yA1 = xA1 - xB1
uA2 = vA2 - vB0
(t3-t2) = ( norm(xA1 - xB1) - (t2-t1)^2*a0 )/norm(vA2 - vB0)
所以这里重要的计算是：一旦你选择你的t2，你就可以计算
t3 = t2 + ( norm(xA1 - xB1) - (t2-t1)^2*a0 )/norm(vA2 - vB0)
第 2 步： 如前所述，在第 1 步 t1 时，两艘 space 飞船的位置是
xA1 = xA0 + t1*vA0 + t1^2*a0*(vB0 - vA0)/(2*norm(vB0 - vA0))
xB1 = xB0 + t*vB0
他们的速度是vA1 = vB1 = vB0。
在时间 t1 应用加速度 a = a0*(xB1 - xA1)/norm(xB1 - xA1)。那么，A和B的位置和速度随时间的变化如下：
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
选择任何满足
的 t2
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
然后在时间 t2 你得到位置和速度
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
第三步：计算下一个时刻
t3 = t2 + ( norm(xA1 - xB1) - (t2-t1)^2*a0 )/norm(vA2 - vB0)
并且由于 A 沿直线以恒定速度 vA2 移动：
xA3 = xA2 + (t3-t2)*vA2
vA3 = vA2
xB3 = xB2 + (t3-t2)*vB0 or if you prefer xB3 = xB0 + t3*vB0
vB3 = vB0
第4步：这是最后一段，当A减速与B相遇时：
t4 = t3 + (t2-t1)
在时间t3施加加速度a = a0*(xA1 - xB1)/norm(xA1 - XB1)，与步骤2正好相反。然后，A和B的位置和速度随时间变化如下：
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
对于t4我们应该
xA4 = xB4 and vA4 = vB0
现在我意识到有相当多的细节，所以我可能有一些错别字和错误。然而，这个想法对我来说看起来很合理，但我建议你重做一些计算，只是为了确定。
假设你有飞船A和B的初始位置和速度分别为xA0, vA0和xB0, vB0。如您所说，B 没有加速度，而是以恒定速度 vB0 移动。因此，它均匀地沿直线行进。它的运动描述为：xB = xB0 + t*vB0。宇宙飞船 A 可以打开和关闭恒定大小的加速度 a0，但可以根据需要改变方向。
由于B飞船做匀速直线运动vB0，所以实际上定义了一个惯性坐标系。换句话说，如果我们将原来的坐标系平移并附在 B 上，新的坐标系沿直线匀速运动，因此也是惯性的。变换是伽利略变换，因此可以定义以下坐标变化（在两个方向上）
y = x - xB0 - t*vB0
u = v - vB0
x = y + xB0 + t*vB0
v = u + vB0
特别是，对于任何时刻的 B t 我们得到
yB = xB - xB0 - t*vB0 = xB0 + t*vB0 - xB0 - t*vB0 = 0``
时间 t=0,
yA0 = xA0 - xB0
uA0 = vA0 - vB0
所以我们将在这个新的坐标系中设计控件，然后他们将其移回原始坐标系。首先，宇宙飞船 A 正在移动，因此它的速度始终具有相同的大小 norm(uA) = norm(uA0)，但它的方向均匀变化。为此，可以简单地采用叉积向量
L0 = cross(yA0, uA0) / ( norm( cross(yA0, uA0) ) * norm(uA0) )
并在每个时刻 t 应用加速度
a = a0 * cross(L0, uA)
这意味着A的运动定律满足微分方程
dyA/dt = uA
duA/dt = a0 * cross(L0 , uA)
然后
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
只有在 norm(uA)^2 = dot(uA, uA) = norm(uA0) 时才有可能，即所有 t 的大小 norm(uA) = norm(uA0) 是恒定的。
让我们检查一下加速度大小的范数：
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
因为 norm(uA) = norm(uA0) = const 从原点 B 绘制为矢量 uA 的 A 速度的尖端始终位于以原点为中心的球体 norm(uA) = norm(uA0) 上。同时
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
因此 uA 始终位于垂直于矢量 L0 并通过原点的平面上。因此，uA指向该平面与球体norm(uA) = norm(uA0)的交点，即uA过一个圆。换句话说，等式
duA/dt = a0 cross(L0, uA)
定义在通过原点并垂直于 L0 的平面中绕矢量原点 uA 的旋转。接下来，取
dyA/dt - a0*cross(L0, yA) = uA - a0*cross(L0, yA)
关于 t 微分：
duA/dt - a0*cross(L0, dyA/dt) = duA/dt - a0*cross(L0, uA) = 0
这意味着存在一个常数向量 dyA/dt - a0*cross(L0, yA) = const_vect 我们可以将最后一个等式重写为
dyA/dt = a0*cross(L0, yA - cA)
and even like
d/dt( yA - cA ) = a0*cross(L0, yA - cA)
与 uA 相同的参数意味着 yA - cA 穿过一个以原点为中心并在垂直于 L0 的平面内的圆。因此，yA 在平面中穿过原点的圆，垂直于 L0 并以 cA 为中心。人们只需要找到圆的半径和圆心。那么A在方程下的运动
dyA/dt = uA
duA/dt = a0* cross(L0, uA)
简化为等式
dyA/dt = a0 * cross(L0, yA - cA)
yA(0) = yA0
为了找到半径R，我们设置时间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
然后中心沿着垂直于 uA0 和 L0 的矢量，所以
cA = yA0 + R * cross(L0, uA0) / (norm(L0)*norm(uA0))
然后，我们可以通过选择原点yA0和单位垂直向量uA0/norm(uA0)和-cross(L0, uA0) / (norm(L0)*norm(uA0))，在运动发生的平面上建立一个二维坐标系。所以A在坐标系中与B作匀速直线运动的运动可以描述为
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))
这是初值问题的解：
dyA/dt = uA0
duA/dt = a0 * cross(L0, uA)
yA(0) = yA0
uA(0) = uA0
所以我的新建议是合并
步骤 0: 对于从 0 到 t0 的时间段 t 应用以下加速度和运动，旋转方向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))
直到选择 t0 的时刻，这样速度的方向 vA(t) 相对于 vB0 处于更好的位置，这样从时刻 t0 开始，你可以应用我之前的回答中概述的四个步骤。当然你也可以使用这个新的圆周运动控件来制作你自己更喜欢的组合。
新版本：
假设你有飞船A和B的初始位置和速度分别为xA0, vA0和xB0, vB0。如您所说，B 没有加速度，而是以恒定速度 vB0 移动。因此，它均匀地沿直线行进。它的运动描述为：xB = xB0 + t*vB0。宇宙飞船 A 可以打开和关闭恒定大小的加速度 a0，但可以根据需要改变方向。 A的速度不能超过一定值v_max > 0.
由于B飞船做匀速直线运动vB0，所以实际上定义了一个惯性坐标系。换句话说，如果我们将原来的坐标系平移并附在 B 上，新的坐标系沿直线匀速运动，因此也是惯性的。变换是伽利略变换，因此可以定义以下坐标变化（在两个方向上）
y = x - xB0 - t*vB0
u = v - vB0
x = y + xB0 + t*vB0
v = u + vB0
特别是，对于 B 在任何时刻 t 我们得到
yB = xB - xB0 - t*vB0 = xB0 + t*vB0 - xB0 - t*vB0 = 0``
时间 t=0,
yA0 = xA0 - xB0
uA0 = vA0 - vB0
我们的目标是在这个新坐标系中设计控件，然后他们将其移回原始坐标系。那么让我们换个坐标：
y = x - xB
u = v - vB0
因此，在这个新的惯性坐标系中，我们正在解决控制理论问题并设计出良好的控制，我们将使用 Lyapunov 函数（该函数允许我们保证某些稳定的行为并设计适当的表达式对于加速度 a) 速度平方的大小 L = norm(u)^2。我们想设计加速度a，使运动初始阶段的李亚普诺夫函数单调稳定减小，同时速度适当重新定向。
定义单位向量
L_unit = cross(x0A - xB0, v0A - vB0) / norm(cross(x0A - xB0, v0A - vB0))
设在B所附的坐标系中A的运动满足常微分方程组（这两个系统中的方程都是牛顿方程，因为两个系统都是惯性的）：
dy/dt = u
du/dt = - a0 * (u - cross(L_unit, u)) / norm(u - cross(L_unit, u))
也就是说加速度设置为
a = - a0 * (u - cross(L_unit, u)) / norm(u - cross(L_unit, u))
观察设计norm(a) = a0。因为向量 u 和 cross(L_unit, u) 是正交的并且大小相等（简单地说 cross(L_unit, u) 是向量 u 的九十度旋转），分母简化为
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)
所以微分方程组简化为
dy/dt = u
du/dt = -(a0/sqrt(2)) * u/norm(u) + (a0/sqrt(2)) * cross(L_unit, u)) / norm(u)
系统的设计使得 A 始终在通过原点 B 并垂直于矢量 L_unit.
的平面内移动
因为u和cross(L_unit, u)垂直，它们的点积为0，这允许我们计算lyapunov函数沿着上述系统的解的时间导数（u' 表示转置列向量 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
这意味着 norm(u) 随着时间的推移减少到 0，正如所希望的那样。
控制运动的微分方程组最初看起来是非线性的，但可以线性化并明确求解。但是，为了简单起见，我决定对其进行数值积分。
控制运动的微分方程组最初看起来是非线性的，但可以线性化并明确求解。但是，为简单起见，我决定对其进行数值积分。为此，我选择了一种几何积分器方法，其中将系统分为两个明确可解的系统，将其解组合在一起以给出（非常好的近似）原始系统的解。这些系统是：
dy/dt = u / 2
du/dt = -(a0/sqrt(2)) u / norm(u)
和
dy/dt = u / 2
du/dt = (a0/sqrt(2)) cross(L_unit, u) / norm(u)
最初，第二个系统是非线性的，但是在我们计算之后：
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
我们得出结论，在这个系统定义的运动过程中，
速度是恒定的，即 norm(u) = norm(u0) 其中 u0 = u(0)。因此，系统及其解决方案现在看起来像：
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));
和
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)
原系统的解可以近似如下。 Select 一个时间步 h。那么如果在时间t时飞船的位置和速度已经被计算为y, u，那么更新后的飞船在时间t + h的位置和速度可以通过首先让飞船沿着解移动来计算第二个系统从y, u开始，时间h/2，然后沿着第一个系统的解移动时间h，然后沿着第二个系统的解移动时间[=64] =].
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
以下是上面主要代码中使用的附加函数：
% 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
旧版本：
我开发了两个模型。两种模型最初都是在带有 B 惯性参考系 y, u 的移动中描述的（请参阅我之前的答案），然后将坐标转换为原始坐标 x, v。我把基于函数norm(u)^2的控制设计成李雅普诺夫函数，这样在算法的第一步，加速度就设计成李雅普诺夫函数norm(u)^2稳步下降。在第一个版本中，下降速度是二次方的，但模型更容易集成，而在第二个版本中，下降速度是指数级的，但模型需要 Runge-Kutta 积分。而且我还没有很好地调整它。我觉得第一版应该好看
取L_unit = cross(y0, u0) / norm(cross(y0, u0)).
版本 1： 型号为：
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))
要集成它，将它分成一对系统：
dy/dt = y
du/dt = - a0 * u / norm(u)
dy/dt = y
du/dt = - a0 * cross(L_unit, u) / norm(u0) (see previous answers)
并以h时间间隔的小增量将它们一个接一个地整合，然后在这两个系统之间连续来回。我尝试了一些 Matlab 代码：
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
其中相关函数是：
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
版本 2： 型号为：
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代码：
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
其中相关函数是：
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
我正在尝试找出一种在两艘宇宙飞船之间建立会合点的算法。
没有重力或阻力。两艘宇宙飞船在开始时都有一个位置和一个速度。飞船B继续前进，没有加速，所以飞船A需要加速来拉近它们之间的距离，然后在到达飞船B的位置时匹配速度。
飞船可以瞬间改变推力方向，但只能使用最大加速度或者不加加速度。我还想限制飞船在机动过程中的速度差。
我希望输出是多条轨迹腿的形式，即：leg1：在 t1 秒内加速方向 x， leg2：滑行 t2 秒， leg3: y 方向加速 t3 秒。
我不需要最优解，但我想要 "look right"。
我试图制造一个冲量来平衡速度，并将其添加到向飞船 B 移动的冲量中，但即使飞船 A 以正确的速度结束，它也无法到达目标位置。我自己尝试了脉冲，它们似乎按预期执行，所以我猜这是我将它们加在一起的方式，这就是问题所在。我不知道我是不是没有正确地实施它，或者这种方法是否根本行不通。希望数学和物理能力强的人能赐教。
这是我使用的代码：
// 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);
以及计算进近航段的函数：
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;
}
我试图概述一个有点简单的方法，信封后面这么说，分为四个简单的步骤。
假设您分别拥有space飞船A和B的初始位置和速度xA0, vA0和xB0, vB0。如您所说，B 没有加速度，并且以恒定速度 vB0 移动。因此，它均匀地沿直线行进。其运动描述为：
xB = xB0 + t*vB0
宇宙飞船 A 可以打开和关闭恒定大小的加速度 a0，但可以根据需要改变方向。
我真的希望你的速度限制满足norm(vA0 - vB0) < v_max否则，你要构建的加速度控制会变得更加复杂。
第 1 步：消除 A 和 B 之间的速度差异。施加恒定加速度
a = a0 *(vB0 - vA0) / norm(vB0 - vA0)
到space船A，那么A船和B船的位置和速度随时间变化如下：
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
在时间 t1 = norm(vB0 - vA0)/a0 spaceship A 的速度是 vB0，其大小和方向等于 spaceship B 的速度。在 t1 如果 A 关闭其加速度并保持关闭状态，它将平行于 B 行进，只是在 space 中有一个偏移量。
解释：（算法不需要，但解释了后续步骤中使用的计算） 由于spaceB船做匀速运动，沿直线以恒定速度vB0运动，实际上定义了一个惯性坐标系。换句话说，如果我们将原来的坐标系平移并附在 B 上，新的坐标系沿直线匀速运动，因此也是惯性的。变换是伽利略变换，因此可以定义以下坐标变化（在两个方向上）
y = x - xB0 - t*vB0
u = v - vB0
x = y + xB0 + t*vB0
v = u + vB0
在第 1 步 t1 时，两艘 space 飞船的位置是
xA1 = xA0 + t1*vA0 + t1^2*a0*(vB0 - vA0)/(2*norm(vB0 - vA0))
xB1 = xB0 + t*vB0
他们的速度是vA1 = vB1 = vB0。于是
yA1 = xA1 - xB0 - t1*vB0
yB1 = xB1 - xB0 - t1*vB0 = xB0 + t1*vB0 - xB0 - t1*vB0 = 0
在这个坐标系中，如果在t1时刻A关闭加速度并保持关闭状态，它只是静止的，即它的位置yA1不会随时间变化。现在，我们所要做的就是沿着直线段 AB 将 A 从点 yA1 移动到 0，由向量 - yA1 = vector(AB) 定义（从 A 指向来源 B).这个想法是，现在 A 可以简单地沿 AB 以恒定加速度移动一段时间 (t2-t1)，获得一些不超过速度限制 morm(uA2 + vB0) < v_max 的速度 uA2，然后转向关闭加速飞行一段时间(t3-t2)，待定，速度uA2，最后开启减速AB时间(t4-t3) = (t2-t1)，以及在t4时刻A和B相遇，A的速度为0（在新的坐标系中，与B一起飞行的那个）。这意味着两艘船在相同的位置并且在原始坐标系中具有相同的速度（作为矢量）。
现在，
yA = yA1 - (t-t1)^2*a0*yA1/(2*norm(yA1))
uA = (t-t1)*a0*yA1/norm(yA1)
所以在 t2（所有点 yA1, yA2, yA3 和 0 共线）：
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)
现在，让我们return回到原来的坐标系。
yA1 = xA1 - xB1
uA2 = vA2 - vB0
(t3-t2) = ( norm(xA1 - xB1) - (t2-t1)^2*a0 )/norm(vA2 - vB0)
所以这里重要的计算是：一旦你选择你的t2，你就可以计算
t3 = t2 + ( norm(xA1 - xB1) - (t2-t1)^2*a0 )/norm(vA2 - vB0)
第 2 步： 如前所述，在第 1 步 t1 时，两艘 space 飞船的位置是
xA1 = xA0 + t1*vA0 + t1^2*a0*(vB0 - vA0)/(2*norm(vB0 - vA0))
xB1 = xB0 + t*vB0
他们的速度是vA1 = vB1 = vB0。
在时间 t1 应用加速度 a = a0*(xB1 - xA1)/norm(xB1 - xA1)。那么，A和B的位置和速度随时间的变化如下：
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
选择任何满足
的t2
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
然后在时间 t2 你得到位置和速度
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
第三步：计算下一个时刻
t3 = t2 + ( norm(xA1 - xB1) - (t2-t1)^2*a0 )/norm(vA2 - vB0)
并且由于 A 沿直线以恒定速度 vA2 移动：
xA3 = xA2 + (t3-t2)*vA2
vA3 = vA2
xB3 = xB2 + (t3-t2)*vB0 or if you prefer xB3 = xB0 + t3*vB0
vB3 = vB0
第4步：这是最后一段，当A减速与B相遇时：
t4 = t3 + (t2-t1)
在时间t3施加加速度a = a0*(xA1 - xB1)/norm(xA1 - XB1)，与步骤2正好相反。然后，A和B的位置和速度随时间变化如下：
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
对于t4我们应该
xA4 = xB4 and vA4 = vB0
现在我意识到有相当多的细节，所以我可能有一些错别字和错误。然而，这个想法对我来说看起来很合理，但我建议你重做一些计算，只是为了确定。
假设你有飞船A和B的初始位置和速度分别为xA0, vA0和xB0, vB0。如您所说，B 没有加速度，而是以恒定速度 vB0 移动。因此，它均匀地沿直线行进。它的运动描述为：xB = xB0 + t*vB0。宇宙飞船 A 可以打开和关闭恒定大小的加速度 a0，但可以根据需要改变方向。
由于B飞船做匀速直线运动vB0，所以实际上定义了一个惯性坐标系。换句话说，如果我们将原来的坐标系平移并附在 B 上，新的坐标系沿直线匀速运动，因此也是惯性的。变换是伽利略变换，因此可以定义以下坐标变化（在两个方向上）
y = x - xB0 - t*vB0
u = v - vB0
x = y + xB0 + t*vB0
v = u + vB0
特别是，对于任何时刻的 B t 我们得到
yB = xB - xB0 - t*vB0 = xB0 + t*vB0 - xB0 - t*vB0 = 0``
时间 t=0,
yA0 = xA0 - xB0
uA0 = vA0 - vB0
所以我们将在这个新的坐标系中设计控件，然后他们将其移回原始坐标系。首先，宇宙飞船 A 正在移动，因此它的速度始终具有相同的大小 norm(uA) = norm(uA0)，但它的方向均匀变化。为此，可以简单地采用叉积向量
L0 = cross(yA0, uA0) / ( norm( cross(yA0, uA0) ) * norm(uA0) )
并在每个时刻 t 应用加速度
a = a0 * cross(L0, uA)
这意味着A的运动定律满足微分方程
dyA/dt = uA
duA/dt = a0 * cross(L0 , uA)
然后
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
只有在 norm(uA)^2 = dot(uA, uA) = norm(uA0) 时才有可能，即所有 t 的大小 norm(uA) = norm(uA0) 是恒定的。
让我们检查一下加速度大小的范数：
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
因为 norm(uA) = norm(uA0) = const 从原点 B 绘制为矢量 uA 的 A 速度的尖端始终位于以原点为中心的球体 norm(uA) = norm(uA0) 上。同时
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
因此 uA 始终位于垂直于矢量 L0 并通过原点的平面上。因此，uA指向该平面与球体norm(uA) = norm(uA0)的交点，即uA过一个圆。换句话说，等式
duA/dt = a0 cross(L0, uA)
定义在通过原点并垂直于 L0 的平面中绕矢量原点 uA 的旋转。接下来，取
dyA/dt - a0*cross(L0, yA) = uA - a0*cross(L0, yA)
关于 t 微分：
duA/dt - a0*cross(L0, dyA/dt) = duA/dt - a0*cross(L0, uA) = 0
这意味着存在一个常数向量 dyA/dt - a0*cross(L0, yA) = const_vect 我们可以将最后一个等式重写为
dyA/dt = a0*cross(L0, yA - cA)
and even like
d/dt( yA - cA ) = a0*cross(L0, yA - cA)
与 uA 相同的参数意味着 yA - cA 穿过一个以原点为中心并在垂直于 L0 的平面内的圆。因此，yA 在平面中穿过原点的圆，垂直于 L0 并以 cA 为中心。人们只需要找到圆的半径和圆心。那么A在方程下的运动
dyA/dt = uA
duA/dt = a0* cross(L0, uA)
简化为等式
dyA/dt = a0 * cross(L0, yA - cA)
yA(0) = yA0
为了找到半径R，我们设置时间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
然后中心沿着垂直于 uA0 和 L0 的矢量，所以
cA = yA0 + R * cross(L0, uA0) / (norm(L0)*norm(uA0))
然后，我们可以通过选择原点yA0和单位垂直向量uA0/norm(uA0)和-cross(L0, uA0) / (norm(L0)*norm(uA0))，在运动发生的平面上建立一个二维坐标系。所以A在坐标系中与B作匀速直线运动的运动可以描述为
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))
这是初值问题的解：
dyA/dt = uA0
duA/dt = a0 * cross(L0, uA)
yA(0) = yA0
uA(0) = uA0
所以我的新建议是合并
步骤 0: 对于从 0 到 t0 的时间段 t 应用以下加速度和运动，旋转方向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))
直到选择 t0 的时刻，这样速度的方向 vA(t) 相对于 vB0 处于更好的位置，这样从时刻 t0 开始，你可以应用我之前的回答中概述的四个步骤。当然你也可以使用这个新的圆周运动控件来制作你自己更喜欢的组合。
新版本：
假设你有飞船A和B的初始位置和速度分别为xA0, vA0和xB0, vB0。如您所说，B 没有加速度，而是以恒定速度 vB0 移动。因此，它均匀地沿直线行进。它的运动描述为：xB = xB0 + t*vB0。宇宙飞船 A 可以打开和关闭恒定大小的加速度 a0，但可以根据需要改变方向。 A的速度不能超过一定值v_max > 0.
由于B飞船做匀速直线运动vB0，所以实际上定义了一个惯性坐标系。换句话说，如果我们将原来的坐标系平移并附在 B 上，新的坐标系沿直线匀速运动，因此也是惯性的。变换是伽利略变换，因此可以定义以下坐标变化（在两个方向上）
y = x - xB0 - t*vB0
u = v - vB0
x = y + xB0 + t*vB0
v = u + vB0
特别是，对于 B 在任何时刻 t 我们得到
yB = xB - xB0 - t*vB0 = xB0 + t*vB0 - xB0 - t*vB0 = 0``
时间 t=0,
yA0 = xA0 - xB0
uA0 = vA0 - vB0
我们的目标是在这个新坐标系中设计控件，然后他们将其移回原始坐标系。那么让我们换个坐标：
y = x - xB
u = v - vB0
因此，在这个新的惯性坐标系中，我们正在解决控制理论问题并设计出良好的控制，我们将使用 Lyapunov 函数（该函数允许我们保证某些稳定的行为并设计适当的表达式对于加速度 a) 速度平方的大小 L = norm(u)^2。我们想设计加速度a，使运动初始阶段的李亚普诺夫函数单调稳定减小，同时速度适当重新定向。
定义单位向量
L_unit = cross(x0A - xB0, v0A - vB0) / norm(cross(x0A - xB0, v0A - vB0))
设在B所附的坐标系中A的运动满足常微分方程组（这两个系统中的方程都是牛顿方程，因为两个系统都是惯性的）：
dy/dt = u
du/dt = - a0 * (u - cross(L_unit, u)) / norm(u - cross(L_unit, u))
也就是说加速度设置为
a = - a0 * (u - cross(L_unit, u)) / norm(u - cross(L_unit, u))
观察设计norm(a) = a0。因为向量 u 和 cross(L_unit, u) 是正交的并且大小相等（简单地说 cross(L_unit, u) 是向量 u 的九十度旋转），分母简化为
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)
所以微分方程组简化为
dy/dt = u
du/dt = -(a0/sqrt(2)) * u/norm(u) + (a0/sqrt(2)) * cross(L_unit, u)) / norm(u)
系统的设计使得 A 始终在通过原点 B 并垂直于矢量 L_unit.
因为u和cross(L_unit, u)垂直，它们的点积为0，这允许我们计算lyapunov函数沿着上述系统的解的时间导数（u' 表示转置列向量 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
这意味着 norm(u) 随着时间的推移减少到 0，正如所希望的那样。
控制运动的微分方程组最初看起来是非线性的，但可以线性化并明确求解。但是，为了简单起见，我决定对其进行数值积分。
控制运动的微分方程组最初看起来是非线性的，但可以线性化并明确求解。但是，为简单起见，我决定对其进行数值积分。为此，我选择了一种几何积分器方法，其中将系统分为两个明确可解的系统，将其解组合在一起以给出（非常好的近似）原始系统的解。这些系统是：
dy/dt = u / 2
du/dt = -(a0/sqrt(2)) u / norm(u)
和
dy/dt = u / 2
du/dt = (a0/sqrt(2)) cross(L_unit, u) / norm(u)
最初，第二个系统是非线性的，但是在我们计算之后：
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
我们得出结论，在这个系统定义的运动过程中，
速度是恒定的，即 norm(u) = norm(u0) 其中 u0 = u(0)。因此，系统及其解决方案现在看起来像：
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));
和
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)
原系统的解可以近似如下。 Select 一个时间步 h。那么如果在时间t时飞船的位置和速度已经被计算为y, u，那么更新后的飞船在时间t + h的位置和速度可以通过首先让飞船沿着解移动来计算第二个系统从y, u开始，时间h/2，然后沿着第一个系统的解移动时间h，然后沿着第二个系统的解移动时间[=64] =].
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
以下是上面主要代码中使用的附加函数：
% 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
旧版本：
我开发了两个模型。两种模型最初都是在带有 B 惯性参考系 y, u 的移动中描述的（请参阅我之前的答案），然后将坐标转换为原始坐标 x, v。我把基于函数norm(u)^2的控制设计成李雅普诺夫函数，这样在算法的第一步，加速度就设计成李雅普诺夫函数norm(u)^2稳步下降。在第一个版本中，下降速度是二次方的，但模型更容易集成，而在第二个版本中，下降速度是指数级的，但模型需要 Runge-Kutta 积分。而且我还没有很好地调整它。我觉得第一版应该好看
取L_unit = cross(y0, u0) / norm(cross(y0, u0)).
版本 1： 型号为：
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))
要集成它，将它分成一对系统：
dy/dt = y
du/dt = - a0 * u / norm(u)
dy/dt = y
du/dt = - a0 * cross(L_unit, u) / norm(u0) (see previous answers)
并以h时间间隔的小增量将它们一个接一个地整合，然后在这两个系统之间连续来回。我尝试了一些 Matlab 代码：
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
其中相关函数是：
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
版本 2： 型号为：
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代码：
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
其中相关函数是：
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