背景
他関節のロボットアームの手先位置は、多数のリンクとセンサーから推定した関節角度から計算できます。
推定される手先位置にはリンク長のオフセット誤差や推定した関節角度と実関節角度とのオフセット誤差等の影響で実世界とのズレが生じてしまいます。
これらの誤差を推定する方法に非線形の最小二乗法を用いることがよくあるそうで、ROSのパッケージとして公開されているものがあります。ここではその使用方法を記載します。
- ロボットパラメータを推定するROSパッケージ
インストール方法
環境
ubuntu20.04
ros noetic
依存パッケージのインストール
install_cerese-solver.sh
sudo apt install libceres-dev
または
install_cerese-solver.sh
cd ~/
git clone https://ceres-solver.googlesource.com/ceres-solver
cd ceres-solver/
mkdir ceres-bin
cd ceres-bin
cmake ..
make -j3
make test
sudo make install
パッケージのビルド
cd ~catkin_ws/src
git clone -b ros1 https://github.com/mikeferguson/robot_calibration.git
catkin build robot_calibration
使用方法
パッケージ理解
- 最適化をしている部分 (
robot_calibration/robot_calibration/src/optimizer.cppのl370〜l381
optimizer.cpp (l.370 ~ l.381)
// Setup the actual optimization
ceres::Solver::Options options;
options.use_nonmonotonic_steps = true;
options.function_tolerance = 1e-10;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = params.max_num_iterations;
options.minimizer_progress_to_stdout = progress_to_stdout;
if (progress_to_stdout)
std::cout << "\nSolver output:" << std::endl;
summary_.reset(new ceres::Solver::Summary());
ceres::Solve(options, problem, summary_.get());
最適化関数に、パラメータ(option)、(problem)、結果(summary)を入力しています.
- 最適化クラス (
Optimizer)
robot_calibration/robot_calibration/src /optimizer.cpp より
クラスの引数: std::string robot_description
classの引数
Optimizer::Optimizer(const std::string& robot_description) :
optimize関数
int Optimizer::optimize(OptimizationParams& params,
std::vector<robot_calibration_msgs::CalibrationData> data,
bool progress_to_stdout)
{
l169_block
// Create the block
ceres::CostFunction * cost = Chain3dToChain3d::Create(models_[a_name],
models_[b_name],
offsets_.get(),
data[i]);
l196_blockの追加
problem->AddResidualBlock(cost,
NULL, // squared loss
free_params);
- cost_struct (chain3d_to_chain3d)
robot_calibration/include/robot_calibration/ceres/chain3d_to_chain3d_error.hより
l49_クラスの引数
Chain3dToChain3d(ChainModel* a_model,
ChainModel* b_model,
CalibrationOffsetParser* offsets,
robot_calibration_msgs::CalibrationData& data)
{
a_model_ = a_model;
b_model_ = b_model;
offsets_ = offsets;
data_ = data;
}
- キャリブレーションノード (
robot_calibration/robot_calibration/src /calibrate.cppより)
l74 data変数の生成
std::vector<robot_calibration_msgs::CalibrationData> data;
l88_urdf内容の取得
description_msg.data = capture_manager.getUrdf();
l175_最適化クラス,パラメータークラスの生成
robot_calibration::OptimizationParams params;
robot_calibration::Optimizer opt(description_msg.data);
l200_最適化の実行
opt.optimize(params, data, verbose);
- robot_calibration_msgs
CalibrationData.msg
# State of the robot when this data was collected
sensor_msgs/JointState joint_states
# Observations, one entry per sensor that is collecting data
Observation[] observations
Observation.msg
# Name of the "sensor" that generate this data.
string sensor_name
# Features "detected" by the sensor.
geometry_msgs/PointStamped[] features
# Sensor information
ExtendedCameraInfo ext_camera_info
# Debugging data (optional)
sensor_msgs/PointCloud2 cloud
sensor_msgs/Image image
参考
cers-solver.org
ceres solverの紹介、tutorial等
ceres solver の使い方（日本語）
最近 の ロボ ッ トキ ャ リブ レー シ ョン技 術 [石 井 優*幅][1997]
ロボットの3次元作業空間に基準となるワールド座標系を設定し,ロボットにその座標系で位置・姿勢や軌道を指定しても,与えられた目標値通りに精度よく制御することは難しい.これはロボット自体が種々の誤差を持つため,多関節ロボットの先端部にあるハンドの位置・姿勢に大きな誤差が生じるからである.ティーチングプレイバック制御におけるロボットの繰り返し再現精度は通常のロボットで0.3[mm]程度であるが,3次元作業空間における位置決め精度は20[mm]になる場合もある.
Judd[7]によれば,全体の位置決め誤差のうち,各回転
角のオフセット値による誤差が約90%,各関節の長さに
よる誤差がおよそ5%を占めている.
ロボットの位置・姿勢精度検証システムに関する研
究
cos関数の線形最小二乗法
三角関数内の角度内にパラメータがある場合、加法定理で分離して１つの変数とする.
多関節型ロボットの基本機能の計測に関する研究
精密測定における最小二乗法の使い方
2リンクアームについて
ポイント: どこの誤差を最小にしたいのか意識する
Ceres Solverを使って歪んだ“多面体”を構築する
-
Ceres Solverを使うにはoperator()が定義してあるstructを定義する必要がある
-
operator() はパラメータと残差のバッファを受け取り，その中で，受け取ったパラメータに対応する残差を格納してreturnするだけ