KerboScript в примерах и задачах. Часть 7. Сближение и стыковка, или Гусары, молчать!

Пролог
27 октября 1967 года с площадки 31 космодрома Байконур на орбиту с наклонением 51,7° был запущен аппарат "Космос-186". Через три дня, 30 октября 1967 года, ему вдогонку был запущен аппарат "Космос-188" в ту же плоскость орбиты. Оба этих аппарата были беспилотными вариантами корабля "Союз". После успешного отделения второго аппарата Космос-186 при помощи радиолокационной антенны должен был найти его, сблизиться в автоматическом режиме и совершить стыковку. Антенна захватила пассивный корабль на расстоянии 24 км от активного, и система автоматического управления начала высчитывать манёвр сближения. Само маневрирование происходило на "слепом", т.е. недоступном для наземных станций слежения, участке орбиты.
Первой увидела летящие корабли станция слежения в Евпатории. Она зафиксировала по телеметрии, что есть признаки захвата и стыковки. Телекамера активного корабля передала изображение неподвижного относительно него пассивного корабля. Теперь уже сомнений не было. Стыковка состоялась! Через 54 минуты после выдачи команды на поиск и сближение с кораблем-мишенью был произведен механический захват.
Это была первая в мире успешная автоматическая стыковка двух космических аппаратов, через полтора года после первой в мире стыковки в ручном режиме, проведенной экипажем американского корабля Джемини-8.
Пролетев два витка в состыкованном состоянии, "Космосы" были расстыкованы и продолжили каждый свою программу полёта. Космос-186 на следующий день был успешно сведен с орбиты и совершил посадку спускаемого аппарата. На Космосе-188 забарахлила система схода с орбиты, и корабль пришлось подорвать по команде с Земли.

Пост про kOS начинается здесь

Сближение, причаливание и стыковка - маневрирование по сведению двух космических аппаратов практически в одну точку пространства с нулевой относительной скоростью. Такие манёвры имеют большое значение как в КСП, так и в жизни, позволяя собирать на орбите корабли для миссий в дальний космос или модульные космические станции.

Для маневрирования по причаливанию придется использовать механизм "сырого" управления (raw steering) в kOS. Это значит, что, в отличие от команд типа lock steering to R(x,y,z), автоматически высчитывающих сигналы управления для удержания заданной ориентации, будут применяться команды, напрямую имитирующие нажатия игрока на клавиши.

Будем считать, что дальнее сближение уже проведено, т.е. в начальном состоянии корабли находятся на расстоянии меньше 2 км друг от друга с небольшой относительной скоростью.



Дальнейшее будет во многом переводом видео с канала CheersKevin. Если кому-то проще воспринимать с видео, вот ссылка (видео, естественно, на английском):
[media=//www.youtube.com/watch?v=Wa7le4-7ogY]

Итак, начинаем.
На человеческом языке сформулируем такой алгоритм сближения и причаливания:
0. Выбираем стыковочный узел, к которому нужно пристыковаться.
Определим "сферу безопасности" вокруг цели - всё маневрирование, кроме последнего этапа прямого сближения со стыковочным узлом на цели, должно происходить за пределами этой сферы.
1. Включаем РСУ. Управление переключаем на стыковочный узел.
2. Гасим относительную скорость.
3. Если корабль находится внутри сферы безопасности, то фиксируем его ориентацию и с помощью двигателей РСУ отводим от цели за пределы сферы безопасности.
4. Ориентируемся так, чтобы оси стыковочных узлов были параллельны.
5. Сдвигаем корабль так, чтобы стыковочные узлы были не только параллельно ориентированы, но и оказались на одной прямой.
6. Двигаемся вперёд к стыковочному узлу цели, гася скорость по мере приближения.
7. Гасим скорость до 0,2 - 0,3 м/с на расстоянии 2 м до цели, далее поддерживаем эту скорость до срабатывания стыковочных механизмов.

Схема траектории:
Рис. 1. Схема сближения для корабля, изначально находящегося за пределами сферы безопасности (сплошные стрелки) и внутри сферы безопасности (пунктирные стрелки).

"Сырое" управление аппаратом делается через структуру ship:control. В ней есть функции управления двигателем (действуют как нажатия на Shift/Ctrl), команд на вращение (нажатия W/A/S/D/Q/E) и на перемещение (I/J/K/L/H/N). Для маневрирования на сближение ориентация будет удерживаться автоматически через опосредованное управление, а команды на работу РСУ в режиме перемещения будут программироваться в прямом ("сыром") режиме.

Команды прямого управления перемещением:

SHIP:CONTROL:FORE       число в интервале [-1; 1], управляет движением вперёд/назад (клавиши H/N)
SHIP:CONTROL:TOP        число в интервале [-1; 1], управляет движением вверх/вниз (клавиши I/K)
SHIP:CONTROL:STARBOARD  число в интервале [-1; 1], управляет движением вправо/влево (клавиши L/J)
SHIP:CONTROL:TRANSLATION  вектор с компонентами (STARBOARD, TOP, FORE)

Управление, как видно, ведётся относительно ориентации корабля. Направления "вперёд", "вправо" и "вверх" относительно активного корабля определяются векторами SHIP:FACING:FOREVECTOR, SHIP:FACING:STARVECTOR и SHIP:FACING:TOPVECTOR, соответственно.

Теперь разберёмся, как с помощью этих команд перемещаться и удерживать заданную скорость движения.

Поскольку нам важно только то, как активный корабль движется относительно цели, удобно ввести систему координат, начало которой находится на корабле-цели. Стандартная система координат имеет начало на активном корабле, т.е. нужно вводить сдвиг начала координат:

RELATIVE_POSITION = SHIP:POSITION - TARGET:POSITION
так как SHIP:POSITION = V(0,0,0) по определению
RELATIVE_POSITION = -TARGET:POSITION

Cкорость также удобно считать относительно орбитальной скорости корабля-цели:

V_RELATIVE = SHIP:VELOCITY:ORBIT - TARGET:VELOCITY:ORBIT

Кроме этого, рассмотрим элементы структуры "стыковочный узел" в kOS, которые в дальнейшем пригодятся:

PART:FACING  - так же, как и с кораблём, возвращает ориентацию детали в пространстве в форме R(x,y,z). Доступно для любых деталей
PART:POSITION  - положение конкретной детали. Доступно для любых деталей
PART:SHIP  - аппарат, частью которого является деталь. Доступно для любых деталей
PORT:NODETYPE  - возвращает размер стыковочного узла (0 для 0.625 м, 1 для 1,25 м, 1p5 для 1,875 и т.д.)
PORT:NODEPOSITION - возвращает положение узла стыковки (не совпадает с PORT:POSITION, т.к. узел стыковки расположен не в центре детали)
PORT:ACQUIRERANGE - расстояние, на котором срабатывает притяжение

Приступаем к кодингу.

0. Выбираем стыковочный узел, в который будем целиться.
Определим "сферу безопасности" вокруг цели.

Стыковочный узел и радиус сферы безопасности будут глобальными переменными, которые нужны для работы программы стыковки, и далее будут использоваться в других участках кода.
Корабль, к которому стыкуемся, предварительно должен быть выбран как цель либо вручную в игре, либо в предыдущей части скрипта.
Чтобы по дороге к стыковочному узлу не задеть что-нибудь нужное, вроде радиаторов, антенн или солнечных панелей, определим вокруг цели сферу безопасности. Всё маневрирование, кроме подхода к стыковочному узлу в самом конце, будет производиться за пределами этой сферы.
 Для не слишком больших кораблей можно взять сферу в 25 метров и успокоиться.

set tgtport to target:dockingports[0].
set safedistance to 25.

При необходимости можно написать более сложную программу, которая перебирает все детали и определяет радиус конкретного корабля на основе их положения. Это предлагается сделать самостоятельно.

1. Включаем РСУ. Управление переключаем на стыковочный узел.
rcs on. set ship:dockingports[0]:controlfrom.[/code]
VESSEL:DOCKINGPORTS выдаёт список всех стыковочных узлов на аппарате. Для простоты будем в этой задаче считать, что и на активном корабле, и на цели лишь по одному стыковочному узлу, т.е. первый (и единственный) узел в списке - это как раз та деталь, с которой нужно управлять.
Деталь, с которой идёт управление кораблём (т.е. ориентация этой детали принимается за ориентацию всего корабля), обозначается как VESSEL:CONTROLPART. Чтобы переключить управление на конкретную деталь, нужна команда PART:CONTROLFROM (вызов команды эквивалентен нажатию "Control from here" в меню ПКМ).

2. Гасим относительную скорость.

Для начала определим скорость цели относительно корабля.
kOS не умеет выдавать скорости для отдельных деталей, но может для аппаратов. Чтобы получить аппарат, частью которого является какая-то деталь, можно воспользоваться суффиксом PART:SHIP и оттуда уже брать все свойства, которые можно получить для аппарата.

lock v_relative to ship:velocity:orbit - tgtport:ship:velocity:orbit.

 Чтобы свести относительную скорость в ноль, нужно дать команду на работу РСУ против скорости корабля относительно цели. Но РСУ работает в осях "вправо-вверх-вперёд", привязанных к ориентации корабля, а вектор относительной скорости задан в осях, привязанных к планете. Значит, нужно научиться переводить скорость (и другие векторы) в координатную систему, связанную с осями корабля.

Рассмотрим, как это делается, на примере двухмерного случая.

Рис. 2. Преобразование от одной системы координат к другой.

Обозначение вектора как V = (a₀, b₀) означает, что V = a₀i₀ + b₀j₀, где i₀ и j₀ - единичные векторы вдоль осей базовой координатной системы.
Нам же нужно представить вектор в виде V = a_кi_к + b_кj_к, где i_к и j_к - единичные векторы вдоль осей координатной системы корабля.
В этом случае коэффициенты a_к и bк[/sub] находятся проецированием вектора V на новые координатные оси:
a_к = (V · i_к), b_к = (V · j_к).

Следовательно, чтобы выдать управляющими двигателями импульс в сторону вектора V, нужна следующая функция:

function translatevec {
  parameter vec.
  local vec_copy to vec.
//нормируем вектор, чтобы от него осталось только направление смещения
  if vec:sqrmagnitude > 1 {
    set vec_copy to vec:normalized.
  }
  local facing to ship:facing.
  local tf to vdot(vec_copy, facing:vector).
  local ts to vdot(vec_copy, facing:starvector).
  local tt to vdot(vec_copy, facing:topvector).
  set ship:control:translation V(ts,tt,tf).
}

Гашение относительной скорости выглядит таким образом:

function kill_relative_velocity {
  parameter tgtport, thr to 0.1.
// thresh - относительная скорость, по достижении которой считаем цель неподвижной 
// (по умолчанию ставим на 0.1 м/с)
  local v_relative to ship:velocity:orbit - tgtport:ship:velocity:orbit.
  until v_relative:sqrmagnitude < thr*thr {
    translatevec(-v_relative).
    wait 0.
    set v_relative to ship:velocity:orbit - tgtport:ship:velocity:orbit.
  }
}

3. Если корабль находится внутри сферы безопасности, то фиксируем его ориентацию и с помощью двигателей РСУ отводим от цели за пределы сферы безопасности.

if tgtport:ship:position:mag < safedistance {
  local facing0 to ship:facing.
  lock steering to facing0.
  // Отходим от цели по прямой со скоростью 2 м/с
  local lock v_relative to ship:velocity:orbit - tgtport:ship:velocity:orbit.
  local lock tgtpos to tgtport:ship:position.
  until tgtpos:mag > safedistance*1.5 {
    local tgtvel to -tgtpos:normalized * 2.
    translatevec(tgtvel - v_relative).
    wait 0.
  }
  unlock v_relative.
  unlock tgtpos.
  // не забудем затормозиться, когда отощли на безопасное расстояние
  kill_relative_velocity(tgtport).
}

4. Ориентируемся так, чтобы оси стыковочных узлов были параллельны.

Будем держать такую ориентацию, чтобы стыковочные узлы смотрели в противоположные стороны, но направление "вверх" у обоих узлов совпадало.

unlock steering.
wait 0.
lock steering to lookdirup(-tgtport:facing:forevector, tgtport:facing:topvector).

5. Сдвигаем корабль так, чтобы стыковочные узлы были не только параллельно ориентированы, но и оказались на одной прямой.

Тут начинается самое интересное.
Самое сложное маневрирование потребуется, если корабль вначале находится "позади" цели, т.е. со стороны, противоположной стыковочному узлу. Нарисуем схему перемещений в этом случае вместе с координатной системой.

Движение в этом случае выглядит так:
I) Двигаться вдоль вектора j, пока расстояние "вбок" от цели не станет больше R_safe.
II) Двигаться вдоль вектора i, пока корабль не окажется на расстоянии больше R_safe впереди цели.
III) Сдвинуться к оси стыковочного узла цели.
IV) Двигаться вперёд к цели.

За вектор i следует взять вектор TGTPORT:FACING, в направлении которого смотрит порт, к которому стыкуемся, а для вектора j подойдёт любой единичный вектор, перпендикулярный i. Удобно взять за этот вектор VXCL(TGTPORT:FACING, -TARGET:POSITION), т.е. вектор положения корабля, из которого убрали компоненту, направленную вдоль i.

Теперь вопрос: как двигаться к заданной точке? Способ, который точно сработает - держать скорость постоянно направленной в эту точку. То есть для движения к нужной точке будем программировать команды, которые будут удерживать относительную скорость направленной на эту точку.

Последний момент: определим безопасную скорость маневрирования. С такой скоростью можно двигаться, чтобы иметь запас времени для торможения при опасности столкновения с целью. Из уравнения для тормозного пути при равноускоренном движении L = v²/2a получаем, что безопасная скорость пропорциональна корню из расстояния до цели.

function v_safe {
  parameter dist.
  return sqrt(dist) / 2. // даёт 5 м/с на расстоянии 100 метров - вроде разумно
}
function moveto {
  parameter origin. // объект, относительно которого задаётся положение
  parameter pos. // где нужно оказаться относительно положения origin
  parameter speed to v_safe(origin:position:mag). // с какой скоростью двигаться
  parameter tol to 0.5 * speed. // в какой окрестности "засчитывается" попадание
  local lock v_wanted to speed * (origin:position + pos):normalized.
  local lock v_relative to ship:velocity:orbit - origin:velocity:orbit.
  // "попали" тогда, когда ship:position - origin:position = pos
  // ship:position = pos + origin:position
  // а ship:position всегда равно V(0,0,0)
  until (origin:position + pos):mag < tol {
    translatevec(v_wanted - v_relative).
  }
  unlock v_wanted.
  unlock v_relative.
}
function approach {
  parameter tgtport, rsafe.
  local lock vec_i to tgtport:facing.
  local lock vec_j to vxcl(vec_i, -tgtport:ship:position):normalized.
  
  // фаза I
  if vxcl(vec_i, -tgtport:ship:position):mag < rsafe {
    print "Going around the target".
    moveto(tgtport:ship, vxcl(vec_j, -tgtport:ship:position) + vec_j*rsafe).
  }
  // фаза II
  if vdot(-tgtport:ship:position, vec_i) < rsafe {
    print "Getting in front of target".
    moveto(tgtport:ship, (vec_i + vec_j)*rsafe).
  }
  // фаза III
  // выравниваемся уже не по центру масс корабля-цели, а по оси стыковочного узла
  print "Getting in front of target docking port".
  moveto(tgtport:ship, tgtport:position - tgtport:ship:position + vec_i*rsafe).
  print "Ready for final approach".
  unlock vec_i.
  unlock vec_j.
}

6. Двигаемся вперёд к стыковочному узлу цели, гася скорость по мере приближения.

Приближение к узлу будем проводить на скорости чуть медленнее, чем остальное маневрирование, но положим нижний предел скорости 0,25 м/с.

function dock_finalize {
  parameter tgtport.
  print "Starting final docking approach".
  local dist to tgtport:nodeposition:mag * 0.75.
  // положение, в котором должен находиться центр масс активного аппарата относительно цетра масс цели, чтобы стыковочные 
  local newposition to tgtport:facing * dist + tgtport:nodeposition - ship:controlpart:position - tgtport:ship:position.
  until (tgtport:nodeposition - ship:controlpart:position):mag < tgtport:acquirerange*1.25 {
    moveto(tgtport:ship, newposition, max(0.25, v_safe(tgtport:ship:position) / 2), dist * 0.25).
    set dist to dist*0.75.
    set newposition to tgtport:facing * dist + tgtport:nodeposition - ship:controlpart:position - tgtport:ship:position.
  }
  unlock all.
}

7. Гасим скорость до 0,2 - 0,3 м/с на расстоянии 2 м до цели, далее поддерживаем эту скорость до срабатывания стыковочных механизмов.

Это, по существу, уже сделано, остаётся ждать стыковки. Момент, когда происходит стыковка, можно отследить по тому, что корабль становится состоящим как бы из двух "частей". Эти части в kOS называются "элементами", и получить их список можно суффиксом SHIP:ELEMENTS. Поэтому последний этап стыковки будет

wait until ship:elements:length > 1.

Если всё прошло успешно, то корабли состыкованы. Можно думать, что с ними делать дальше.

Здесь рассмотрены основные операции, не включающие проверки одинаковости стыковочных узлов или возврата управления со стыковочного узла на капсулу после завершения стыковки. Немного более замудрённый код лежит у меня в гитхаб-репозитории.