1.在恒力扫查算法中增加ADRC算法，优化力数据输入及反馈。2.针对实时获取的机器人位姿做滤波，优化噪声影响计算。

SourceCode/OnGoingTasks/ForceControl/Demo/ForceControl/Data/poseList.txt

@@ -0,0 +1,17 @@
+-53.89385 -6.743176 500 -0.05161741 0.02653864 0.9983143 
+-54.12031 -5.207519 499.8 -0.05124845 0.02706202 0.9983191 
+-54.28967 -4.045112 499.8333 -0.05105245 0.02905356 0.9982734 
+-54.51295 -1.913761 499.7143 -0.05039231 0.0349615 0.9981174 
+-55.08918 1.409482 499.5714 -0.05038597 0.04471739 0.9977282 
+-55.66422 4.60705 499.4286 -0.05164719 0.05043616 0.997391 
+-56.12002 7.560856 499.2857 -0.05439709 0.05501859 0.9970024 
+-56.66143 11.56695 498.8571 -0.05001126 0.07238787 0.9961219 
+-57.08358 18.50531 498.4286 -0.02499454 0.09531506 0.9951333 
+-57.35617 26.01483 497.7143 -0.03982382 0.05589754 0.997642 
+-57.50999 32.44188 497 -0.0669629 0.03352772 0.997192 
+-57.16146 39.43418 496 -0.07075375 0.09019075 0.993408 
+-56.23135 48.50888 495 -0.0396059 0.1679043 0.9850075 
+-55.07056 58.37511 494 -0.0008314587 0.1433435 0.9896726 
+-53.56244 66.79867 493 -0.01615773 0.09944129 0.9949123 
+-52.82597 69.68119 492.5 -0.01986636 0.08800001 0.9959223 
+-51.98225 72.75071 492 -0.01336412 0.08798831 0.9960319 

SourceCode/OnGoingTasks/ForceControl/Demo/ForceControl/MainWindow.xaml.cs

@@ -573,7 +573,7 @@ namespace ForceControl
             try
             {
                 //从文本中读取路径点
-                string[] lines = System.IO.File.ReadAllLines("..\\Data\\poseListBang.txt");
+                string[] lines = System.IO.File.ReadAllLines("..\\Data\\poseList.txt");
                 _poseList.Clear();
                 foreach (string line in lines)
                 {

SourceCode/RUSTasks/TaskAdmittanceControl/AdmittanceControl/CartForceScan.cpp

@@ -1,4 +1,3 @@
-
 #include "CartForceScan.h"
 
 void CartForceScan::Init()
@@ -73,59 +72,81 @@ void CartForceScan::CartScanFunction(WayPoint& wayPoint)
     // 获取机器人目标参考位置
     Eigen::Matrix4d t2BRefer = Eigen::Map<Eigen::Matrix4d>(_refCart.position).transpose();
 
-    // 开始
     //The transformation of force/torques in different coordinate systems
     Sophus::SE3d t2BSE3(t2BCur);
-    Sophus::Matrix6d AdjT2B = t2BSE3.Adj();
-    Sophus::Vector6d baseFk = AdjT2B * toolFk;
+    Sophus::Vector6d t2Bse3 = t2BSE3.log();
 
-    // 机器人基座坐标系中的期望力
-    Sophus::Vector6d baseFd;
-    baseFd << baseFk(0), baseFk(1), 10, baseFk(3), baseFk(4), baseFk(5);
+    if (first)
+    {
+        tT2Bse3 = t2Bse3;
+        dtT2Bse3.setZero();
+    }
+    TrackingDifferentiator(t2Bse3, tT2Bse3, dtT2Bse3);
 
-    // deltaF
-    Sophus::Vector6d deltaFk = baseFk - baseFd;
+    Sophus::SE3d t2BSE3TD = Sophus::SE3d::exp(tT2Bse3);
+    t2BCur = t2BSE3TD.matrix();
+    Sophus::Matrix6d AdjT2B = t2BSE3TD.Adj();
+    Sophus::Vector6d baseFk = AdjT2B * toolFk;
 
     if (first)
     {
         t2Bkminus1 = t2BCur;
-        first = false;
-    }
 
-    // 状态观测器
-    
-    
+        uNLF.setZero();
 
+        oBF = baseFk;
+        doBF.setZero();
+        ddoBF.setZero();
 
+        tFd = baseFk;
+        dtFd.setZero();
+
+        tDF.setZero();
+        dtDF.setZero();
+        first = false;
+    }
+    // 扩张状态观测器
+    ExtendedStateObserver(baseFk, oBF, doBF, ddoBF, uNLF);// done
+
+    // 机器人基座坐标系中的期望力
+    Sophus::Vector6d baseFd;
+    baseFd << oBF(0), oBF(1), 10, oBF(3), oBF(4), oBF(5);
 
     // 最速跟踪微分器
+    TrackingDifferentiator(baseFd, tFd, dtFd);// done
 
+    // 非线性误差反馈律
+    uNLF = NonLinearFeedback(tFd, dtFd, oBF, doBF);// done
 
+    TrackingDifferentiator(oBF - baseFd, tDF, dtDF);
 
-    //// 导纳系统阻尼b自适应迭代
-    //double mu1 = 1.0; double mu2 = 1.0;
-    //Sophus::Vector6d ddeltaFk;
-    //Sophus::Vector6d uLim;
-    //Sophus::Vector6d sigma;
-    //Sophus::Vector6d phi;
-    //Sophus::Vector6d deltaB;
-    //Sophus::Vector6d Ve;
-    //Eigen::Matrix4d deltaTkminus1 = t2BCur * t2Bkminus1.inverse();
-    //Ve.head(3) << deltaTkminus1.block<3, 1>(0, 3);
-    //Eigen::AngleAxisd deltaRkminus1(deltaTkminus1.block<3, 3>(0, 0));
-    //Ve.tail(3) << deltaRkminus1.angle() * deltaRkminus1.axis();
+    // deltaF
+    Sophus::Vector6d deltaFk = tDF;
+
+    // 导纳系统阻尼b自适应迭代
+    double mu1 = 1.0; double mu2 = 1.0;
+    Sophus::Vector6d ddeltaFk = (deltaFk - deltaFkminus1) / Ts;
+    Sophus::Vector6d uLim;
+    Sophus::Vector6d sigma;
+    Sophus::Vector6d phi;
+    Sophus::Vector6d deltaB;
+    Sophus::Vector6d Ve;
+    Eigen::Matrix4d deltaTkminus1 = t2BCur * t2Bkminus1.inverse();
+    Ve.head(3) << deltaTkminus1.block<3, 1>(0, 3);
+    Eigen::AngleAxisd deltaRkminus1(deltaTkminus1.block<3, 3>(0, 0));
+    Ve.tail(3) << deltaRkminus1.angle() * deltaRkminus1.axis();
     //Sophus::Vector6d dXk = Ve - Vkminus1;
-    //if (dXk(2) != 0.0)
-    //{
-    //    ddeltaFk = (deltaFk - deltaFkminus1) / Ts;
-    //    uLim = (B.diagonal() + M.diagonal()).cwiseQuotient(B.diagonal());
-    //    //sigma = uLim.cwiseInverse();
-    //    sigma = (mu1 * deltaFk.cwiseAbs() + mu2 * ddeltaFk.cwiseAbs() + uLim).cwiseInverse();
-    //    phi = phi + sigma.cwiseProduct(deltaFk.cwiseQuotient(B.diagonal()));
-    //    deltaB = (B.diagonal().cwiseQuotient(dXk)).cwiseProduct(phi);
-    //    B.diagonal() = B.diagonal() + deltaB;
-    //    //Bd = B.toDenseMatrix();
-    //}
+    Sophus::Vector6d dXk = Ve;
+    if (dXk(2) != 0.0)
+    {
+        uLim = (B.diagonal() + M.diagonal()).cwiseQuotient(B.diagonal());
+        //sigma = uLim.cwiseInverse();
+        sigma = (mu1 * deltaFk.cwiseAbs() + mu2 * ddeltaFk.cwiseAbs() + uLim).cwiseInverse();
+        phi = phi + sigma.cwiseProduct(deltaFk.cwiseQuotient(B.diagonal()));
+        deltaB = (B.diagonal().cwiseQuotient(dXk)).cwiseProduct(phi);
+        B.diagonal() = B.diagonal() + deltaB;
+        //Bd = B.toDenseMatrix();
+    }
 
     // deltaF -> deltaX
 #if false //方法一
@@ -147,15 +168,16 @@ void CartForceScan::CartScanFunction(WayPoint& wayPoint)
     Sophus::Vector6d Vk = beta0 * deltaFk + beta1 * deltaFkminus1 + beta0 * deltaFkminus2 + beta2 * Vkminus1 + beta3 * Vkminus2;
 #endif //理论上方法二更好，实际上由于RCI内部可能存在的调节机制两种方法效果差不多
 
-    Sophus::Vector6d deltaXdk = Vk - Vkminus1;
+    Sophus::Vector6d deltaV = Vk - Vkminus1;
+    Sophus::Vector6d deltaXdk = Vk;
     // 等效轴
-    Eigen::Vector3d dAxis(baseFk(0), baseFk(1), baseFk(2)); dAxis.normalize();
+    Eigen::Vector3d dAxis(oBF(0), oBF(1), oBF(2)); dAxis.normalize();
     Eigen::AngleAxisd deltaR(deltaXdk.tail(3).norm(), dAxis);
 
     Eigen::Matrix4d deltaT = Eigen::Matrix4d::Identity();
     deltaT.block<3, 1>(0, 3) = deltaXdk.head(3);
-    //deltaT.block<3,3>(0,0)=deltaR.toRotationMatrix();
-    deltaT.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
+    deltaT.block<3, 3>(0, 0) = deltaR.toRotationMatrix();
+    //deltaT.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
 
     deltaFkminus2 = deltaFkminus1;
     deltaFkminus1 = deltaFk;
@@ -164,11 +186,6 @@ void CartForceScan::CartScanFunction(WayPoint& wayPoint)
     //将真实位移加入算法过程
     if (true)
     {
-        //Eigen::Matrix4d deltaTkminus1 = t2BCur * t2Bkminus1.inverse();
-        ////Vkminus1 = Sophus::SE3d(deltaTkminus1).log() + Vkminus2;
-        //Vkminus1.head(3) << deltaTkminus1.block<3, 1>(0, 3);
-        //Eigen::AngleAxisd deltaR(deltaTkminus1.block<3, 3>(0, 0));
-        //Vkminus1.tail(3) << deltaR.angle() * deltaR.axis();
         Vkminus1 = Ve;
     }
     else
@@ -179,41 +196,110 @@ void CartForceScan::CartScanFunction(WayPoint& wayPoint)
     t2Bkminus1 = t2BCur;
 
     // X = XRefer + XDelta
-    //Eigen::Matrix4d t2BTarget = deltaT * t2BRefer;
-    Eigen::Matrix4d t2BTarget = t2BRefer;
+    Eigen::Matrix4d t2BTarget = deltaT * t2BRefer;
+    //Eigen::Matrix4d t2BTarget = t2BRefer;
 
     Eigen::Map<Eigen::Matrix<double, 4, 4, Eigen::RowMajor>>(wayPoint.cartPos.position) = t2BTarget;
 
 #if true
-
     LogInfo((
-        std::to_string(deltaFk(0)) + " " +
-        std::to_string(deltaFk(1)) + " " +
-        std::to_string(deltaFk(2)) + " " +
-        std::to_string(baseFk(0)) + " " +
-        std::to_string(baseFk(1)) + " " +
+        std::to_string(tDF(2)) + " " +
+        std::to_string(uNLF(2)) + " " +
         std::to_string(baseFk(2)) + " " +
-        std::to_string(t2BCur(0, 3)) + " " +
-        std::to_string(t2BCur(1, 3)) + " " +
+
         std::to_string(t2BCur(2, 3)) + " " +
-        std::to_string(t2BRefer(0, 3)) + " " +
-        std::to_string(t2BRefer(1, 3)) + " " +
         std::to_string(t2BRefer(2, 3)) + " " +
-        std::to_string(t2BTarget(0, 3)) + " " +
-        std::to_string(t2BTarget(1, 3)) + " " +
         std::to_string(t2BTarget(2, 3)) + " " +
-        std::to_string(deltaT(0, 3)) + " " +
-        std::to_string(deltaT(1, 3)) + " " +
         std::to_string(deltaT(2, 3)) + " " +
-        //std::to_string(Vk(2)) + " " +
-        //std::to_string(dXk(2)) + " " +
-        //std::to_string(ddeltaFk(2)) + " " +
-        //std::to_string(uLim(2)) + " " +
-        //std::to_string(sigma(2)) + " " +
-        //std::to_string(phi(2)) + " " +
-        //std::to_string(deltaB(2)) + " " +
-        //std::to_string(B.diagonal()(2)) + " " +
+        std::to_string(deltaV(2, 3)) + " " +
+
+        std::to_string(ddeltaFk(2)) + " " +
+        std::to_string(Vk(2)) + " " +
+        std::to_string(dXk(2)) + " " +
+        std::to_string(uLim(2)) + " " +
+        std::to_string(sigma(2)) + " " +
+        std::to_string(phi(2)) + " " +
+        std::to_string(deltaB(2)) + " " +
+        std::to_string(B.diagonal()(2)) + " " +
         "\n").c_str());
 
 #endif
+}
+
+void CartForceScan::ExtendedStateObserver(Sophus::Vector6d mValue, Sophus::Vector6d& oValue, Sophus::Vector6d& doValue, Sophus::Vector6d& ddoValue, Sophus::Vector6d u)
+{
+    Sophus::Vector6d betaO1, betaO2, betaO3;
+    //betaO1.setConstant(1.0 / Ts);
+    //betaO2.setConstant(1.0 / (1.6 * std::pow(Ts, 1.5)));
+    //betaO3.setConstant(1.0 / (8.6 * std::pow(Ts, 2.2)));
+    betaO1.setConstant(200);
+    betaO2.setConstant(400);
+    betaO3.setConstant(600);
+
+    double del = 100 * Ts;
+
+    Sophus::Vector6d e = oValue - mValue;
+    oValue = oValue + Ts * (doValue - betaO1.cwiseProduct(e));
+    doValue = doValue + Ts * (ddoValue - betaO2.cwiseProduct(Fal(e, 0.5, del)) + u);
+    ddoValue = ddoValue + Ts * (-betaO3.cwiseProduct(Fal(e, 0.25, del)));
+}
+
+Sophus::Vector6d CartForceScan::Fal(Sophus::Vector6d e, double alpha, double delta)
+{
+    Sophus::Vector6d fal;
+    for (int i = 0; i < 6; i++)
+    {
+        if (std::abs(e(i)) <= delta)
+        {
+            fal(i) = e(i) / (std::pow(delta, 1 - alpha));
+        }
+        else
+        {
+            fal(i) = sign(e(i)) * std::pow(std::abs(e(i)), 1 - alpha);
+        }
+    }
+
+    return fal;
+}
+
+void CartForceScan::TrackingDifferentiator(Sophus::Vector6d eValue, Sophus::Vector6d& tValue, Sophus::Vector6d& dtValue)
+{
+    double gammaT = 5000;//gama为调节系数,gama越大跟踪效果越好,但微分信号会增加高频噪声,反之,微分信号越平滑,会产生一定的滞后
+    double etaT = Ts;//eta为步长, eta越小，滤波效果越好
+    double etaT0 = 30 * etaT;//一般来说, eta0略大于步长eta
+    Sophus::Vector6d fh = Fhan(tValue - eValue, dtValue, gammaT, etaT0);
+
+    tValue = tValue + etaT * dtValue;
+    dtValue = dtValue + etaT * fh;
+}
+
+Sophus::Vector6d CartForceScan::Fhan(Sophus::Vector6d x1, Sophus::Vector6d x2, double gama, double eta)
+{
+    Sophus::Vector6d fhan;
+    for (int i = 0; i < 6; i++)
+    {
+        double d = gama * std::pow(eta, 2);//eta为滤波因子 越大滤波效果越好
+        double a0 = eta * x2(i);
+        double y = x1(i) + a0;
+        double a1 = std::sqrt(d * (d + 8 * std::abs(y)));
+        double a2 = a0 + sign(y) * (a1 - d) / 2;
+        double a = (a0 + y) * fsg(y, d) + a2 * (1 - fsg(y, d));
+
+        fhan(i) = -gama * (a / d) * fsg(a, d) - gama * sign(a) * (1 - fsg(a, d));
+    }
+
+    return fhan;
+}
+
+Sophus::Vector6d CartForceScan::NonLinearFeedback(Sophus::Vector6d V1, Sophus::Vector6d V2, Sophus::Vector6d Z1, Sophus::Vector6d Z2)
+{
+    Sophus::Vector6d e1 = V1 - Z1;
+    Sophus::Vector6d e2 = V2 - Z2;
+
+    double betaN1 = -2.5;
+    double betaN2 = -0.02;
+    double del = 100 * Ts;
+    Sophus::Vector6d u = betaN1 * Fal(e1, 0.5, del) + betaN2 * Fal(e2, 1.1, del);
+
+    return u;
 }

SourceCode/RUSTasks/TaskAdmittanceControl/AdmittanceControl/CartForceScan.h

@@ -37,6 +37,29 @@ private:
     Sophus::Matrix6d Md;
     Sophus::Matrix6d Bd;
     Sophus::Matrix6d Kd;
+
+    template <typename T>
+    int sign(T val) { return (T(0) < val) - (val < T(0)); }
+
+    template <typename T>
+    int fsg(T val1, T val2) { return (sign(val1 + val2) - sign(val1 - val2)) / 2; }
+
+    Sophus::Vector6d tDF, dtDF;
+    Sophus::Vector6d tT2Bse3, dtT2Bse3;
+
+    // 状态观测器
+    Sophus::Vector6d oBF, doBF, ddoBF;
+    void ExtendedStateObserver(Sophus::Vector6d mValue, Sophus::Vector6d& oValue, Sophus::Vector6d& doValue, Sophus::Vector6d& ddoValue, Sophus::Vector6d u);
+    Sophus::Vector6d Fal(Sophus::Vector6d e, double alpha, double delta);
+
+    // 跟踪微分器
+    Sophus::Vector6d tFd, dtFd;
+    void TrackingDifferentiator(Sophus::Vector6d eValue, Sophus::Vector6d& tValue, Sophus::Vector6d& dtValue);
+    Sophus::Vector6d Fhan(Sophus::Vector6d x1, Sophus::Vector6d x2, double gama, double eta);
+
+    // 非线性反馈律
+    Sophus::Vector6d uNLF;
+    Sophus::Vector6d NonLinearFeedback(Sophus::Vector6d V1, Sophus::Vector6d V2, Sophus::Vector6d Z1, Sophus::Vector6d Z2);
 };
 #endif