RobWork框架编程（2）：使用雅可比数值迭代算法对机器人进行逆运动学求解

在上一篇博客文章中，我们介绍了如何创建一个使用RobWork机器人框架的工程，以及如何加载机器人设备文件。
接下来，在本篇博客中，我们将讨论如何使用RobWork求解UR3机器人的运动学逆解，使用的方法是通用的雅可比数值迭代方法，数值迭代算法虽然没有封闭解（也叫解析解）算法计算的结果精确，求解的速度也比封闭解算法慢不少，并且在有解的情况下也可能会求解失败，但是雅可比数值迭代算法的优势也非常的明显，比如：通用，我只需要正向运动学的DH参数就可以进行求逆操作，封闭解算法比较理想，但是真实情况是由于制造和装配误差的存在，基于机器人厂商给定的设计图纸的尺寸建立的正向运动学是不准确的，就我所接触的一些品牌的机器人来说，国产机器人的制造精度貌似还不错，基本满足使用要求，此时可以使用封闭解算法来求逆。相反，知名度很大的UR机器人精度就差别很大，它的精度维持主要靠出厂时标定的DH补偿数据，UR机器人出厂之前都会进行运动学参数标定，标定得到的补偿数据会存放在一个叫calibration.conf文件中，并将该文件存放在与机械臂配套的机器人控制器里，用户可以通过ssh远程登录访问控制器来获取UR机器人的标定补偿数据。不同公司的策略不一样，UR机器人公司的这种简单粗暴的方式从另外一种角度来看可能也挺好的。你要用这些数据的时候直接从这里拿即可，具体的操作如下：

liuqiang@liuqiang-Vulcan-JINGANG-GTSeries:~$ssh root@192.168.1.111 #登录机器人控制器
root@192.168.1.111's password:  #需要输入密码，默认的密码是easybot
Linux ur-2017332604 3.14.23-rt20URKernel #1 SMP PREEMPT RT Fri Mar 31 20:33:18 CEST 2017 i686

The programs included with the Debian GNU/Linux system are free software;
the exact distribution terms for each program are described in the
individual files in /usr/share/doc/*/copyright.

Debian GNU/Linux comes with ABSOLUTELY NO WARRANTY, to the extent
permitted by applicable law.
root@ur-2017332604:~# cat ./.urcontrol/calibration.conf #这条命令就是打印标定的DH补偿数据
[mounting]
delta_theta = [ -5.05343367106411137e-05, -0.0561242156763783057, -0.0628157016125706208, 0.118814438034714087, 5.68822754944051158e-05, -1.45429512938645578e-05]
delta_a = [ 1.85427703966640276e-05, 0.000418280909411400392, 0.001565903694771692, -9.66540899511680791e-06, -2.93410449126249217e-05, 0]
delta_d = [ -5.56630992100959343e-05, 8.16504944596539062, -14.4282499666997612, 6.26317959859598705, -3.9946104585394937e-05, 0.000367523159254548593]
delta_alpha = [ -0.000626175392884231741, -0.00167062908967277168, 0.00403522262881641277, -0.000368945620575544808, 0.000173987342825920877, 0]
joint_checksum = [ 0xe9245b66, 0x3d2f29a6, 0x840da431, 0x5f70ae22, 0x6e93f19, 0xcee31fb0]
calibration_status = 2 # 0 == notInitialized / 1 == notLinearised / 2 == Linearised
joint_raw_offset = [ 0x1d0897, 0x5d8476, 0x1aec25, 0x9cf1b9, 0x9bec11, 0x1da4e0]
joint_selftest_data_crc = [ 0xddf34bf6, 0xa7727e85, 0xe09cd766, 0xb70c73b2, 0xd9e9302e, 0x5ec3a46c]
root@ur-2017332604:~# exit #退出登录
logout
Connection to 192.168.1.111 closed.

通常我们只需要delta_theta， delta_a， delta_d， delta_alpha的数据，将这些补偿数据跟标准的DH参数表中的数据进行叠加，然后计算正向运动学，你会发现，结合DH补偿参数后，正向运动学的结果跟从控制器上读到的TCP数据（工具中心点位姿）是基本一致的（在机器人官方宣称的精度范围内）。

你可能会注意到，叠加了DH补偿参数之后，之前基于标准的DH参数推导的封闭逆解算法现在无法发挥作用了。此时，你会发现基于雅可比数值迭代的逆解算法派上了用场。

这就是为什么基于雅可比的数值迭代算法有那么多缺点，但是在真实的项目中用的还很普遍，这也是我写这篇博客的原因。
因为雅可比数值迭代逆解算法虽然简单粗暴，但是很实用。

CMakeLists.txt

cmake_minimum_required(VERSION 3.5)

project(ur_ik_solver LANGUAGES CXX)

set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(RobWork_DIR "/home/liuqiang/SourceCode/RobWork/RobWork/cmake")
find_package(RobWork)
include_directories(${ROBWORK_INCLUDE_DIRS})
add_executable(ur_ik_solver main.cpp)
target_link_libraries(ur_ik_solver ${ROBWORK_LIBRARIES})

main.cpp

#include <iostream>
#include <rw/common/Ptr.hpp>
#include <rw/loaders/WorkCellLoader.hpp>
#include <rw/models/WorkCell.hpp>
#include <rw/models/Device.hpp>
#include <rw/kinematics/State.hpp>
#include <rw/invkin/JacobianIKSolver.hpp>
#include <rw/math.hpp>
#include <ctime>

using namespace rw::common;
using namespace rw::loaders;
using namespace rw::models;
using namespace rw::invkin;
using namespace rw::kinematics;
using namespace rw::math;

int main() {
    WorkCell::Ptr workcell =  WorkCellLoader::Factory::load("/home/liuqiang/UR3_2015/UR3.xml");
    State state = workcell->getDefaultState();
    Device::Ptr device = workcell->findDevice("UR3_2015");
    JacobianIKSolver::Ptr solver = ownedPtr(new JacobianIKSolver(device, state));
    solver->setSolverType(JacobianIKSolver::SVD);
    solver->setMaxIterations(50); //如果不设置，默认是迭代20次
    Q q1(6, 1.57, -2.4, 2.4, -1.57, -1.57, 0);
    device->setQ(q1, state);
    std::cout << device->getQ(state) << "\n";
    Transform3D<> tcp_pose = device->baseTend(state);
    std::cout << tcp_pose << "\n";
    Q q2(6, 1.5, -2., 2., -1.5, -1.5, 0.5);
    device->setQ(q2, state);
    std::cout << device->getQ(state) << "\n";
    Transform3D<> tcp_pose2 = device->baseTend(state);
    std::cout << tcp_pose2 << "\n";
    device->setQ(q1, state);
    std::cout << device->getQ(state) << "\n";
    clock_t start_time = clock();
    for (int i = 0; i < 10000; ++i) {
        auto result = solver->solve(tcp_pose2, state);
        std::cout << result.size() << "\n";
        std::cout << result[0] << "\n";
    }
    clock_t end_time = clock();
    std::cout << "use: " << (static_cast<double>(end_time - start_time) / CLOCKS_PER_SEC) / 10000 << " s\n";
    return 0;
}

在RobWork机器人框架中，Device设备对象本身是不存储状态信息的（当然也不是什么数据都不存储），即Stateless无状态的设备，设备的状态信息存储在一个单独的State对象中，因此涉及操作状态数据时，就需要传入单独维护的State对象。
基于RobWork的数值迭代解在耗时上确实比封闭解长不少，在上述的10000次雅可比数值迭代结果的平均值是每次迭代耗时use: 0.00144038 s，即约1.5 ms
而我之前使用单纯的封闭解算法计算得出的耗时为0.0000608s，即约0.061ms，简单计算得出雅可比数值迭代一次的耗时是封闭解算法的23.69倍。

结论：能用封闭解算法的情况下使用封闭解当然是最好的了，如果现实条件不允许，那么使用数值迭代法不失为一种良策。