ロボットのパラメータ推定

# 背景

他関節のロボットアームの手先位置は、多数のリンクとセンサーから推定した関節角度から計算できます。

推定される手先位置にはリンク長のオフセット誤差や推定した関節角度と実関節角度とのオフセット誤差等の影響で実世界とのズレが生じてしまいます。

これらの誤差を推定する方法に非線形の最小二乗法を用いることがよくあるそうで、ROSのパッケージとして公開されているものがあります。ここではその使用方法を記載します。

- ロボットパラメータを推定するROSパッケージ

https://github.com/mikeferguson/robot_calibration/tree/ros1


# インストール方法

```text:環境
ubuntu20.04
ros noetic
```

依存パッケージのインストール

```bash:install_cerese-solver.sh
sudo apt install libceres-dev
```
または
```bash: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`

```cpp: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`)を入力しています.

https://github.com/mikeferguson/robot_calibration/blob/ros1/robot_calibration/src/optimizer.cpp#L370-L381

- 最適化クラス (`Optimizer`) 

`robot_calibration/robot_calibration/src
/optimizer.cpp` より

クラスの引数: `std::string robot_description`

```cpp:classの引数
Optimizer::Optimizer(const std::string& robot_description) :
```


```cpp:optimize関数
int Optimizer::optimize(OptimizationParams& params,
                        std::vector<robot_calibration_msgs::CalibrationData> data,
                        bool progress_to_stdout)
{
```

```cpp:l169_block
        // Create the block
ceres::CostFunction * cost = Chain3dToChain3d::Create(models_[a_name],
                                                              models_[b_name],
                                                              offsets_.get(),
                                                              data[i]);
```

```cpp: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より`

```cpp: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`より)

```cpp:l74 data変数の生成
std::vector<robot_calibration_msgs::CalibrationData> data;
```

```cpp:l88_urdf内容の取得
description_msg.data = capture_manager.getUrdf();
```

```cpp:l175_最適化クラス,パラメータークラスの生成
robot_calibration::OptimizationParams params;
robot_calibration::Optimizer opt(description_msg.data);
```

```cpp:l200_最適化の実行
opt.optimize(params, data, verbose);
```

- robot_calibration_msgs

```yaml: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
```

```yaml: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等

http://ceres-solver.org/index.html

### ceres solver の使い方（日本語）

https://daily-tech.hatenablog.com/entry/2019/02/26/050713

### 最近 の ロボ ッ トキ ャ リブ レー シ ョン技 術 [石 井 優*幅][1997]

```
ロボットの3次元作業空間に基準となるワールド座標系を設定し,ロボットにその座標系で位置・姿勢や軌道を指定しても,与えられた目標値通りに精度よく制御することは難しい.これはロボット自体が種々の誤差を持つため,多関節ロボットの先端部にあるハンドの位置・姿勢に大きな誤差が生じるからである.ティーチングプレイバック制御におけるロボットの繰り返し再現精度は通常のロボットで0.3[mm]程度であるが,3次元作業空間における位置決め精度は20[mm]になる場合もある.
```

```
Judd[7]によれば,全体の位置決め誤差のうち,各回転
角のオフセット値による誤差が約90%,各関節の長さに
よる誤差がおよそ5%を占めている.
```

https://www.jstage.jst.go.jp/article/jrsj1983/15/2/15_2_164/_pdf/-char/ja

### ロボットの位置・姿勢精度検証システムに関する研
究

https://kanazawa-u.repo.nii.ac.jp/?action=repository_action_common_download&item_id=37245&item_no=1&attribute_id=31&file_no=1

### cos関数の線形最小二乗法

三角関数内の角度内にパラメータがある場合、加法定理で分離して１つの変数とする.

![Screenshot from 2023-05-12 18-18-18.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/589906/a0b1ec93-7296-c6f3-e473-d4bf13442aad.png)

https://buaiso.blogspot.com/2012/08/cos.html

### 多関節型ロボットの基本機能の計測に関する研究

https://www.jstage.jst.go.jp/article/qes/5/6/5_30/_pdf/-char/ja

### 精密測定における最小二乗法の使い方

2リンクアームについて
ポイント: どこの誤差を最小にしたいのか意識する

![Screenshot from 2023-05-16 10-51-05.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/589906/7948ca16-73b0-5c0a-f9ec-3131b0b42988.png)

![Screenshot from 2023-05-16 10-51-22.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/589906/834d8156-b449-8a3d-43b7-bbe28878a9ac.png)


https://www.jstage.jst.go.jp/article/jjspe/76/10/76_10_1130/_pdf

### Ceres Solverを使って歪んだ“多面体”を構築する


- Ceres Solverを使うにはoperator()が定義してあるstructを定義する必要がある

- operator() はパラメータと残差のバッファを受け取り，その中で，受け取ったパラメータに対応する残差を格納してreturnするだけ

https://qiita.com/eduidl/items/355465c95d479d48cfa0