李群控制器SO(3)测试

测试代码是一个用于控制 SO(3) 空间中的系统的比例控制器。它通过计算控制策略来使当前状态逼近期望状态。该控制器使用比例增益 kp 进行参数化，然后进行一系列迭代以更新系统状态，最终检查状态误差是否小于给定的阈值。这个控制器用于姿态控制等应用。以下为测试源码：

#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); // 检查误差是否小于阈值
}

LIE-GROUP-CONTROLLERS  

包含专为李群设计的控制器的纯头文件 C++ 库

库背后的一些理论  

The library aims to contain some controllers designed in lie groups. The library depends only on Eigen and manif.

该库旨在包含一些以李群理论为基础设计的控制器。 该库仅依赖于 Eigen 和 manif。

All the controllers defined in lie-group-controllers have in common that they inherit from a templated base class (CRTP). It allows one to write generic code abstracting the controller details. This follows the structure of manif and Eigen.

lie-group-controllers 中定义的所有控制器都有一个共同点，即它们都继承自模板化基类 (CRTP)。它允许人们编写抽象控制器细节的通用代码。这遵循 manif 和 Eigen 的结构。

The library implements two controllers:

该库实现了两个控制器：

Proportional Controller (P controller)

比例控制器（P控制器）

Proportional Derivative Controller (PD controller)

比例微分控制器（PD控制器）

控制器具有以下形式

平凡化                      比例控制器                             比例微分控制器

where X and Xᵈ are elements of a Lie group. ∘ is the group operator. ψ represents an element in the Lie algebra of the Lie group whose coordinates are expressed in ℝⁿ.

其中 X 和 Xᵈ 是李群的元素。∘ 是群算子。ψ 表示李群李代数中的一个元素，其坐标用ℝⁿ 表示。

The controllers support all the groups defined in manif. Namely:

控制器支持 manif.h 中定义的所有群。即：

ℝ(n): Euclidean space with addition.

SO(2): rotations in the plane.

SE(2): rigid motion (rotation and translation) in the plane.

SO(3): rotations in 3D space.

SE(3): rigid motion (rotation and translation) in 3D space.

ℝ(n)：带加法的欧几里得空间。

SO(2)：平面内的旋转。

SE(2)：平面内的刚性运动（旋转和平移）。

SO(3)：3D 空间中的旋转。

SE(3)：3D 空间中的刚性运动（旋转和平移）。

SE_2(3)：3D 空间中的扩展位姿（旋转、平移和速度），（据我所知）由文章https://arxiv.org/pdf/1410.1465.pdf引入。注意：此处的实现与文章中的开发略有不同。

Bundle<>：允许将流形束作为单个李群进行操作。在参考论文https://arxiv.org/abs/1812.01537的第四节中称为复合流形。

其他李群可以而且将会被添加，欢迎贡献。

Please you can find further information in

请您在以下位置找到更多信息：

Modern Robotics: Mechanics, Planning, and Control,

Kevin M. Lynch and Frank C. Park,

Cambridge University Press, 2017,

ISBN 9781107156302

基本使用

The library implements proportional and proportional derivative controllers on Lie groups. What follows are two simple snippets that you can follow to build and use such controllers. For sake of simplicity, only controllers in SO(3) are shown. The very same applies to the other Lie groups

该库在李群上实现比例和比例微分控制器。 下面是两个简单的片段，您可以按照它们来构建和使用此类控制器。为了简单起见，仅示出了SO(3)中的控制器。这同样适用于其他李群

比例控制器 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();

比例微分控制器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();//获取控制输出

依赖库  

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

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

cmake

构建库  

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

 If you want to enable tests set the BUILD_TESTING option to ON.

在您的项目中使用李群控制器  

在您的项目中使用李群控制器

lie-group-controllers 提供原生 CMake 支持，使该库可以在 CMake 项目中轻松使用。请添加到您的 CMakeLists.txt

project(foo)

find_package(LieGroupControllers REQUIRED)

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

target_link_libraries(${PROJECT_NAME}LieGroupControllers::LieGroupControllers)

manif可用操作

manif 是一个李理论库，用于针对机器人应用的状态估计。它被开发为带有 Python 3 包装器的纯标头 C++11 库。        

Available Operations  

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

Operation		Code
	Base Operation
Inverse		X.inverse()
Composition		X * Y                   X.compose(Y)
Hat		w.hat()
Act on vector		X.act(v)
Retract to group element		w.exp()
Lift to tangent space		X.log()
Manifold Adjoint		X.adj()
Tangent adjoint		w.smallAdj()
	Composed Operation
Manifold right plus		X + w                   X.plus(w)                   X.rplus(w)
Manifold left plus		w + X                   w.plus(X)                   w.lplus(X)
Manifold right minus		X - Y                   X.minus(Y)                   X.rminus(Y)
Manifold left minus		X.lminus(Y)
Between		X.between(Y)
Inner Product		w.inner(t)
Norm		w.weightedNorm()                   w.squaredWeightedNorm()

参考网址：
https://github.com/ami-iit/lie-group-controllers  李群控制器源网址

https://www.youtube.com/watch?v=nHOcoIyJj2o&ab_channel=InstitutdeRob%C3%B2ticaiInform%C3%A0ticaIndustrial%2CCSIC-UPC  （视频）机器人专家的李理论Lie theory for the roboticist

https://arxiv.org/abs/1812.01537  机器人状态估计的微观李理论