Prueba SO(3) del controlador de grupo de mentiras

El código de prueba es un controlador proporcional para controlar un sistema en el espacio SO(3). Hace que el estado actual se acerque al estado deseado calculando la estrategia de control. El controlador se kpparametriza con una ganancia proporcional y luego realiza una serie de iteraciones para actualizar el estado del sistema y finalmente verifica si el error de estado es menor que un umbral determinado. Este controlador se utiliza en aplicaciones como el control de actitud. El siguiente es el código fuente de prueba:

#include <catch2/catch.hpp>
#include <manif/manif.h>
#include <LieGroupControllers/ProportionalController.h>
#include <LieGroupControllers/ProportionalDerivativeController.h>
using namespace LieGroupControllers;
int main()
{
    manif::SO3d desiredState, state;
    desiredState.setRandom();
    state.setRandom();
    std::cout << "当前姿态：\n"<<
        state.rotation() << std::endl;


    std::cout << "目标姿态：\r\n"<< desiredState.rotation() << std::endl;
    auto feedForward = Eigen::Vector3d::Zero();


    // 实例化控制器
    ProportionalControllerSO3d controller;
    constexpr double kp = 10;
    controller.setGains(kp);//设置控制器增益
    controller.setDesiredState(desiredState);//设置期望状态
    controller.setFeedForward(feedForward);//设置前馈


    //测试控制器
    constexpr double dT = 0.01;// 时间步长 秒  控制周期0.01s
    constexpr std::size_t numberOfIteration = 1e3; // 迭代次数1000
    for (std::size_t i = 0; i < numberOfIteration; i++)
    {
        controller.setState(state);// 设置当前状态
        controller.computeControlLaw(); // 计算控制策略
        auto controlOutput = controller.getControl();// 获取控制输出


        //传播系统的动态 Propagate the dynamics of the system.
        //首先，我们获取控制输出，这在这个特定情况下是惯性坐标系中的角速度 First of all we get the control output. In this particular case is the angular velocity
        // expressed in the inertial frame.
        // 然后使用 Manifold 左加运算符 Then the Manifold left plus operator is used
        // state = controlOutputDT  + state 应该理解为
        // state_k+1 = exp(omega * dT) * state_k
        manif::SO3d::Tangent controlOutputDT = controlOutput.coeffs() * dT;
        std::cout << "第" << i << "次控制角度增量（瞬时控制角速度*dt）：" << controlOutputDT << std::endl;
        state = controlOutputDT + state;


    }
    std::cout << "Test Over!调节时间10秒\n";
    std::cout << "最终姿态：\r\n" << state.rotation() << std::endl;
    std::cout << "姿态误差：\r\n" << desiredState.rotation() - state.rotation() << std::endl;


    // 检查误差
    //auto error = state.compose(desiredState.inverse()).log(); // 计算误差:  最终姿态坐标系在期望姿态坐标系中的姿态矩阵表示 取log
    //REQUIRE(error.coeffs().norm() < 1e-4); // 检查误差是否小于阈值
}

CONTROLADORES DEL GRUPO DE MENTIRAS

Biblioteca C++ de solo encabezado que contiene controladores diseñados para grupos de Lie

Alguna teoría detrás de la biblioteca

La biblioteca pretende contener algunos controladores diseñados en grupos de mentiras. La biblioteca depende sólo de Eigen y manif.

Esta biblioteca está destinada a contener algunos controladores diseñados sobre la base de la teoría de grupos de Lie. La biblioteca solo depende de Eigen y manifiesto.

Todos los controladores definidos en lie-group-controllers tienen en común que heredan de una clase base basada en plantilla (CRTP). Permite escribir código genérico que abstraiga los detalles del controlador. Esto sigue la estructura de manif y Eigen.

Todos los controladores definidos en lie-group-controllers tienen una cosa en común, es decir, todos heredan de una clase base con plantilla (CRTP). Permite a las personas escribir código genérico que abstrae los detalles del controlador. Esto sigue la estructura del manifiesto y Eigen.

La biblioteca implementa dos controladores:

Controlador proporcional (controlador P)

Controlador derivativo proporcional (controlador PD)

El controlador tiene la siguiente forma

Controlador proporcional trivial Controlador proporcional-derivativo

donde X y Xᵈ son elementos de un grupo de Lie. ∘ es el operador del grupo. ψ representa un elemento en el álgebra de Lie del grupo de Lie cuyas coordenadas se expresan en ℝⁿ.

donde X y Xᵈ son elementos del grupo de Lie. ∘ es un operador de grupo. ψ representa un elemento en el álgebra de Lie de grupos de Lie, y sus coordenadas se denotan por ℝⁿ.

Los controladores admiten todos los grupos definidos en manif. A saber:

El controlador admite todos los grupos definidos en manifest.h. Ahora mismo:

ℝ(n): Espacio euclidiano con suma.

SO(2): rotaciones en el plano.

SE(2): movimiento rígido (rotación y traslación) en el plano.

SO(3): rotaciones en el espacio 3D.

SE(3): movimiento rígido (rotación y traslación) en el espacio 3D.

ℝ(n): Espacio euclidiano con suma.

SO(2): Rotación en el plano.

SE(2): Movimiento rígido en un plano (rotación y traslación).

SO(3): Rotación en el espacio 3D.

SE(3): Movimiento rígido (rotación y traslación) en el espacio 3D.

SE_2(3): Pose extendida (rotación, traslación y velocidad) en el espacio 3D, introducida (hasta donde yo sé) por el artículo https://arxiv.org/pdf/1410.1465.pdf. Nota: La implementación aquí difiere ligeramente del desarrollo del artículo.

Paquete<>: permite la manipulación de paquetes múltiples como un único grupo de Lie. En la cuarta sección del artículo de referencia https://arxiv.org/abs/1812.01537 se denominan variedades compuestas.

Se pueden agregar y se agregarán otros grupos de Lie; las contribuciones son bienvenidas.

Por favor puede encontrar más información en

Encuentre más información en:

Modern Robotics: Mechanics, Planning, and Control,

Kevin M. Lynch and Frank C. Park,

Cambridge University Press, 2017,

ISBN 9781107156302

uso básico

La biblioteca implementa controladores derivativos proporcionales y proporcionales en grupos de Lie. Lo que sigue son dos fragmentos simples que puede seguir para crear y utilizar dichos controladores. Para simplificar, sólo se muestran los controladores en SO(3). Lo mismo se aplica a los otros grupos de Lie.

Esta biblioteca implementa controladores proporcionales y proporcional-derivados en grupos de Lie. A continuación se muestran dos fragmentos simples que puede seguir para crear y utilizar dicho controlador. Para simplificar, sólo se muestra el controlador en SO(3). Lo mismo se aplica a otros grupos de Lie.

Controlador proporcional SO(3)

//设置随机初始状态和零前馈 
//set random initial state and zero feedforward
// manif::SO3d是一个三维旋转矩阵类，用于表示三维空间中的旋转
manif::SO3d desiredState, state;
desiredState.setRandom();
state.setRandom();//随机设置旋转矩阵的值
Eigen::Vector3d feedForward = Eigen::Vector3d::Zero();//创建一个零向量


//创建控制器 create the controller.
ProportionalControllerSO3d controller;// 一个比例控制器，用于计算控制律


// 如果您想使用正确的普通控制器In case you want to use the right trivialized controller
// ProportionalControllerTplSO3d<Trivialization::Right> controller;// 一个右平凡化的比例控制器, 它与上面提到的比例控制器类似，但使用了不同的数学方法来计算控制律


//设置比例增益 set the proportional gain 
const double kp = 10;
controller.setGains(kp);// 设置比例增益


//设置所需的状态、前馈和状态 set the desired state, the feed-forward, and the state
controller.setDesiredState(desiredState);// 设置期望状态
controller.setFeedForward(feedForward);// 设置前馈
controller.setState(state);// 设置状态


//计算控制律 compute the control law
controller.computeControlLaw();
const auto& controlOutput = controller.getControl();

Controlador Derivativo Proporcional SO(3)

// set random initial state and zero feedforward设置了随机的初始状态和零前馈
manif::SO3d desiredState, state;
desiredState.setRandom();
state.setRandom();
manif::SO3d::Tangent stateDerivative = Eigen::Vector3d::Zero();
manif::SO3d::Tangent desiredStateDerivative = Eigen::Vector3d::Zero();
Eigen::Vector3d feedForward = Eigen::Vector3d::Zero();


// create the controller.
ProportionalDerivativeControllerSO3d controller;//一个比例微分控制器，用于计算控制律


// In case you want to use the right trivialized controller
// ProportionalDerivativeControllerTplSO3dcontroller;//如果您想使用正确的普通控制器.这是一个右平凡化的比例导数控制器。它与上面提到的比例导数控制器类似，但使用了不同的数学方法来计算控制律


// set the proportional and the derivative gains
//设置比例增益和微分增益
constdouble kp =10;
constdouble kd =2* std::sqrt(kp);
controller.setGains(kp, kd);


// set the desired state, its derivative, the feed-forward, and the state设置所需的状态、其微分、前馈和状态
controller.setDesiredState(desiredState, desiredStateDerivative);
controller.setFeedForward(feedForward);
controller.setState(state, stateDerivative);


//计算控制律 compute the control law
controller.computeControlLaw();
constauto& controlOutput = controller.getControl();//获取控制输出

biblioteca dependiente

manif https://github.com/artivis/manif/tree/devel

Eigen3 https://gitlab.com/libeigen/eigen/-/tree/3.4.1?ref_type=heads

cmake

construir biblioteca

git clone https://github.com/GiulioRomualdi/lie-group-controllers.git

cd lie-group-controllers

mkdir build && cd build

cmake ../

cmake --build .

[sudo] cmake --build . --target install

Si desea habilitar las pruebas, configure la BUILD_TESTING opción en ON.

Utilice controladores de Lie Group en sus proyectos

lie-group-controllers proporciona soporte nativo de CMake, lo que hace que esta biblioteca sea fácil de usar en proyectos de CMake. Por favor agregue a su CMakeLists.txt

project(foo)

find_package(LieGroupControllers REQUIRED)

add_executable(${PROJECT_NAME} src/foo.cpp)

target_link_libraries(${PROJECT_NAME}LieGroupControllers::LieGroupControllers)

Manif operaciones disponibles

manif es una biblioteca de teoría de mentiras para la estimación de estado para aplicaciones de robótica. Fue desarrollado como una biblioteca C++ 11 de solo encabezado con un contenedor Python 3.

Operaciones disponibles

https://github.com/artivis/manif/tree/devel

Operación		Código
	Operación básica
Inverso		X.inverso()
Composición		X * Y X.componer(Y)
Sombrero		qué()
Actuar sobre el vector		X.acto(v)
Retraer al elemento del grupo		w.exp()
Ascensor al espacio tangente		X.log()
Adjunto del colector		X.adj()
Adjunto tangente		w.pequeñoAdj()
	Operación compuesta
Colector derecho plus		X + w X.plus(w) X.rplus(w)
Colector izquierdo plus		w + X w.más(X) w.lmás(X)
Colector derecho menos		X - Y X.minus(Y) X.rminus(Y)
Colector izquierdo menos		X.lmenos(Y)
Entre		X.entre(Y)
Producto Interno		w.interior(t)
Norma		w.weightedNorm() w.squaredWeightedNorm()

URL de referencia:
https://github.com/ami-iit/lie-group-controllers URL de origen del controlador del grupo Lie

https://www.youtube.com/watch?v=nHOcoIyJj2o&ab_channel=InstitutdeRob%C3%B2ticaiInform%C3%A0ticaIndustrial%2CCSIC-UPC (Vídeo) Teoría de la mentira para el robotista

https://arxiv.org/abs/1812.01537 Teoría de la mentira microscópica para la estimación del estado del robot

[Álgebra de mentiras del grupo de mentiras] Introducción a los controladores de grupos de mentiras (controladores de grupos de mentiras): demostración de controlador proporcional para controlar sistemas en el espacio SO (3)...