Dobot CR3の座標値からジョイント角を求める方法

by t_tsuzuki
イズミビジョンラボ合同会社 > 技術資料 > Dobot CR3の座標値からジョイント角を求める方法

数値的解法によりエンドエフェクタのターゲット座標値におけるジョイント角度を求めます。
目標値をして入力する値はエンドエフェクタのXYZ座標値３つと姿勢を表す四元数（w, x, y, z)の４つとなり、疑似逆行行列の計算により最小二乗法による最適値が得られます。

ソースコード

本コードではFK（順運動学）も使用するため＜Dobot CR3の座標算出について＞で作成したコードも利用します。

IK演算で使用する関数の作成
#pragma once
#include <Eigen/Dense>
#include "ComputeFK.h"　//FK関数を記述

//座標値（オイラー角）から四元数を計算します
Eigen::Quaterniond GetQuaternion(Coordinate coord)
{
	auto cx = std::cos(0.5f * coord.rx / 180 * PI);
	auto sx = std::sin(0.5f * coord.rx / 180 * PI);
	auto cy = std::cos(0.5f * coord.ry / 180 * PI);
	auto sy = std::sin(0.5f * coord.ry / 180 * PI);
	auto cz = std::cos(0.5f * coord.rz / 180 * PI);
	auto sz = std::sin(0.5f * coord.rz / 180 * PI);

	Eigen::Quaterniond q = Eigen::Quaterniond(sx * sy * sz + cx * cy * cz,
		sx * cy * cz - cx * sy * sz,
		sx * cy * sz + cx * sy * cz,
		-sx * sy * cz + cx * cy * sz);
  
  w>=0となるようにw<0の場合は全成分に-1をかけます
	if (q.w() < 0) {
		q.x() = -q.x();
		q.y() = -q.y();
		q.z() = -q.z();
		q.w() = -q.w();
	}
	return q;
}

//２つの座標値がトレランス内かどうかを返します
bool ToleranceCheck(Coordinate coord1, Coordinate coord2, double tolerance)
{
	if (std::abs(coord1.x - coord2.x) > tolerance)
		return false;

	if (std::abs(coord1.y - coord2.y) > tolerance)
		return false;
	if (std::abs(coord1.z - coord2.z) > tolerance)
		return false;

	Eigen::Quaterniond quat0 = GetQuaternion(coord1);
	Eigen::Quaterniond quat1 = GetQuaternion(coord2);

	if (!quat0.isApprox(quat1, tolerance))
		return false;

	return true;
}

//2つの４元数の差を抽出します。（符号を絶対値が最大成分で合わせます）
Eigen::Vector4d GetDifferenceValue(Eigen::Quaterniond base, Eigen::Quaterniond diff)
{
	Eigen::Vector4d vec;

	double absx = std::abs(base.x());
	double absy = std::abs(base.y());
	double absz = std::abs(base.z());
	double absw = std::abs(base.z());

	if (std::max(absx, absy) >= std::max(absz, absw))
	{
		if (absx > absy)
		{
			if (base.x() * diff.x() < 0)
			{
				diff.x() *= -1;
				diff.y() *= -1;
				diff.z() *= -1;
				diff.w() *= -1;
			}
		}
		else
		{
			if (base.y() * diff.y() < 0)
			{
				diff.x() *= -1;
				diff.y() *= -1;
				diff.z() *= -1;
				diff.w() *= -1;
			}
		}
	}
	else {
		if (absz > absw)
		{
			if (base.z() * diff.z() < 0)
			{
				diff.x() *= -1;
				diff.y() *= -1;
				diff.z() *= -1;
				diff.w() *= -1;
			}
		}
		else
		{
			if (base.w() * diff.w() < 0)
			{
				diff.x() *= -1;
				diff.y() *= -1;
				diff.z() *= -1;
				diff.w() *= -1;
			}
		}
	}
	vec.x() = diff.x() - base.x();
	vec.y() = diff.y() - base.y();
	vec.z() = diff.z() - base.z();
	vec.w() = diff.w() - base.w();

	return vec;
}

//２つの座標値の一致度の評価値を返します。（０に近い方が一致度が高い）
double MatchingScore(Coordinate target, Coordinate current)
{
　//それぞれの差分の2乗を計算して総和を返します
	double val1 = std::pow(target.x - current.x, 2);
	double val2 = std::pow(target.y - current.y, 2);
	double val3 = std::pow(target.z - current.z, 2);

	Eigen::Quaterniond q1 = GetQuaternion(target);
	Eigen::Quaterniond q2 = GetQuaternion(current);

	Eigen::Vector4d vec = GetDifferenceValue(q1, q2);

	double val4 = std::pow(vec.x(), 2);
	double val5 = std::pow(vec.y(), 2);
	double val6 = std::pow(vec.z(), 2);
	double val7 = std::pow(vec.w(), 2);

	return val1 + val2 + val3 + val4 + val5 + val6 + val7;
}
IK演算関数

特異点回避の為ニュートン・ラプソン法における偏微分値を計算で求めるのではなく、微小変化量＋d、-d変化したときのエンドエフェクタ座標値の内、目標座標値に近づく方の微小変化量から偏微分値を求めています。

//currentJValues 現在のジョイント角度
//targetCoordinate 目標のデカルト座標値
//dhParameters ジョイント毎のDHパラメーター
//targetJValues　ターゲット位置でのジョイント角度
//tolerance 演算完了時の目標値と演算値の許容値
//calTimes 数値解法実施時の最大繰り返し回数
bool ComputeIK(JointValues currentJValues, Coordinate targetCoordinate, 
std::array<DHParameter, 6>dhParameters,　JointValues& targetJValues, 
double tolerance, double calTimes)
{
	//現在の座標を計算(x,y,z,rx,ry,rz)
	Coordinate curCoordinate;
	ComputeFK(currentJValues, dhParameters, curCoordinate);
　Eigen::Quaterniond quatTar = GetQuaternion(targetCoordinate);

	if (!ToleranceCheck(targetCoordinate, curCoordinate, tolerance))
	{
		//ヤコビ行列の演算
		Eigen::Matrix<double, 7, 6> Jacob;
　　//求めたtargetCoordinateが許容値内になるまで繰り返し
		for (int i = 0; i < calTimes; i++) {

			Eigen::Quaterniond quatCur = GetQuaternion(curCoordinate);
			//各要素の目標値と現在値の差分
　    Eigen::Vector<double, 7> coord_dif;
			coord_dif(0) = targetCoordinate.x - curCoordinate.x;
			coord_dif(1) = targetCoordinate.y - curCoordinate.y;
			coord_dif(2) = targetCoordinate.z - curCoordinate.z;
　
			Eigen::Vector4d d0 = GetDifferenceValue(quatCur, quatTar);
			coord_dif(3) = (d0.x());
			coord_dif(4) = (d0.y());
			coord_dif(5) = (d0.z());
			coord_dif(6) = (d0.w());
      
 　　 //偏微分値を求めるための変化量
			const double offsetVal = 0.01;

			for (int j = 0; j < 6; j++)
			{
　　　　 //各ジョイント角を＋側に微小変化させたときの角度
				JointValues plusOffsetJVal;
        //各ジョイント角を＋側に微小変化させたときの角度
				JointValues minusOffsetJVal;
				if (j == 0) //J1
				{
					plusOffsetJVal = JointValues(currentJValues.J1 + offsetVal, currentJValues.J2,
            currentJValues.J3, currentJValues.J4, currentJValues.J5, currentJValues.J6);

					minusOffsetJVal = JointValues(currentJValues.J1 - offsetVal, currentJValues.J2,
            currentJValues.J3, currentJValues.J4, currentJValues.J5, currentJValues.J6);
				}
				else if (j == 1) //J2
				{
					plusOffsetJVal = JointValues(currentJValues.J1, currentJValues.J2 + offsetVal,
            currentJValues.J3, currentJValues.J4, currentJValues.J5, currentJValues.J6);

					minusOffsetJVal = JointValues(currentJValues.J1, currentJValues.J2 - offsetVal, 
            currentJValues.J3, currentJValues.J4, currentJValues.J5, currentJValues.J6);
				}
				else if (j == 2) //J3
				{
					plusOffsetJVal = JointValues(currentJValues.J1, currentJValues.J2, 
            currentJValues.J3 + offsetVal, currentJValues.J4, currentJValues.J5,
            currentJValues.J6);

					minusOffsetJVal = JointValues(currentJValues.J1, currentJValues.J2,
            currentJValues.J3 - offsetVal, currentJValues.J4, currentJValues.J5,
            currentJValues.J6);
				}
				else if (j == 3) //J4
				{
					plusOffsetJVal = JointValues(currentJValues.J1, currentJValues.J2, 
            currentJValues.J3, currentJValues.J4 + offsetVal, currentJValues.J5,
            currentJValues.J6);

					minusOffsetJVal = JointValues(currentJValues.J1, currentJValues.J2,
            currentJValues.J3, currentJValues.J4 - offsetVal, currentJValues.J5,
            currentJValues.J6);
				}
				else if (j == 4) //J5
				{
					plusOffsetJVal = JointValues(currentJValues.J1, currentJValues.J2,
            currentJValues.J3, currentJValues.J4, currentJValues.J5 + offsetVal,
            currentJValues.J6);

					minusOffsetJVal = JointValues(currentJValues.J1, currentJValues.J2,
            currentJValues.J3, currentJValues.J4, currentJValues.J5 - offsetVal,
            currentJValues.J6);
				}
				else if (j == 5) //J6
				{
					plusOffsetJVal = JointValues(currentJValues.J1, currentJValues.J2,
            currentJValues.J3, currentJValues.J4, currentJValues.J5,
            currentJValues.J6 + offsetVal);

					minusOffsetJVal = JointValues(currentJValues.J1, currentJValues.J2,
            currentJValues.J3, currentJValues.J4, currentJValues.J5, 
            currentJValues.J6 - offsetVal);
				}

        //各ジョイント角を＋側に微小変化させたときの座標値
				Coordinate plusOffsetCoord;
        //各ジョイント角をー側に微小変化させたときの座標値
				Coordinate minusOffsetCoord;

				ComputeFK(plusOffsetJVal, dhParameters, plusOffsetCoord);
				ComputeFK(minusOffsetJVal, dhParameters, minusOffsetCoord);
        //各ジョイント角を＋側に微小変化させたときの四元数
				Eigen::Quaterniond qPlus = GetQuaternion(plusOffsetCoord);
        //各ジョイント角をー側に微小変化させたときの四元数
				Eigen::Quaterniond qMinus = GetQuaternion(minusOffsetCoord);

　　　　 //+方向か－方向かどちらが目標値に近づくか判定し近い方をヤコビ行列の成分に代入
				if (MatchingScore(targetCoordinate, plusOffsetCoord) 
             < MatchingScore(targetCoordinate, minusOffsetCoord)) {

					Jacob(0, j) = (plusOffsetCoord.x - curCoordinate.x) / offsetVal;
					Jacob(1, j) = (plusOffsetCoord.y - curCoordinate.y) / offsetVal;
					Jacob(2, j) = (plusOffsetCoord.z - curCoordinate.z) / offsetVal;

					Eigen::Vector4d d = GetDifferenceValue(quatCur, qPlus);

					Jacob(3, j) = (d.x()) / offsetVal;
					Jacob(4, j) = (d.y()) / offsetVal;
					Jacob(5, j) = (d.z()) / offsetVal;
					Jacob(6, j) = (d.w()) / offsetVal;
				}
				else {

					Jacob(0, j) = (minusOffsetCoord.x - curCoordinate.x) / -offsetVal;
					Jacob(1, j) = (minusOffsetCoord.y - curCoordinate.y) / -offsetVal;
					Jacob(2, j) = (minusOffsetCoord.z - curCoordinate.z) / -offsetVal;

					Eigen::Vector4d d = GetDifferenceValue(quatCur, qMinus);

					Jacob(3, j) = (d.x()) / -offsetVal;
					Jacob(4, j) = (d.y()) / -offsetVal;
					Jacob(5, j) = (d.z()) / -offsetVal;
					Jacob(6, j) = (d.w()) / -offsetVal;
				}
			}
      //転置行列を作成
			Eigen::Matrix<double, 6, 7> JT = Jacob.transpose();
      //疑似逆行行列を求める
			Eigen::MatrixXd JPlus = ((JT * Jacob).inverse()) * JT;
　　  //最小二乗によって得られた角度変化量に補正値（今回は0.9）をかける
			Eigen::Vector<double, 6> diffs = 0.9 * JPlus * coord_dif;

			
      JointValues tmpJValues;
			Coordinate tmpCoordinate;
　　　 //ジョイント角度のリミット値を反映させる
			tmpJValues.J1 = currentJValues.J1 + diffs[0];
			if (tmpJValues.J1 > 357)
				tmpJValues.J1 = 357;

			if (tmpJValues.J1 < -357)
				tmpJValues.J1 = -357;

			tmpJValues.J2 = currentJValues.J2 + diffs[1];
			if (tmpJValues.J2 > 178)
				tmpJValues.J2 = 178;

			if (tmpJValues.J2 < -155)
				tmpJValues.J2 = -155;

			tmpJValues.J3 = currentJValues.J3 + diffs[2];
			if (tmpJValues.J3 > 178)
				tmpJValues.J3 = 178;

			if (tmpJValues.J3 < -178)
				tmpJValues.J3 = -178;

			tmpJValues.J4 = currentJValues.J4 + diffs[3];
			if (tmpJValues.J4 > 178)
				tmpJValues.J4 = 178;

			if (tmpJValues.J4 < -178)
				tmpJValues.J4 = -178;

			tmpJValues.J5 = currentJValues.J5 + diffs[4];
			if (tmpJValues.J5 > 178)
				tmpJValues.J5 = 178;

			if (tmpJValues.J5 < -178)
				tmpJValues.J5 = -178;

			tmpJValues.J6 = currentJValues.J6 + diffs[5];
			if (tmpJValues.J6 > 357)
				tmpJValues.J6 = 357;

			if (tmpJValues.J6 < -357)
				tmpJValues.J6 = -357;

			//一度回転行列に変換後、再度オイラー角にする
			ComputeFK(tmpJValues, dhParameters, tmpCoordinate);
      //求めた座標値と目標値の差分が許容値内か確認
			if (ToleranceCheck(targetCoordinate, tmpCoordinate, tolerance))
			{
　　　　 //許容値内の場合trueを返して終了
				targetJValues.J1 = tmpJValues.J1;
				targetJValues.J2 = tmpJValues.J2;
				targetJValues.J3 = tmpJValues.J3;
				targetJValues.J4 = tmpJValues.J4;
				targetJValues.J5 = tmpJValues.J5;
				targetJValues.J6 = tmpJValues.J6;
				return true;
			}
			else
			{
　　　　 //許容値外の場合演算した座標値を現在値に更新して再演算
				currentJValues.J1 = tmpJValues.J1;
				currentJValues.J2 = tmpJValues.J2;
				currentJValues.J3 = tmpJValues.J3;
				currentJValues.J4 = tmpJValues.J4;
				currentJValues.J5 = tmpJValues.J5;
				currentJValues.J6 = tmpJValues.J6;

				curCoordinate.x = tmpCoordinate.x;
				curCoordinate.y = tmpCoordinate.y;
				curCoordinate.z = tmpCoordinate.z;
				curCoordinate.rx = tmpCoordinate.rx;
				curCoordinate.ry = tmpCoordinate.ry;
				curCoordinate.rz = tmpCoordinate.rz;
			}
		}
    //演算回数内で終了しなかった場合falseを返す
		targetJValues.J1 = currentJValues.J1;
		targetJValues.J2 = currentJValues.J2;
		targetJValues.J3 = currentJValues.J3;
		targetJValues.J4 = currentJValues.J4;
		targetJValues.J5 = currentJValues.J5;
		targetJValues.J6 = currentJValues.J6;

		return false;
	}
  else{
	  targetJValues.J1 = currentJValues.J1;
	  targetJValues.J2 = currentJValues.J2;
	  targetJValues.J3 = currentJValues.J3;
	  targetJValues.J4 = currentJValues.J4;
	  targetJValues.J5 = currentJValues.J5;
	  targetJValues.J6 = currentJValues.J6;

	  return true;
  }
}
実行関数
#include <iostream>
#include <format>
#include "ComputeFK.h"

int main()
{
	std::array<DHParameter, 6> dhParameters{};

	//DHパラメーターを入力
	dhParameters[0].Set(0, 0, 0, 134.8);
	dhParameters[1].Set(90, 0, -90, 0);
	dhParameters[2].Set(0, -274.541, 0, 0);
	dhParameters[3].Set(0, -230.216, -90, 128.356);
	dhParameters[4].Set(90, 0, 0, 116.113);
	dhParameters[5].Set(-90, 0, 0, 105);

	JointValues currentJoint{};
	currentJoint.Set(10, 30, 90, -20, -85, 10);
	Coordinate coord{};
	ComputeFK(currentJoint, dhParameters, coord);
	std::cout << "**Current**" << std::endl;
	std::cout << "J1:" << std::format("{:f}", currentJoint.J1) << std::endl;
	std::cout << "J2:" << std::format("{:f}", currentJoint.J2) << std::endl;
	std::cout << "J3:" << std::format("{:f}", currentJoint.J3) << std::endl;
	std::cout << "J4:" << std::format("{:f}", currentJoint.J4) << std::endl;
	std::cout << "J5:" << std::format("{:f}", currentJoint.J5) << std::endl;
	std::cout << "J6:" << std::format("{:f}", currentJoint.J6) << std::endl;
	std::cout << "X:" << std::format("{:f}", coord.x) << std::endl;
	std::cout << "Y:" << std::format("{:f}", coord.y) << std::endl;
	std::cout << "Z:" << std::format("{:f}", coord.z) << std::endl;
	std::cout << "Rx:" << std::format("{:f}", coord.rx) << std::endl;
	std::cout << "Ry:" << std::format("{:f}", coord.ry) << std::endl;
	std::cout << "Rz:" << std::format("{:f}", coord.rz) << std::endl;

	Coordinate targetCoord{};
  //目標の座標値
	targetCoord.Set(-400, 100, 150, 175, 10, 150);
	JointValues targetJoint{};
	if (!ComputeIK(currentJoint, targetCoord, dhParameters, targetJoint, 0.001, 100)) 
	{
		std::cout << "IKCalError!" << std::endl;
	}
	ComputeFK(targetJoint, dhParameters, targetCoord);
	std::cout << "**Target**" << std::endl;
	std::cout << "J1:" << std::format("{:f}", targetJoint.J1) << std::endl;
	std::cout << "J2:" << std::format("{:f}", targetJoint.J2) << std::endl;
	std::cout << "J3:" << std::format("{:f}", targetJoint.J3) << std::endl;
	std::cout << "J4:" << std::format("{:f}", targetJoint.J4) << std::endl;
	std::cout << "J5:" << std::format("{:f}", targetJoint.J5) << std::endl;
	std::cout << "J6:" << std::format("{:f}", targetJoint.J6) << std::endl;
	std::cout << "X:" << std::format("{:f}", targetCoord.x) << std::endl;
	std::cout << "Y:" << std::format("{:f}", targetCoord.y) << std::endl;
	std::cout << "Z:" << std::format("{:f}", targetCoord.z) << std::endl;
	std::cout << "Rx:" << std::format("{:f}", targetCoord.rx) << std::endl;
	std::cout << "Ry:" << std::format("{:f}", targetCoord.ry) << std::endl;
	std::cout << "Rz:" << std::format("{:f}", targetCoord.rz) << std::endl;

}

実行結果

**Current**
J1:10.000000
J2:30.000000
J3:90.000000
J4:-20.000000
J5:-85.000000
J6:10.000000
X:-402.375169
Y:-210.578224
Z:134.277342
Rx:169.269479
Ry:3.116937
Rz:89.269479
**Target**
J1:-30.804278
J2:21.440212
J3:99.194303
J4:-20.704780
J5:-95.139184
J6:-90.795246
X:-399.999777
Y:99.999935
Z:149.999914
Rx:174.999992
Ry:10.000008
Rz:149.999990

ロボットのデカルト座標値算出方法