六维力和力矩传感器静态标定(F/T sensor calibration)_人工智能

通常在机械臂腕部与末端夹持器之间安装六维力传感器，用于机器人在装配过程中的力反馈控制。六维力传感器能够测量三维空间中任何坐标系下的三维力和三维力矩。在机械臂处于静态时，机械臂腕部六维力传感器测出的力与力矩数据由三部分组成:

传感器自身系统误差
传感器与末端夹持器自身重力作用
末端夹持器所受外部接触力

若要得到装配轴在装配过程中所受外部接触力，需要消除六维力传感器系统自身误差以及装配轴重力作用两方面的影响。因此，我们需要通过标定与辨识算法，剔除这些干扰因素的影响，以便更精确地获取与环境的接触力信息。辨识与标定方法可分为静态标定与动态标定。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在六维力传感器坐标系中的坐标为 $(x,y,z)$ ，重力G 在三个坐标轴方向的作用分力分别 $(G_x ,G_y ,G_z )$ ，重力G 对三个坐标轴的作用力矩分别为 $( M_{gx} ,M_{gy},M_{gz})$ 。根据力与力矩的关系可以得到：六维力和力矩传感器静态标定(F/T sensor calibration) 将六维力传感器直接测量得到的三个方向的力分量记为 $(F_x,F_y,F_z)$ ，三个方向的力矩分量记为 $(M_x,M_y,M_z)$ ,假设标定的时候没有外部作用力作用在末端夹持器上，则力传感器所测得的力和力矩有负载重力影响及零点组成，则有六维力和力矩传感器静态标定(F/T sensor calibration) 联立得：其中 $F_{x0} , F_{y0}, F_{z0} , M_{x0} , M_{y0} , M_{z0}, x , y , z$ 均为常数(这些值实际情况应该不是常数，毕竟力和力矩传感器数据波动还是很厉害的，这里静态标定就不考虑那么多了)，令六维力和力矩传感器静态标定(F/T sensor calibration) 即：控制机械臂末段夹持器的位姿，取 N 个不同的姿态（ N ≥ 3 ，要求至少有 3 个姿态下的机械臂末端夹持器的指向向量不共面），得到 N 组六维力传感器数据， $M = F\cdot A$ ，其中 $A = [x,y,z,k_1,k_2,k_3]^T$ 则矩阵的最小二乘解为 $A = (F^TF)^{-1}\cdot F^TM$ ,根据多维数据，可以得到负载重心centriod在六维力传感器坐标系中的坐标 $(x, y, z)$ 以及常数 $(k_1, k_2, k_3)$ 的值。矩阵的最小二乘解可以参考这个：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} )$ 已经重力的大小 $g$ ,将已经求出的负载重心坐标 $( x, y,z )$ 及常数 $(k_1,k_2,k_3)$ ，带入得：六维力和力矩传感器静态标定(F/T sensor calibration) 由此可以得到传感器的零点、负载的重量以及负载重心的坐标已经全部得出。

测试代码部分：

结合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;
}

理论部分：

测试代码部分：

您必须 登录 才能发表评论！

您必须登录才能发表评论！