六維力和力矩傳感器靜態標定(F/T sensor calibration)

通常在機械臂腕部與末端夾持器之間安裝六維力傳感器，用於機器人在裝配過程中的力反饋控制。六維力傳感器能夠測量三維空間中任何座標系下的三維力 $(F_{x},F_{y},F_{z})$ 和三維力矩 $(M_{x},M_{y},M_{z})$ 。在機械臂處於靜態時，機械臂腕部六維力傳感器測出的力與力矩數據由三部分組成:

傳感器自身系統誤差
傳感器與末端夾持器自身重力作用
末端夾持器所受外部接觸力

若要得到裝配軸在裝配過程中所受外部接觸力，需要消除六維力傳感器系統自身誤差以及裝配軸重力作用兩方面的影響。因此，我們需要通過標定與辨識算法，剔除這些干擾因素的影響，以便更精確地獲取與環境的接觸力信息。辨識與標定方法可分爲靜態標定與動態標定。https://zhuanlan.zhihu.com/p/62958434

靜態標定中，讓機器人運動到幾個姿態並停止，根據穩定時力矩傳感器的數值與最小二乘法來標定負載參數及傳感器自身參數。這種方式忽略了運動慣性的影響。
動態標定：即考慮負載運動速度等對力矩傳感器的影響。這種處理方式與機器人的動力學參數辨識類似，讓機器人數個軸連續地運動，採集力矩傳感器的數值與關節角度，完成對負載及零漂的辨識。

可能由於六維傳感器比較昂貴，雖然傳感器標定的理論很多論文都能找到，但是基於ROS的標定程序似乎很少，而且對傳感器有一定要求，GitHub上的force_torque_tool包需要用到imu傳感器測得座標系當中三維的重力加速度，記得實驗室使用的ATI gamma系列F/T sensor 似乎沒有imu模塊，所以暫時放棄，後續如果需要比較高精度的效果時可能還會給機器人末端安裝imu，到時候再回來研究這個程序。目前疫情在家，就先自己動手寫一個靜態標定的簡單demo，記錄一下。

理論部分：

[1] 高培陽. 基於力傳感器的工業機器人恆力磨拋系統研究[D].華中科技大學,2019.

[2] 董浩.軸孔裝配工業機器人柔順控制算法研究[D].河南理工大學,2018.

六維力傳感器座標系中負載重力的作用示意圖：

將六維力傳感器三個力分量的零點值記爲 $(F_{x0} ,F_{y0} , F_{z0})$ ，三個力矩分量的零點值記爲 $(M_{x0},M_{y0} ,M_{z0})$ 六維力傳感器的座標系 $\left \{ C \right \}$ 爲空間直角座標系，有三個座標軸，傳感器與末端夾持器重力爲，傳感器與末端夾持器質心centroid在六維力傳感器座標系中的座標爲，重力G 在三個座標軸方向的作用分力分別，重力G 對三個座標軸的作用力矩分別爲 $( M_{gx} ,M_{gy},M_{gz})$ 。根據力與力矩的關係可以得到：

將六維力傳感器直接測量得到的三個方向的力分量記爲，三個方向的力矩分量記爲,假設標定的時候沒有外部作用力作用在末端夾持器上，則力傳感器所測得的力和力矩有負載重力影響及零點組成，則有

聯立得：

其中 $F_{x0} , F_{y0}, F_{z0} , M_{x0} , M_{y0} , M_{z0}, x , y , z$ 均爲常數(這些值實際情況應該不是常數，畢竟力和力矩傳感器數據波動還是很厲害的，這裏靜態標定就不考慮那麼多了)，令

即：

控制機械臂末段夾持器的位姿，取 N 個不同的姿態（ N ≥ 3 ，要求至少有 3 個姿態下的機械臂末端夾持器的指向向量不共面），得到 N 組六維力傳感器數據， $M = F\cdot A$ ，其中

則矩陣的最小二乘解爲 $A = (F^TF)^{-1}\cdot F^TM$ ,根據多維數據，可以得到負載重心centriod在六維力傳感器座標系中的座標以及常數的值。矩陣的最小二乘解可以參考這個：https://blog.csdn.net/Sunshine_in_Moon/article/details/45797343

參考文獻中考慮到機器人安裝過程中和基座之間有一定傾角，同樣也會使傳感器的測量值與實際值存在一定偏差，目前實驗在仿真階段，就不考慮安裝傾角問題（如需考慮，請參考文獻）。記世界座標系爲 $\left \{ O \right \}$ ，安裝的基座標系 $\left \{ B \right \}$ ，則由基座標系(或世界座標系，因爲沒有考慮傾角)到六維力傳感器座標系的姿態變換矩陣爲 $_{W}^{C}\textrm{R}$ ，在ROS中可以通過 moveit! 編程接口獲取。重力在世界座標系 $\left \{ O \right \}$ 中的可以表示爲

$_{ }^{W}\textrm{G}=[0,0,-g]^T$ ,通過座標變換，得到重力在六維力傳感器座標系 $\left \{ C \right \}$ 中的向量爲： $_{ }^{C}\textrm{G}=_{W}^{C}\textrm{R}\cdot_{ }^{W}\textrm{G}=_{C}^{W}\textrm{R}^T\cdot_{ }^{W}\textrm{G}$ 。

則：

$\left [ F_x,F_y,F_z \right ]^T = \left [G_x,G_y,G_z \right ]^T+\left [ F_{x0},F_{y0},F_{z0} \right ]^T =_{C}^{W}\textrm{R}^T\cdot_{ }^{W}\textrm{G} + \left [ F_{x0},F_{y0},F_{z0} \right ]^T$

同理，取 N 個不同的姿態得到 N 組六維力傳感器數據， $f = R\cdot B$ ，其中 $B = [0,0,-g,F_{x0},F_{y0},F_{z0}]^T$ ，矩陣式和最小二乘式如下：

則矩陣的最小二乘解爲 $B = (R^TR)^{-1}\cdot R^Tf$ , 根據多維數據，可以得到傳感器的力零點值 $（F_{x0} , F_{y0}, F_{z0} ）$ $(F_{x0} , F_{y0}, F_{z0} )$ 已經重力的大小,將已經求出的負載重心座標及常數，帶入得：

由此可以得到傳感器的零點、負載的重量以及負載重心的座標已經全部得出。

測試代碼部分：

結合moveit！接口，用了三個機器人位姿，通過eigen矩陣完成最小二乘法計算過程，暫時只通過ROS發佈fake_force_torque信息，簡單的測試了一下，不確定是否有理論的錯誤。。。。僅供參考。。。

TODO：對傳感器數據連續採樣，對多個位姿的數據進行平均，然後再進行最小二乘法計算。

#include "ros/ros.h"
#include "GetSensorData.h"

GetSensorData::GetSensorData(ros::NodeHandle &n,double frequency,
                             std::string topic_wrench,std::string topic_calibration)
                             : nh_(n), loop_rate_(frequency)
{
    ///// Subscribers
    sub_wrench_ = nh_.subscribe(topic_wrench, 5,
                                &GetSensorData::wrench_callback, this, ros::TransportHints().reliable().tcpNoDelay());

    ////// Publishers
    pub_sensor_cali_info_ = nh_.advertise<least_squares_cali::calibration>(topic_calibration, 5);

    /// initializing the class variables
    F.setZero();
    T.setZero();
    wrench_force.setZero();
    wrench_torque.setZero();
    cali_info.setZero();

    R.setZero();
    N.setZero();
    f.setZero();
    cali_bias.setZero();
    rotation_matrix.setZero();

    /// 沒有這個自旋線程,ROS消息就像卡住一樣,打印不出來,必須ctrl+c才能顯示
    ros::AsyncSpinner *spinner;
    spinner = new ros::AsyncSpinner(1);
    spinner->start();

}

void GetSensorData::wrench_callback(const geometry_msgs::WrenchStampedConstPtr msg) {
    // Reading the FT-sensor in its own frame
    wrench_force <<
            0,msg->wrench.force.z, -(msg->wrench.force.y),1,0,0,
            -(msg->wrench.force.z),0,msg->wrench.force.x,0,1,0,
            msg->wrench.force.y,-(msg->wrench.force.x),0,0,0,1;

    wrench_torque << msg->wrench.torque.x,msg->wrench.torque.y, msg->wrench.torque.z;

    f <<  msg->wrench.force.x,msg->wrench.force.y,msg->wrench.force.z;

}

void GetSensorData::run() {

    initMoveGroup();

    moveNextPose();

    compute_cali_info();

    while (nh_.ok()) {
        publish_cali_info();
        ros::spinOnce();
        loop_rate_.sleep();
    }

}

void GetSensorData::initMoveGroup()
{
    arm_group = new moveit::planning_interface::MoveGroupInterface("manipulator");
    arm_group->setPlannerId("RRTConnectkConfigDefault");
}

void GetSensorData::moveNextPose()
{
    // execute random poses
    arm_group->setRandomTarget();
    arm_group->move();
    get_trans_matrix();
    // <3,6>表示3行6列,(0,0)表示起始位置
    F.block<3,6>(0,0) = wrench_force;
    R.block<3,3>(0,0) = rotation_matrix.transpose();
    R.block<3,3>(0,3) << 1,0,0,0,1,0,0,0,1;
    T.block<3,1>(0,0) = wrench_torque;
    N.block<3,1>(0,0) = f;

    // 設置當前狀態爲初始狀態
    arm_group->setStartStateToCurrentState();
    arm_group->setRandomTarget();
    arm_group->move();
    get_trans_matrix();
    F.block<3,6>(3,0) = wrench_force;
    R.block<3,3>(3,0) = rotation_matrix.transpose();
    R.block<3,3>(3,3) << 1,0,0,0,1,0,0,0,1;
    T.block<3,1>(3,0) = wrench_torque;
    N.block<3,1>(3,0) = f;

    arm_group->setStartStateToCurrentState();
    arm_group->setRandomTarget();
    arm_group->move();
    get_trans_matrix();
    F.block<3,6>(6,0) = wrench_force;
    R.block<3,3>(6,0) = rotation_matrix.transpose();
    R.block<3,3>(6,3) << 1,0,0,0,1,0,0,0,1;
    T.block<3,1>(6,0) = wrench_torque;
    N.block<3,1>(6,0) = f;

    std::cout<< " F ：" << F << std::endl;
    std::cout<< " R ：" << R << std::endl;
    std::cout<< " T ：" << T << std::endl;
    std::cout<< " N ：" << N << std::endl;
}

void GetSensorData::compute_cali_info()
{
    // 最小二乘法
    cali_info = (F.transpose() * F).inverse() * (F.transpose()) * T;
    std::cout<< "cali_info :"<< cali_info << std::endl;

    cali_bias = (R.transpose() * R).inverse() * (R.transpose()) * N;
    std::cout<< "cali_bias :"<< cali_bias << std::endl;
}

void GetSensorData::publish_cali_info()
{
    // Admittance Dynamics computation
    least_squares_cali::calibration calibration_msg;
    calibration_msg.header.stamp = ros::Time::now();
    calibration_msg.header.frame_id = "FT_sensor_link";

    calibration_msg.centroid.x = cali_info[0];
    calibration_msg.centroid.y = cali_info[1];
    calibration_msg.centroid.z = cali_info[2];

    calibration_msg.force_bias.x  = cali_bias[3];
    calibration_msg.force_bias.y  = cali_bias[4];
    calibration_msg.force_bias.z  = cali_bias[5];

    calibration_msg.torque_bias.x  = cali_info[3]-cali_bias[4]*cali_info[2]+cali_bias[5]*cali_info[1];
    calibration_msg.torque_bias.y  = cali_info[4]-cali_bias[5]*cali_info[0]+cali_bias[3]*cali_info[2];
    calibration_msg.torque_bias.z  = cali_info[5]-cali_bias[3]*cali_info[1]+cali_bias[4]*cali_info[0];
    pub_sensor_cali_info_.publish(calibration_msg);

}

bool GetSensorData::get_trans_matrix()
{
    tf::StampedTransform transform;
    try {
        listener.lookupTransform("base_link", "ee_link", ros::Time(0), transform);
        tf::matrixTFToEigen(transform.getBasis(), rotation_matrix);
    }
    catch (tf::TransformException ex) {
        rotation_matrix.setZero();
        ROS_WARN( "Waiting for TF from: ");
        return false;
    }

    return true;
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "get_sensor_data");

    ros::NodeHandle nh;
    double frequency = 100.0;

    std::string topic_wrench;
    std::string topic_calibration;

    /// LOADING PARAMETERS FROM THE ROS SERVER
    if (!nh.getParam("topic_wrench", topic_wrench)) {
        ROS_ERROR("Couldn't retrieve the topic name for the force/torque sensor.");
        return -1;
    }

    if (!nh.getParam("topic_calibration", topic_calibration)) {
        ROS_ERROR("Couldn't retrieve the topic topic_calibration");
        return -1;
    }

    GetSensorData get_sensor_data(nh,frequency,topic_wrench,topic_calibration);

    get_sensor_data.run();

    return 0;
}

六維力和力矩傳感器靜態標定(F/T sensor calibration)

理論部分：

測試代碼部分：

Nginx R31 doc 官方文檔-01-nginx 如何安裝

利用Robosense 16線雷達跑cartographer 2D

Robosense 16線激光雷達在ros中使用

Cartographer編譯安裝

apt-get報錯：E：無法獲得鎖/var/lib/dpkg/lock E :無法獲取dpkg前端鎖

加速度計和陀螺儀指南——數學模型和基本算法

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結