第9章 数值优化
优化中的牛顿法
高斯 - 牛顿算法
高斯 - 牛顿法示例，逆运动学问题
非线性最小二乘法（曲线拟合）
非线性回归
非线性回归与非线性拟合的区别
列文伯格 - 马夸尔特算法
优化中的牛顿法
高斯 - 牛顿算法
高斯 - 牛顿算法是牛顿法的一种改进，用于寻找函数的最小值。
与牛顿法不同，它不需要二阶导数（二阶导数可能较难计算）。
参考文献：https://math.stackexchange.com/questions/19948/pseudoinverse-matrix-and-svd
高斯 - 牛顿法示例，逆运动学问题
接下来我们将求解一个三连杆平面机器人的逆运动学问题。
这段C++代码主要涉及正向运动学和逆运动学计算。它使用Eigen库处理矩阵和几何变换，定义了多个类型和函数模板，提供了数值微分功能，并实现了目标函数，计算机器人末端执行器在二维平面上的位置和姿态。
主要内容概括：
- 包含的库
：包括数学、输入输出流以及Eigen库的密集矩阵运算、几何运算和数值微分头文件。
- 类型定义
：定义了向量类型、矩阵类型和二维仿射变换类型。
- 函数声明
：
forward_kinematics：正向运动学函数。
pseudoInverse：伪逆函数模板。
transformationMatrixToPose：将变换矩阵转换为姿态的函数。
distanceError：计算距离误差函数。
inverse_kinematics：逆运动学函数（两种形式）。
：
Functor：一个通用的函数对象结构体，定义了输入和输出类型及其维度。
numericalDifferentiationFKFunctor：数值微分正向运动学函数对象，实现了具体的目标函数，计算机器人末端在平面上的位置（x,y）和旋转角度（theta）。
：
signum：符号函数模板，处理符号类型和非符号类型。
normaliseAngle和
normaliseAngle2：角度归一化函数模板。
这段代码可以用于机器人运动学计算，特别是涉及到正向和逆向的运动学问题。
task.hpp
#include <math.h> // 包含数学库的头文件
#include <iostream> // 包含输入输出流的头文件
#include <Eigen/Dense> // 包含Eigen库的密集矩阵运算头文件
#include <Eigen/Geometry> // 包含Eigen库的几何运算头文件
#include <eigen3/unsupported/Eigen/NumericalDiff> // 包含Eigen库不支持的数值微分头文件
typedef Eigen::VectorXd vector_t;// 定义向量类型
typedef Eigen::MatrixXd matrix_t;// 定义矩阵类型
typedef Eigen::Transform<double,2,Eigen::Affine> trafo2d_t;// 定义二维仿射变换类型
trafo2d_t forward_kinematics(vector_t const& q );// 声明正向运动学函数
template<typenameT>
T pseudoInverse(const T &a,double epsilon = std::numeric_limits<double>::epsilon());// 声明伪逆函数模板
template<typename_Scalar,int NX = Eigen::Dynamic,int NY = Eigen::Dynamic>
structFunctor
{
typedef _Scalar Scalar;// 定义标量类型
enum{
InputsAtCompileTime = NX,// 编译时输入维度
ValuesAtCompileTime = NY // 编译时输出维度
};
typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;// 输入类型
typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;// 输出类型
typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;// 雅可比类型
int m_inputs, m_values;// 输入和输出维度
Functor():m_inputs(InputsAtCompileTime),m_values(ValuesAtCompileTime){}// 默认构造函数
Functor(int inputs,int values):m_inputs(inputs),m_values(values){}// 参数化构造函数
intinputs()const{return m_inputs;}// 获取输入维度
intvalues()const{return m_values;}// 获取输出维度
};
structnumericalDifferentiationFKFunctor:Functor<double>
{
numericalDifferentiationFKFunctor():Functor<double>(3,3){}// 简单构造函数
intoperator()(const Eigen::VectorXd &q, Eigen::VectorXd &fvec)const// 实现目标函数
{
trafo2d_t t =forward_kinematics(q);// 调用正向运动学函数
double theta =atan2(t.rotation()(1,0),t.rotation()(0,0));// 计算旋转角度
double x = t.translation()(0);// 获取平移X分量
double y = t.translation()(1);// 获取平移Y分量
fvec(0)= x;// 设置输出向量X分量
fvec(1)= y;// 设置输出向量Y分量
fvec(2)= theta;// 设置输出向量旋转角度
return0;// 返回成功状态
}
};
Eigen::MatrixXd numericalDifferentiationFK(const Eigen::VectorXd &q);// 声明数值微分正向运动学函数
Eigen::VectorXd transformationMatrixToPose(trafo2d_t const&m);// 声明将变换矩阵转换为姿态的函数
Eigen::VectorXd distanceError(trafo2d_t const&golesStart, trafo2d_t const&poseStart);// 声明距离误差函数
template<typenameT>inlineconstexpr
intsignum(T x, std::false_type is_signed);// 声明符号函数模板，非符号类型
template<typenameT>inlineconstexpr
intsignum(T x, std::true_type is_signed);// 声明符号函数模板，符号类型
template<typenameT>
voidnormaliseAngle(T &q);// 声明角度归一化函数模板
template<typenameT>
voidnormaliseAngle2(T &q);// 声明另一个角度归一化函数模板
voidnormaliseAngle(Eigen::VectorXd &q);// 声明角度归一化函数
voidnormaliseAngle2(Eigen::VectorXd &q);// 声明另一个角度归一化函数
vector_t inverse_kinematics(vector_t const& q_start, trafo2d_t const& goal );// 声明逆运动学函数，带起始位置
vector_t inverse_kinematics(trafo2d_t const& goal );// 声明逆运动学函数，带目标位置task.cpp
#include "task.hpp" // 包含自定义任务的头文件
typedef Eigen::VectorXd vector_t;// 定义Eigen库中的向量类型，用于表示关节角度等
typedef Eigen::MatrixXd matrix_t;// 定义Eigen库中的矩阵类型，用于表示雅可比矩阵等
typedef Eigen::Transform<double,2,Eigen::Affine> trafo2d_t;// 定义二维仿射变换类型，用于表示机器人连杆的变换
/**************************************************
* 计算平面3连杆机器人的正向运动学的函数。
* 输入参数q为关节角度向量，返回值为末端执行器的变换矩阵。
*************************************************/
trafo2d_t forward_kinematics(vector_t const& q ){
// 检查关节角度向量的大小是否为3，如果不是，则程序将中止
assert( q.size()==3);
// 定义两个关节之间的恒定偏移，初始化为单位矩阵
trafo2d_t link_offset = trafo2d_t::Identity();
link_offset.translation()(1)=1.;// 在y轴方向上的平移量设为1
// 定义机器人的起始位姿，初始化为单位矩阵
trafo2d_t trafo = trafo2d_t::Identity();
for(int joint_idx =0; joint_idx <3; joint_idx++){
// 添加由当前关节贡献的旋转变换
trafo *= Eigen::Rotation2D<double>(q(joint_idx));
// 添加当前连杆的偏移变换
trafo = trafo * link_offset;
}
// 返回末端执行器的变换矩阵
return trafo;
}
template<typenameT>
T pseudoInverse(const T &a,double epsilon)
{
//Eigen::DecompositionOptions flags;
int flags;
// 对于非方阵，设置分解选项
if(a.cols()!= a.rows())
{
flags = Eigen::ComputeThinU | Eigen::ComputeThinV;
}
else
{
// 对于方阵，设置分解选项
flags = Eigen::ComputeFullU | Eigen::ComputeFullV;
}
// 计算SVD分解
Eigen::JacobiSVD<T>svd(a, flags);
// 计算容忍度
double tolerance = epsilon * std::max(a.cols(), a.rows())* svd.singularValues().array().abs()(0);
// 返回伪逆矩阵
return svd.matrixV()*(svd.singularValues().array().abs()> tolerance).select(svd.singularValues().array().inverse(),0).matrix().asDiagonal()* svd.matrixU().adjoint();
}
// 数值微分正向运动学函数，输入关节角度向量q，返回雅可比矩阵
Eigen::MatrixXd numericalDifferentiationFK(const Eigen::VectorXd &q)
{
numericalDifferentiationFKFunctor functor;// 定义函数对象
Eigen::NumericalDiff<numericalDifferentiationFKFunctor>numDiff(functor);// 定义数值微分对象
Eigen::MatrixXd fjac(3,3);// 定义雅可比矩阵
numDiff.df(q, fjac);// 计算雅可比矩阵
return fjac;// 返回雅可比矩阵
}
// 将变换矩阵转换为位姿向量，包含x、y和旋转角度theta
Eigen::VectorXd transformationMatrixToPose(trafo2d_t const&m)
{
double theta =atan2(m.rotation()(1,0), m.rotation()(0,0));// 计算旋转角度
double x = m.translation()(0);// 获取x方向的平移量
double y = m.translation()(1);// 获取y方向的平移量
Eigen::VectorXd fvec(3);// 定义位姿向量
fvec(0)= x;// 设置x方向的平移量
fvec(1)= y;// 设置y方向的平移量
fvec(2)= theta;// 设置旋转角度
return fvec;// 返回位姿向量
}
// 计算目标位姿与当前位姿之间的距离误差
Eigen::VectorXd distanceError(trafo2d_t const&golesStart, trafo2d_t const&poseStart)
{
returntransformationMatrixToPose(golesStart)-transformationMatrixToPose(poseStart);// 返回位姿差
}
template<typenameT>inlineconstexpr
intsignum(T x, std::false_type is_signed){
returnT(0)< x;// 判断非符号类型的符号函数
}
template<typenameT>inlineconstexpr
intsignum(T x, std::true_type is_signed){
return(T(0)< x)-(x <T(0));// 判断符号类型的符号函数
}
// 符号函数，根据输入类型选择对应的符号计算方法
template<typenameT>inlineconstexpr
intsignum(T x){
returnsignum(x, std::is_signed<T>());// 根据输入类型调用不同的符号函数
}
// 角度归一化函数模板
template<typenameT>
voidnormaliseAngle(T &q)
{
int sign =signum(q);// 获取角度的符号
q =fabs(q);// 取角度的绝对值
q = sign *remainder(q,2* M_PI);// 归一化角度
if(sign <0)
q = q +2* M_PI;// 如果角度为负，加2π
}
// 另一个角度归一化函数模板
template<typenameT>
voidnormaliseAngle2(T &q)
{
int sign =signum(q);// 获取角度的符号
q =remainder(q, sign *2* M_PI);// 归一化角度
if(-2* M_PI <= q && q <=-M_PI)
q = q +2* M_PI;// 如果角度在-2π到-π之间，加2π
elseif(M_PI <= q && q <=2* M_PI)
q =2* M_PI - q;// 如果角度在π到2π之间，取2π减去角度
}
// 归一化向量中的每个角度
voidnormaliseAngle(Eigen::VectorXd &q)
{
for(int i =0; i < q.rows(); i++)
{
normaliseAngle(q(i));// 调用角度归一化函数
}
}
// 归一化向量中的每个角度，使用另一种归一化方法
voidnormaliseAngle2(Eigen::VectorXd &q)
{
for(int i =0; i < q.rows(); i++)
{
normaliseAngle2(q(i));// 调用另一个角度归一化函数
}
}
/*************************************************
* 任务：
* 完成上述正向运动学函数定义的机器人逆运动学函数。
* 它应返回给定目标参数指定的关节角度q。
* 只需要匹配目标的平移部分（不旋转）。
*
* 提示：
* - 这是一个非线性优化问题，可以使用迭代算法解决。
* - 要获取雅可比矩阵，请使用数值微分
* - 要反转雅可比矩阵，请使用Eigen::JacobiSVD
* - 当误差范数小于1e-3时，算法应停止
* - 当迭代次数达到200次时，算法应停止
************************************************/
vector_t inverse_kinematics(vector_t const& q_start, trafo2d_t const& goal )
{
vector_t q = q_start;// 初始化关节角度向量为起始值
vector_t delta_q(3);// 定义关节角度增量向量
double epsilon =1e-3;// 定义误差容忍度
int i =0;// 迭代次数初始化
double gamma;// 定义增量因子
double stepSize =10;// 定义步长
// 迭代求解逆运动学
while((distanceError(goal,forward_kinematics(q)).squaredNorm()> epsilon)&&(i <200))
{
Eigen::MatrixXd jacobian =numericalDifferentiationFK(q);// 计算雅可比矩阵
Eigen::MatrixXd j_pinv =pseudoInverse(jacobian);// 计算雅可比矩阵的伪逆
Eigen::VectorXd delta_p =transformationMatrixToPose(goal)-transformationMatrixToPose(forward_kinematics(q));// 计算位姿误差
gamma =sqrt(pow(delta_p(0),2)+pow(delta_p(1),2));// 计算误差范数
//std::cout<<"gamma" <<gamma <<std::endl;
// 如果误差范数大于2，对误差进行缩放
if(gamma >2)
{
delta_p(0)=delta_p(0)/ stepSize * gamma;// 缩放x方向误差
delta_p(1)=delta_p(1)/ stepSize * gamma;// 缩放y方向误差
}
else
{
delta_p(0)=delta_p(0)/ stepSize;// x方向误差除以步长
delta_p(1)=delta_p(1)/ stepSize;// y方向误差除以步长
}
delta_q = 
最低0.47元/天 解锁文章
扫一扫
1483
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
