４輪独立ステアリング移動機構の開発記録
はじめに
ロボコン関係で見る方が多いと思うので、一応先に個人的な結論ですが、ほとんどの場合”オムニで良い”です。
大抵の場合、敗因は足回りにはありません。足の差で負けて初めて、開発を始めればいいです。
コスパが悪すぎるので、余裕がないなら作るべきではありません。
じゃあなぜ作ったのか？
ロマンがあったからです。機体設計やってたら、一度は作ってみたいと思いますよね。それだけです。
単純に弊サークルの技術力向上とアピール点を増やしたいという目的もありました。
ただ、一応実用的な側面はあって、僕は関東夏2023,学ロボ2024とオムニを使っていましたが、樽をキレイにしておかないと滑るし、消耗するし、振動すごいし、諸々の状態によって移動性能が変化したり、速度を出しすぎると直進性が悪くなったり、モーター角度制御による移動精度も大なり小なり滑ってしまってダメダメで、開発難易度は低くて扱いやすいですが、問題点も多いと感じる移動機構でした。
そんな悩みをおおよそ解決してしまうのが独立ステアリング機構で、理想的な独ステは足回りの理論値であるように思えたため、作ってみました。
ほんだい
前置きが長くなってしました。とにかく書いていきます。
設計
初代独ステ
最初にノリで設計した、部室にある余り物で設計した独ステ。
インホイールにすることでホイール用モータを下側に配置し、コンパクトかつ低重心な設計にしました。
これがすべての始まり。
一度設計してみると、作ってみたくなって、作ってしまうと改善点が見つかって、二代目をつくってみたくなってしまいますよね。
たとえばこれは、ステア速度や保守性、3dpギアの脆弱性に問題が有りました。単純に見た目も好きじゃない。
二代目独ステ
ということで、作った二代目の独ステ。
インホイールの思想を受け継ぎつつ、直結でいけて、エンコーダも付いているロボマスを使うことで、部品点数を極力減らし、諸々の速度を計算、3dpで簡単に作れて、部室に転がってる部品で作れるように設計した。
設計してみると（以下略
工房に依頼したデカアルミ板が全く出来上がって来ないので仕方なく3dpで作った。
後輩が手動でも使えるように、ステア速度はできるだけ早くして応答性をあげたい。
摺動にガタがある点、ギアのかみ合わせも改善するのと、3dpだけでも強度出るんじゃね？ってことで三代目の設計を始める。
もうここまでくると止まらない。
三代目独ステ
ということで三代目。
M6三本でユニットを取り外せるメンテナンス性と、諸々の改善、3dp軸でも体重を耐えれるような設計になっていて、ギアを露出させるという個人開発でしか許されない性癖設計も発露。
他にも、有限回転の制御がめんどくさい＋ホトインタラプタの設置と回路的に煩わしいとかで、配線ユニットに収めて無限回転にしたり、リミットスイッチでホーミングしたり、ストライカにロゴ入れたり、好きなように改造を重ね、
完成（上半身は大した機構じゃないけど別記事にする）
設計から試運転含め諸々が完成するのに1か月ちょいでした。
試運転
（※勘違いしており5倍も出ない）
制御
トップダウンで解説していきます。
まず、回路構成がこんな感じ。
電源基盤の24V電流容量は最終的に120Aになってます
上図の通り、制御基板には角度、速度指令しか送らないので、運動学計算はROS2ノードで行いました。
最終的な計算部分を抜粋します。移動指令vx,vy,vwを受け、各ホイールの指令値を計算してます。
posはロボットの座標系から見た各ホイールのx座標、y座標が格納されています。
ここで、自分がロボット座標系をロボット正面がyに向くように設定していた関係で、atanのx,yを逆にしており注意が必要です。
移動量が0の時にいちいち原点に戻らないようにしていたり、無駄なステア回転をなくすために90度以上回転が必要な場合はホイールの回転方向を変えて、最短のステア角になるようにしています。
std::array<WheelCommand, KinematicsController::NUM_WHEELS>
KinematicsController::calculateWheelCommands(float vx, float vy, float wz)
{
std::array<WheelCommand, NUM_WHEELS> wheel_commands;
for (size_t i = 0; i < NUM_WHEELS; ++i) {
const auto& pos = wheel_positions_[i];
const float wheel_vx = vx - wz * pos.y;
const float wheel_vy = vy + wz * pos.x;
wheel_commands[i].speed = sqrtf(wheel_vx * wheel_vx + wheel_vy * wheel_vy);
wheel_commands[i].angle = atan2f(wheel_vx, wheel_vy);
if (wheel_commands[i].speed < 1e-4f) {
wheel_commands[i].speed = 0.0f;
wheel_commands[i].angle = prev_theta_[i];
}
else {
if ( fabs(wheel_commands[i].angle - prev_theta_[i]) > PI_F / 2.0f ) {
wheel_commands[i].angle += (wheel_commands[i].angle > prev_theta_[i]) ? -PI_F : PI_F;
wheel_commands[i].speed = -wheel_commands[i].speed;
}
prev_theta_[i] = wheel_commands[i].angle;
}
}
return wheel_commands;
}
このspeed、angleを以下のCANプロトコルに沿って各モーターへ向け送信します。
CANプロトコル
CAN ID = (board_id << 4) | motor_id
| ビット構成 | 説明 |
|---|---|
| [10:4] | board_id (1〜15) |
| [3:0] | motor_id (1〜8) |
例：
board_id = 3, motor_id = 2 の場合
→ CAN ID = (3 << 4) | 2 = 0x32
🚀 Command Frame: motor_command
方向: PC → Teensy
長さ: 8 bytes
| Byte | フィールド名 | 型 | 説明 | 備考 |
|---|---|---|---|---|
| 0 | mode | uint8_t | 制御モード | 0x00: Disable / 0x01: Speed / 0x02: BoundedAngle / 0x03: UnboundedAngle / 0xFF: Homing |
| 1 | opt | uint8_t | オプション | 現状 0 固定 |
| 2–5 | value | int32_t (little-endian) | 指令値（1000倍スケール） | Speedモード時: rad/s ×1000 / Angleモード時: rad ×1000 |
| 6 | seq | uint8_t | シーケンス番号 | 0〜255で循環 |
| 7 | timeout | uint8_t | タイムアウト時間 | 単位=10ms（0→100msデフォルト） |
次に、これらを受け取ったteensyが各モーターに対し角度制御、速度制御を行っていきます。
ここで、角度制御を行う際、カスケード型制御というのを行っています。
http://www.miyazaki-gijutsu.com/series/control633.html
それっぽい名前になっていますが、単に角度PIDに速度PIDを挟み、制御周期を調整しているだけです。
以下にコード例を示します。以下のコードでは自作のDJIモーター用ライブラリ（motors）と、PID計算ライブラリを使っていますが、これは別記事で解説します。
struct PidParam {
float kp, ki, kd;
float outMin, outMax;
uint32_t sampleMs;
};
constexpr PidParam AnglePidParam{30, 0, 0, -22.0, 22.0, 10};
constexpr PidParam SpeedPidParam{1500, 0, 0, -8000, 8000, 1};
inline Pid makePID(const PidParam &p) {
return Pid(p.kp, p.ki, p.kd, p.outMin, p.outMax, p.sampleMs);
}
Pid AnglePid[4] = { makePID(AnglePidParam), makePID(AnglePidParam), makePID(AnglePidParam), makePID(AnglePidParam)};
Pid SpeedPid[4] = { makePID(SpeedPidParam), makePID(SpeedPidParam), makePID(SpeedPidParam), makePID(SpeedPidParam)};
inline void angleControl(size_t idx, float targetAngle, bool bounded, DjiMotorCan<CAN2> &motors) {
const auto &fb = motors.feedback(idx + 1);
const float fbAngle = fb.getAngleRadiansWrapped();
const float fbSpeed = fb.getSpeedRadiansPerSec();
if(bounded) {
targetAngle = constrain(targetAngle, -PI, PI);
}
else {
if(targetAngle - fbAngle > PI) {
targetAngle -= DjiConstants::PI_2;
} else if(targetAngle - fbAngle < -PI) {
targetAngle += DjiConstants::PI_2;
}
}
int16_t angleCmd = AnglePid[idx].compute(fbAngle, targetAngle);
int16_t speedCmd = SpeedPid[idx].compute(fbSpeed, angleCmd);
motors.sendCurrent(idx + 1, speedCmd);
}
要するに、単に角度PIDの出力を角速度になるようスケール設定し、制御周期を速度PIDよりも遅くしているだけです（角度PIDは10ms間隔、速度PIDは1ms間隔）。
これだけで、Pパラメータを雑に入れただけでかなりの精度と応答性が実現でき、位置制御を行いたい場合はとても有用かと思います。
ところがどっこい、ステア角がドリフトしてしまう問題にあたりました。
以下、その原因と対処を示します。
CANフレームロスト
DJIモーターはデフォルトのフィードバック周波数が1kHzになっているため、1ms間隔で情報が送られてきます。
さらに、内臓ロータリーエンコーダーが”減速前の”角度を0~8191で返してくれます。
ここで、簡単な角度制御をする場合、この角度の変化量を足し算していけばいいわけですが、8191から0に戻るとき、つまり一回転するときは単に変化量を足すとおかしくなっちゃうので、これを検知するためには1ステップで半回転以上したら、それは一回転していると判断して処理するのが一般的かと思います。
こんな感じ。
const int16_t delta_raw = Angle - lastAngle;
int16_t delta;
if (delta_raw >= -HALF_ENCODER_CPR && delta_raw <= HALF_ENCODER_CPR) {
delta = delta_raw;
} else {
delta = (delta_raw > HALF_ENCODER_CPR) ?
(delta_raw - ENCODER_CPR) :
(delta_raw + ENCODER_CPR);
}
ここで問題になってくるのが、本当に1ステップで半回転以上しないのだろうか？という点です。
M2006は最高rpmが500で減速比が36:1なので、計算すると、最大1msあたり500x36x0.001/60=0.3回転するという計算になります。
お、セーフ。とはなりません。
CANフレームがもれなく1msごとに送られてくる保障がどこにあるでしょうか？
今回の場合、最高速で動いていると1フレームでもロストしてしまった場合、もうそれが（逆側に）一回転しているのか、単に高速で動いているのかが判別がつきません。
実際にこの現象によって、最高速を500rpmになるように設定していた場合、著しいドリフトが発生してしまいました。
これは通信品質の問題もあるかと思いますが、安全率の観点からもよろしくありません。
そのため、ひとまずの対処法としては最大速度を389rpmにすることで、1msあたり最高0.23回転となり、1フレームロストした場合でも1ステップで半回転以上しないようにしました。
また、そもそも通信品質を改善するためにCANバスの帯域をできるだけ圧迫しないよう、CANバス1つにつきDJIモーターが4つまでになるよう回路構成を変更したり、物理的なCANバスの枝長に注意したりしました。
（単純計算ですが、DJIモーターフィードバック1つにつきおよそ100bitだとすると、指令用フレームも含め、100x5x1000=0.5Mbpsとなり、既に帯域の半分を占有していることがわかります。DJIモーターESCはclassicCANにしか対応していないので、CANbusにつなぐのは4つまでにしておいた方が良いです。）
以上の試行錯誤により、めでたく4輪独立ステアリング機構が完成しました。
実際のカタログスペックは以下のようになります。
- 直進最高速度：π x 469 x 0.084 / 60 = 2.06 m/s
- 回転速度：2π x π x 0.5 / 2.06 = 4.79 rad/s
- 応答性：あとで計算して埋める
最終調整後、実際に走ってる様子
＞今度ちゃんと撮る
Discussion