スレッド間同期した一定時間間隔スレッドの作成

一定時間間隔の制御と制御同期入出力をFreeRTOSで実装したのでまとめ

やったこと

  • 一定時間間隔の入出力スレッド(IoThread)
  • IoThreadと制御スレッド(ControlThread)のセマフォ同期
  • ミューテックス排他制御によるIoThreadとControlThreadのリソース管理
  • キューによるControlThreadとPC通信スレッド(MainteThread)間通信

想定読者

  • (Free)RTOSに興味がある人
  • 組み込みで一定時間間隔制御が必要な人
続きを読む

C++とPython間の通信構造体

C++とPython間で同じ構造体を用いてTCP通信をしたかったのでまとめ（ROS最高！！！）

環境

macOS 11.6

C++

gcc 13.0.0
TCP通信部はlwipで実装

Python

Python 3.9.6
TCP通信部はsocketで実装
C++は標準の構造体，Pythonはctypesの構造体を使用

構造体定義

C++

#pragma pack(push, 1)
struct TcpHeader {
    uint32_t size;
    uint8_t type;
    TcpHeader(uint32_t size = 0, uint8_t type = 0) : 
        size(size),
        type(type) {}
};
struct PacketControlDataReq {
    TcpHeader header;
    uint8_t arm_id;
    ControlData control_data;
    PacketControlDataReq() : 
        header(sizeof(PacketControlDataReq), MAINTE_TO_ROBOT_CONTROL_DATA) {}
};
struct PacketRobotInfoReq {
    TcpHeader header;
    uint8_t num_arm;
    uint8_t num_joint;
    PacketRobotInfoReq() : 
        header(sizeof(PacketRobotInfoReq), ROBOT_TO_MAINTE_ROBOT_INFO),
        num_arm(ArmId::NUM_ARM),
        num_joint(NUM_JOINT) {}
};
#pragma pack(pop)

Python

class TcpHeader(Structure):
    _pack_ = 1
    _fields_ = [
        ('size', c_uint32),
        ('type', c_uint8)
    ]
    def __init__(self, size = 0, type = 0):
        self.size = size
        self.type = type
class PacketControlDataReq(Structure):
    _pack_ = 1
    _fields_ = [
        ('header', TcpHeader),
        ('arm_id', c_uint8),
        ('control_data', ControlData)
    ]
    def __init__(self):
        self.header = TcpHeader(sizeof(PacketControlDataReq), PacketType.MAINTE_TO_ROBOT_CONTROL_DATA)
class PacketArmInfoReq(Structure):
    _pack_ = 1
    _fields_ = [
        ('header', TcpHeader),
        ('num_arm', c_uint8),
        ('num_joint', c_uint8)
    ]
    def __init__(self):
        self.header = TcpHeader(sizeof(PacketArmInfoReq), PacketType.ROBOT_TO_MAINTE_ARM_INFO)
        self.num_arm = 0
        self.num_joint = 0

C++ >> Python

送信側(C++)

PacketRobotInfoReq robot_info_req;
int err = send(sock, &robot_info_req, sizeof(PacketRobotInfoReq), 0);

受信側(python)

arm_info = PacketArmInfoReq()
memmove(addressof(arm_info), data, sizeof(arm_info))
print("num_arm = {}, num_joint = {}".format(arm_info.num_arm, arm_info.num_joint))

Python >> C++

送信側(python)

control_data = PacketControlDataReq()
client.send(control_data)

受信側(C++)

std::array<char, 2048> rx_buffer;
int len = recv(sock, rx_buffer.data(), rx_buffer.size(), 0);
PacketControlDataReq control_data_req;
memmove(&control_data_req, rx_buffer.data(), sizeof(control_data_req));
printf("arm_id = %d", control_data_req.arm_id);

外乱抑制について備忘録

3Dプリンタが使えないのでpython-controlで遊んでみました
Python Control Systems Library — Python Control Systems Library dev documentation
FF制御のみ，PIDによるFB制御のみ，FFとPIDを組み合わせた制御(FF+FB)，外乱オブザーバーを追加した制御(FF+FB+EX)を比較
f:id:Libra23:20210612132251p:plain
外乱オブザーバーを追加した制御が収束が早く，外乱を抑制できている（ゲインは統一）
ソース
robot-ws/system.py at master · Libra23/robot-ws · GitHub

環境

macOS Big Sur : 11.4
Python : 3.9.5

  • control==0.9.0
  • matplotlib==3.4.2
  • numpy==1.20.3
  • PyYAML==5.4.1
  • scipy==1.6.3

外乱オブザーバーフィードバックによる外乱抑制

外乱オブザーバーとは

簡単に言うとプラントの出力から入力を推定して，実際の入力との差から外乱を推定する
外乱オブザーバの紹介 - Qiita

ブロック線図

フィードフォワード制御とPID制御に外乱オブザーバー補償を追加したコントローラでストップ応答を確認する
ブロック線図は下図
f:id:Libra23:20210612093145j:plain
パラメータ

  • R : 入力
  • Y : 出力
  • D : 外乱
  • D_est : 推定外乱
  • FF : フィードフォワードコントローラ
  • PID : PIDコントローラ
  • T : FFとPIDの出力
  • P : プラント
  • P_n^-1 : 逆プラントモデル
  • Q :フィルタ
  • F : フィルタ
外乱オブザーバーの伝達関数

TとDを入力，出力をYとなるように伝達関数を求める
推定外乱について
D_{est} = Q(P_n^{-1}Y - (T - D_{est}))
D_{est} = \frac{Q}{1 - Q}(P_n^{-1}Y - T)
出力について
Y = P(T - D_{est} + D)
上式を整理すると下記の伝達関数が得られる
Y = \frac{P}{1 - Q + QPP_n^{-1}}T + \frac{P(1 - Q)}{1 - Q + QPP_n^{-1}}D
PP_n^{-1} = 1が成り立つ時， Y = PT + P(1 - Q)Dであるから外乱と外乱推定値は下記式を満たす
D_{est} = \frac{Q}{1 - Q}(P_n^{-1}Y - T) = QD
以上より，外乱を推定できることが分かる

システムの伝達関数

f:id:Libra23:20210612131035j:plain
外乱オブザーバーの伝達関数を簡単のため係数A，Bを用いて示す
A = \frac{P}{1 - Q + QPP_n^{-1}}, B = \frac{P(1 - Q)}{1 - Q + QPP_n^{-1}}
RとDを入力，出力をYとなるように伝達関数を求める
Y \frac{A(FF + PID)}{1 + A PID}R + \frac{B}{1 + A PID}D
ただしF=1としている

自重補償バネを設計する

週一ブログチャレンジはやっていませんが，先週に引き続きロボットの脚について
この脚の膝部分にはばねが入っていて，自重補償に良さそうなばねを選定する（摩擦が支配的な気もするけど
f:id:Libra23:20210528204936j:plain

ばねのポテンシャルエネルギを追加
E_p = -m_1gl_{g1}cos(q_1)-m_2gl_1cos(q_1)-m_2gl_{g2}cos(q_1+q_2)+\frac{1}{2}k_2q_2^2
ラグランジュの運動方程式
M\begin{pmatrix} \ddot{q_1} \\ \ddot{q_2} \end{pmatrix}+h(q, \dot{q})+g(q)=\begin{pmatrix} \tau_1 \\ \tau_2 \end{pmatrix}

M = \begin{pmatrix} m_1l_{g1}^2+m_2(l_1^2+2l_1l_{g2}cos(q_2)+l_{g2}^2)+I_1+I_2 & m_2(l_1l_{g2}cos(q_2)+l_{g2}^2)+I_2 \\ m_2(l_1l_{g2}cos(q_2)+l_{g2}^2)+I_2 & m_2l_{g2}^2+I_2 \\ \end{pmatrix}
h(q, \dot{q}) = \begin{pmatrix} -m_2l_1l_{g2}(2\dot{q_1}+\dot{q2})sin(q_2)\dot{q_2} \\ m_2l_1l_{g2}\dot{q_1}^2sin(q_2)+k_2q_2 \end{pmatrix}
g(q) = \begin{pmatrix} m_1gl_{g1}sin(q_1)+m_2g(l_1sin(q_1)+l_{g2}sin(q_1+q_2)) \\ m_2gl_{g2}sin(q_1+q_2) \end{pmatrix}

ボディ全体の質量をmとして，脚が設置した状態で加速度a = 0と仮定してボディを上下させる
k_2=0の場合
横軸が関節角度 q_2 [rad.]，縦軸が必要トルク \tau_2 [N mm]（真ん中のニョロニョロは移動開始時のスムージングの跡なので無視）
f:id:Libra23:20210528202513p:plain
k_2=100 [N mm/rad.]の場合（真ん中のニョロニョロは，，，
f:id:Libra23:20210528202922p:plain

必要なトルクを1/6程度まで減らすことができる
今回はばね定数1.8 [N mm/deg.]のねじりスプリングをモノタロウさんで購入
線径 1Φmm、ばね定数(N・mm[deg]) 1.8、33-0834、1袋(2個) - ねじりスプリング 1袋(2個) サミニ 【通販モノタロウ】 07500115

歩行する場合は常に設置しているわけではないので，脚の本数と歩容から妥当な値を設定するのが良さそう

ロボットがジャンプできるか動力学計算する

勢いで書いたので低クオリティ！！
開発に関係ないところが忙しくて，今度ちゃんとまとめよう
運動方程式は間違っていないとおもうけど不備はありそうなので，参考程度に，，，

ジャンプできるか検証するロボット
f:id:Libra23:20210520203434j:plain

続きを読む

ESP32の開発環境を整える

開発規模が大きくなったので，Arduino for VSCodeからEPS-IDFに移行
逆運動学計算に時間がかかりすぎて，ゲイン下げないと振動するから複スレッドにしたい

やったこと

  • ESP32をEPS-IDFで開発
  • CMakeでのビルドに挑戦
  • ESP32で本家Eigen（全く情報がなくて困った，，，）
  • GoogleTestを用いたアルゴリズムの単体テスト

想定読者

C++，CMake，単体テスト(gtest)，git submoduleに関心がある人かな
非線形カルマンフィルタとか追加しているけど，単体テスト参照
GitHub - Libra23/robot-ws
g++でビルドすると単体テストが可能で，同じソースでesp-idfでビルドするとesp32用バイナリができる

続きを読む

特異点低感度行列を用いた逆運動学　アルゴリズム編

運動学周りを一度ちゃんとまとめよう
Arduino(ESP32 only)と標準的なC++環境で併用できるKinematicライブラリを作っておきましたー
github.com
ESP32のコンパイルオプションで別れているので，コピペでPC環境でも使える

ESP32だと下記の線形代数ライブラリが必要
GitHub - tomstewart89/BasicLinearAlgebra: A library for using matrices and linear algebra on Arduino

Standard C++だとEigenが必要＋Kinematicライブラリ内のAffine3d.cpp & Affine3d.hppを削除（アフィン変換）

続きを読む