В процессе сближения космических аппаратов могут выделять два этапа: дальнего и ближнего наведения. Нас как раз интересует последний. Этап ближнего наведения подразумевает, что активный объект (например, корабль «Союз») находится в некоторой окрестности пассивного объекта (МКС), с которым будет происходить стыковка. Коррекция ориентации тут происходит за счёт маневровых двигателей. В идеале оси сопел должны располагаться в плоскостях, проходящих через центр масс и перпендикулярных главным осям инерции космического аппарата, а сами двигатели — на максимально возможном удалении. В реальности же конструктивные особенности не всегда позволяют соблюсти это требование, поэтому количество двигателей приходится увеличивать. Главная задача маневровых двигателей — обеспечить возможность ориентировать и стабилизировать космический аппарат относительно всех трёх осей.

Если с перемещением всё вполне интуитивно понятно (включили двигатель большой тяги — и полетели вперёд), то с пониманием вращения у новичков могут возникнуть затруднения. Для примера возьмём один из двигателей «Союза», который создаёт некоторую тягу , направленную вдоль его сопла. Помимо направления и величины тяги, двигатель имеет определённую позициюотносительно центра масс «Союза». Если вычислить векторное произведение , то получим ось вращения объекта, которая называется моментом силы, или крутящим моментом. Так как двигатель «работает» в одну сторону, а сила действует в другую, то можно поменять члены в произведении местами. Визуально это выглядит как перпендикуляр к плоскости, образованной этими векторами. Пример кода для одного двигателя и результат его выполнения:

var force = _thruster.forward;
var pos = _thruster.transform.position - _rigidbody.worldCenterOfMass;
var moment = Vector3.Cross(force, pos);

_rigidbody.AddTorque(moment, ForceMode.VelocityChange); //массу не учитываем

А вот как на самом деле располагаются двигатели на «Союзе»:

Компоновочная схема с расположением сопел двигательной установки по всем трём осям не требует каких-либо разворотов объекта для его движения вдоль осей.

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

Ограничения

Прежде чем начать, нужно оговорить ограничения для нашей будущей модели. Они довольно существенны:

• Внешние возмущающие силы (гравитация, световое давление и т. д.) отсутствуют.

• Массы объектов не учитываются.

• Угловые скорости пассивного объекта равны нулю.

• Двигатели активного объекта могут изменять тягу, но не её направление.

Про последний пункт хотелось бы рассказать чуть подробнее. Создание двигателей с плавным регулированием тяги в широком диапазоне — задача достаточно сложная. Поэтому в большинстве случаев для управления сближением космическим аппаратом используются маршевые двигатели, тяга которых не регулируется и может принимать нулевое или максимальное значение. В нашем случае будем считать, что инженеры постарались и поставили на наш «Союз» чудо-двигатели с идеально регулируемой тягой. К тому же мы делаем игрушку с эффектами, а не симулятор для научных исследований.

Математическая модель

Итак, каждый двигатель имеет некоторую позицию и направление тяги. Мы уже выяснили, что крутящий момент вычисляется через векторное произведение направления тяги и позиции двигателя относительно центра масс. Если просуммировать крутящие моменты всех работающих двигателей, то получим вектор, который и будет итоговым крутящим моментом для космического аппарата. То же самое справедливо и для векторов тяги. Наша цель состоит в том, чтобы вычислить, какие двигатели и с какой тягой нужно включить, чтобы результирующая сила и крутящий момент совпадали с заданными. Для этого составим следующую систему уравнений:

Обозначим и распишем систему по компонентам x, y, z:

Из системы видно, что количество неизвестных больше количества уравнений. К тому же значения неизвестных должны укладываться в определённый диапазон. Так как бороздить просторы Вселенной очень дорого и трудно, то нам нужно оптимальное решение системы, а не любое подходящее. Оптимальное решение означает минимизацию энергетических затрат на управление. Мне не хотелось изобретать велосипед для решения задач линейного программирования, поэтому мой выбор пал на коммерческий решатель Gurobi. Если честно, это единственная библиотечка, которую мне удалось завести в Unity (и то под виндой). Изначально выбор пал на OR-Tools, но после долгих мучений пришлось бросить эту затею. Ну да ладно.

Реализация в Unity

Для начала опишем структуры данных, с которыми будет работать наша модель:

/// <summary>
/// Данные двигателя
/// </summary>
public struct ThrusterData
{
    /// <summary>
    /// Максимальная тяга
    /// </summary>
    public float MaxForce;

    /// <summary>
    /// Направление тяги
    /// </summary>
    public Vector3 Dir;

    /// <summary>
    /// Позиция относительно центра масс
    /// </summary>
    public Vector3 Pos;

    /// <summary>
    /// Крутящий момент
    /// </summary>
    public Vector3 Moment;
}

Экземпляр этого класса будет возвращаться через метод GetData класса Thruster. Сам метод выглядит как-то так:

public ThrusterData GetData(Vector3 centerOfMass)
{
    var dir = transform.forward;
    var pos = transform.position - centerOfMass;

    return new ThrusterData()
    {
        MaxForce = _maxForce,
        Dir = dir,
        Pos = pos,
        Moment = Vector3.Cross(dir, pos)
    };
}

Структура для передачи данных в модель:

/// <summary>
/// Данные для модели
/// </summary>
public struct Data
{
    /// <summary>
    /// Разница позиций активного и пассивного объектов
    /// </summary>
    public Vector3 TransError;

    /// <summary>
    /// Разница вращений активного и пассивного объектов
    /// </summary>
    public Vector3 RotError;

    /// <summary>
    /// Линейная скорость пассивного объекта
    /// </summary>
    public Vector3 TargetVelocity;

    /// <summary>
    /// Линейная скорость активного объекта
    /// </summary>
    public Vector3 Velocity;

    /// <summary>
    /// Угловая скорость активного объекта
    /// </summary>
    public Vector3 AngularVelocity;

    /// <summary>
    /// Данные всех двигателей
    /// </summary>
    public List<ThrusterData> Thrusters = new();
}

Структура для хранения результата вычислений:

/// <summary>
/// Результат решения
/// </summary>
public struct Result
{
    /// <summary>
    /// Результирующая сила
    /// </summary>
    public Vector3 Force;

    /// <summary>
    /// Результирующий момент
    /// </summary>
    public Vector3 Moment;

    /// <summary>
    /// Тяги двигателей
    /// </summary>
    /// <remarks>
    /// Используется для эффектов
    /// </remarks>
    public List<float> Thrusters;
}

Я специально не стал использовать для статьи readonly struct из-за их громоздкости и невозможности использовать инициализаторы для наглядности. По этим же причинам я не буду в полной мере использовать MV*-подход и прочие прелести, чтобы не плодить лишние классы и связи. Раз данные описаны, давайте напишем метод корректировки, объединяющий сбор данных, вычисление требуемых сил и применение результата:

private void Correct(float factor)
{
    //_path - список целей (путь), через которые последовательно проходит активный объект
    var diff = _path[_stepIndex].rotation * Quaternion.Inverse(_centerOfMass.rotation);
    diff.ToAngleAxis(out float angle, out Vector3 axis);
    var rot = Mathf.Deg2Rad * angle * axis;

    var trans = _path[_stepIndex].position - _centerOfMass.position;

    var data = new Data()
    {
        TransError = factor * trans,
        RotError = factor * rot,
        TargetVelocity = _rigidbodyTarget.velocity, //МКС
        Velocity = _rigidbody.velocity,
        AngularVelocity = _rigidbody.angularVelocity,
        Thrusters = _thrusters.Select(t => t.GetData(_centerOfMass.position)).ToList()
    };

    //чудо-метод, в котором происходит математическое колдунство
    var result = Calculate(data);

    if (result == null)
    {
        return;
    }

    _rigidbody.AddForce(result.Force, ForceMode.VelocityChange);
    _rigidbody.AddTorque(result.Moment, ForceMode.VelocityChange);

    for (int i = 0; i < result.Thrusters.Count; i++)
    {
        float force = result.Thrusters[i];

        if (force > 0f)
        {
            //включаем эффекты
            _thrusters[i].Launch(force * _thrusterEffectMultiplier);
        }
    }
}

Наиболее сложной частью является вычисление разницы поворотов с использованием кватернионов. Для получения относительного поворота используется формула:

Действительно, если выполнить нижеприведённый код, то поворот obj1 станет идентичен повороту obj2.

var diff = Quaternion.Inverse(_obj1.rotation) * _obj2.rotation;
_obj1.rotation *= diff;

Но в методе наоборот! В случае AddTorque нам важно направление ошибки относительно желаемой ориентации. Если использовать этот порядок перемножения, то при задании крутящего момента объект начнёт вращаться в обратном направлении. Давайте посмотрим, что получилось:

Хорошо, конечно, что «Союз» вообще полетел в нужном направлении, но при таком раскладе экипаж падёт смертью храбрых, разбившись об МКС. Необходимо добавить объект, относительно которого будет происходить позиционирование корабля. Назовём его dockingPort. С таким исправлением всё должно работать правильно:

var diff =  _path[_stepIndex].rotation * Quaternion.Inverse(_dockingPort.rotation);
diff.ToAngleAxis(out float angle, out Vector3 axis);
var rot = Mathf.Deg2Rad * angle * axis;

var endPos = _path[_stepIndex].position - diff * (_dockingPort.position - _centerOfMass.position);
var trans = endPos - _centerOfMass.position;

Как видите, в последнем позиционировании «Союз» прошёл сквозь цель. Именно поэтому нужен список _path, который позволяет совершать подготовительные манёвры в безопасном месте, а до конечной цели добираться только по одной оси.

Решение задачи оптимизации с использованием Gurobi

Рассмотрим метод Calculate, который формирует и решает задачу линейного программирования:

private Result Calculate(Data data)
{
    var env = new GRBEnv(true);
    env.Start();

    var model = new GRBModel(env);
    var linExpr = new GRBLinExpr();

    //коэффициенты, которые необходимо найти
    var k = new GRBVar[data.Thrusters.Count];
    for (int i = 0; i < data.Thrusters.Count; i++)
    {
        //добавляем в модель переменную и накладываем на неё ограничения 0 <= ki <= MaxForce
        //после решения ответ будет содержаться в k[i].X
        k[i] = model.AddVar(0.0, data.Thrusters[i].MaxForce, 1.0, GRB.CONTINUOUS, $"k_{i}");
        linExpr.AddTerm(1.0, k[i]);
    }

    //сила и момент, разложенные на компоненты x, y, z
    var force = new GRBLinExpr[3] { new GRBLinExpr(), new GRBLinExpr(), new GRBLinExpr() };
    var moment = new GRBLinExpr[3] { new GRBLinExpr(), new GRBLinExpr(), new GRBLinExpr() };

    for (int i = 0; i < data.Thrusters.Count; i++)
    {
        //бежим по компонентам x, y, z
        for (int j = 0; j < 3; j++)
        {
            //ki * dX, ki * dY, ki * dZ
            force[j].AddTerm(-data.Thrusters[i].Dir[j], k[i]);

            //ki * mX, ki * mY, ki * mZ
            moment[j].AddTerm(data.Thrusters[i].Moment[j], k[i]);
        }
    }

    //"связываем" левую и правую части системы 
    for (int i = 0; i < 3; i++)
    {
        model.AddConstr(force[i], GRB.EQUAL, data.TransError[i] - data.Velocity[i] + data.TargetVelocity[i], $"force_{i}");
        model.AddConstr(moment[i], GRB.EQUAL, data.RotError[i] - data.AngularVelocity[i], $"moment_{i}");
    }

    model.SetObjective(linExpr, GRB.MINIMIZE);
    model.Optimize();

    if (model.Status == GRB.Status.OPTIMAL)
    {
        var result = new Result();

        for (int i = 0; i < k.Length; i++)
        {             
            float x = (float)k[i].X; //искомая тяга

            result.Force += -x * data.Thrusters[i].Dir;
            result.Moment += x * data.Thrusters[i].Moment;
            result.Thrusters.Add(x);
        }

        return result;
    }
    else
    {
        Debug.Log("Оптимальное решение не найдено");
    }

    model.Dispose();
    env.Dispose();

    return null;
}

В принципе, тут всё интуитивно понятно. Система уравнений легко угадывается через код:

• Класс GRBLinExpr используется для построения линейных выражений — сумм, составленных из переменных, умноженных на коэффициенты. Такие выражения применяются при определении целевой функции или левой части ограничений.

• Метод AddVar используется для добавления новой переменной в модель. При его вызове можно задать: нижнюю и верхнюю границы переменной, коэффициент в целевой функции и тип переменной (непрерывная, целочисленная и т.д.).

• Метод AddTerm применяется для добавления одного отдельного терма (коэффициент * переменная) в линейное выражение типа GRBLinExpr.

• Метод AddConstr добавляет ограничение в модель. В него передаётся линейное выражение (левая часть), знак сравнения (например, ≤, ≥, ==) и значение правой части ограничения.

Итоговый результат:

Исходник класса Spacecraft:

public class Spacecraft : MonoBehaviour
{
    [Header("Spacecraft")]
    [SerializeField]
    private Rigidbody _rigidbody;

    [SerializeField]
    private Transform _centerOfMass;

    [SerializeField]
    private Transform _dockingPort;

    [SerializeField]
    private List<Thruster> _thrusters;

    [Header("Target")]
    [SerializeField]
    private Rigidbody _rigidbodyTarget;

    [SerializeField]
    private List<Transform> _path;

    [Header("Settings")]
    [SerializeField]
    private float _correctionInterval = 4f;

    [SerializeField]
    private float _thrusterEffectMultiplier = 250f;

    //с таким значением корабль точно будет у текущей цели через _correctionInterval
    private float _factor => 1f / _correctionInterval;
    private int _stepIndex;

    private float _startTime;
    private bool _startDocking;
    private bool _lastCorrection;

    public void StartDocking()
    {
        _startTime = Time.time;
        _stepIndex = 0;
        _lastCorrection = false;
        _startDocking = true;

        Correct(_factor);
        NextTarget();
    }

    private Result Calculate(Data data)
    {
        var env = new GRBEnv(true);
        env.Start();

        var model = new GRBModel(env);
        var linExpr = new GRBLinExpr();

        //коэффициенты, которые необходимо найти
        var k = new GRBVar[data.Thrusters.Count];
        for (int i = 0; i < data.Thrusters.Count; i++)
        {
            //добавляем в модель переменную и накладываем на неё ограничения 0 <= ki <= MaxForce
            //после решения ответ будет содержаться в k[i].X
            k[i] = model.AddVar(0.0, data.Thrusters[i].MaxForce, 1.0, GRB.CONTINUOUS, $"k_{i}");
            linExpr.AddTerm(1.0, k[i]);
        }

        //сила и момент, разложенные на компоненты x, y, z
        var force = new GRBLinExpr[3] { new GRBLinExpr(), new GRBLinExpr(), new GRBLinExpr() };
        var moment = new GRBLinExpr[3] { new GRBLinExpr(), new GRBLinExpr(), new GRBLinExpr() };

        for (int i = 0; i < data.Thrusters.Count; i++)
        {
            //бежим по компонентам x, y, z
            for (int j = 0; j < 3; j++)
            {
                //k_i * d_x, k_i * d_y, k_i * d_z
                force[j].AddTerm(-data.Thrusters[i].Dir[j], k[i]);

                //k_i * m_x, k_i * m_y, k_i * m_z
                moment[j].AddTerm(data.Thrusters[i].Moment[j], k[i]);
            }
        }

        for (int i = 0; i < 3; i++)
        {
            model.AddConstr(force[i], GRB.EQUAL, data.TransError[i] - data.Velocity[i] + data.TargetVelocity[i], $"force_{i}");
            model.AddConstr(moment[i], GRB.EQUAL, data.RotError[i] - data.AngularVelocity[i], $"moment_{i}");
        }

        model.SetObjective(linExpr, GRB.MINIMIZE);
        model.Optimize();

        if (model.Status == GRB.Status.OPTIMAL)
        {
            var result = new Result();

            for (int i = 0; i < k.Length; i++)
            {
                float x = (float)k[i].X; //искомая тяга

                result.Force += -x * data.Thrusters[i].Dir;
                result.Moment += x * data.Thrusters[i].Moment;
                result.Thrusters.Add(x);
            }

            return result;
        }
        else
        {
            Debug.Log("Оптимальное решение не найдено");
        }

        model.Dispose();
        env.Dispose();

        return null;
    }

    private void Correct(float factor)
    {
        var diff = _path[_stepIndex].rotation * Quaternion.Inverse(_dockingPort.rotation);
        diff.ToAngleAxis(out float angle, out Vector3 axis);
        var rot = Mathf.Deg2Rad * angle * axis;

        var endPos = _path[_stepIndex].position - diff * (_dockingPort.position - _centerOfMass.position);
        var trans = endPos - _centerOfMass.position;

        var data = new Data()
        {
            TransError = factor * trans,
            RotError = factor * rot,
            TargetVelocity = _rigidbodyTarget.velocity,
            Velocity = _rigidbody.velocity,
            AngularVelocity = _rigidbody.angularVelocity,
            Thrusters = _thrusters.Select(t => t.GetData(_centerOfMass.position)).ToList()
        };

        var result = Calculate(data);

        if (result == null)
        {
            return;
        }

        _rigidbody.AddForce(result.Force, ForceMode.VelocityChange);
        _rigidbody.AddTorque(result.Moment, ForceMode.VelocityChange);

        for (int i = 0; i < result.Thrusters.Count; i++)
        {
            float force = result.Thrusters[i];

            if (force > 0f)
            {
                _thrusters[i].Launch(force * _thrusterEffectMultiplier);
            }
        }
    }

    private void NextTarget()
    {
        _stepIndex++;

        if (_stepIndex >= _path.Count)
        {
            if (_lastCorrection)
            {
                _startDocking = false;
            }

            _stepIndex = _path.Count - 1;
            _lastCorrection = true;
        }
    }

    private void Start()
    {
        _rigidbody.maxAngularVelocity = float.MaxValue;
        _rigidbody.inertiaTensor = Vector3.zero;
        _rigidbody.inertiaTensorRotation = Quaternion.identity;
        _rigidbody.centerOfMass = _centerOfMass.localPosition;
    }

    private void FixedUpdate()
    {
        if (!_startDocking)
        {
            return;
        }

        if ((Time.time - _startTime) >= _correctionInterval)
        {
            Correct(_factor);
            NextTarget();

            _startTime = Time.time;
        }
    }
}

Чтобы стыковка выглядела более эпично и не такой деревянной, можно поиграться с параметрами _correctionInterval и factor, а также придумать другие критерии для начала процесса корректировки. Я пробовал использовать ПИД-регулятор, но затухающие колебания вокруг цели выглядели, на мой взгляд, некрасиво.