[發明專利]一種人體自動超聲掃描機器人、控制器及控制方法

有效

• 申請號: 201910665363.X
• 申請日: 2019-07-23
• 公開（公告）號: CN110488745B
• 公開（公告）日: 2020-09-29
• 發明（設計）人: 付莊;沈運;費健;謝榮理
• 申請（專利權）人: 上海交通大學
• 主分類號: G05B19/19
• 分類號: G05B19/19
• 代理公司: 上海恒慧知識產權代理事務所(特殊普通合伙) 31317
• 代理人: 徐紅銀;趙楠

1.一種控制器，用于控制機械臂所持超聲檢測探頭，其特征在于，所述控制器包括前饋控制器、位置控制器、力位混合控制器和姿態矯正器；其中：

所述前饋控制器，根據設定的路徑控制所述超聲檢測探頭移動至路徑的起始點，并在所述超聲檢測探頭與皮膚的接觸方向上沿趨向于預設接觸力的方向緩慢移動，直到所述超聲檢測探頭與皮膚到達預設接觸力，以及控制所述超聲檢測探頭沿著預定方向運動；所述前饋控制器輸出端連接所述位置控制器的輸入端，所述前饋控制器將運動方向的位移增量數據輸出到所述位置控制器；

所述姿態矯正器，在所述超聲檢測探頭沿路徑運動預設距離后，根據所述超聲檢測探頭的歷史移動軌跡和實時接觸的摩擦力對所述超聲檢測探頭姿態進行矯正，使所述超聲檢測探頭姿態符合皮膚外輪廓，驅使所述超聲檢測探頭的法向方向和皮膚外輪廓法線方向平行；所述姿態矯正器將參考摩擦力偏移量輸出到所述力位混合控制器；

所述力位混合控制器，根據所述姿態矯正器的結果，控制所述超聲檢測探頭與皮膚接觸方向的壓力恒定，以及運動方向的摩擦力在設定范圍內；所述力位混合控制器輸入端連接所述姿態矯正器的輸出端，輸出端連接所述位置控制器的輸入端，所述力位混合控制器將基于壓力和摩擦力求得的運動控制增量輸出到所述位置控制器，所述運動控制增量包括接觸方向位移增量和姿態變化角度增量；

所述位置控制器，輸入端分別連接所述前饋控制器、所述力位混合控制器的輸出端，輸出端連接所述機械臂，所述位置控制器根據所述前饋控制器的運動方向位移增量、所述力位混合控制器的接觸方向位移增量和姿態變化角度增量、以及機械臂結構參數反解到關節空間坐標，控制所述機械臂所持所述超聲檢測探頭保持接觸力恒定、摩擦力一定、姿態適應皮膚外輪廓運動。

2.根據權利要求1所述的一種控制器，其特征在于，所述控制器還包括安全監測模塊，所述安全監測模塊的輸出端連接所述力位混合控制器的輸入端，用于對所述超聲檢測探頭掃描過程進行關節電流監測、末端力監測和奇異點臨近監測，當其中一項或多項檢測出現異常，將結果反饋給所述力位混合控制器，所述力位混合控制器將自身運算關閉，并發送零增量信號給所述位置控制器驅使所述機械臂運動停止，實現緊急停止所述超聲檢測探頭的運動。

3.根據權利要求1所述的一種控制器，其特征在于，所述姿態矯正器，根據數個連續周期內的所述機械臂夾持所述超聲檢測探頭的末端夾持器的空間坐標擬合目標所述超聲檢測探頭姿態傾角，并與實際所述超聲檢測探頭姿態傾角比較，調整所述力位混合控制器的參考摩擦力大??；

所述力位混合控制器，根據掃描過程中的實際接觸力和實際摩擦力，通過比較所述實際接觸力與所述預設接觸力，調整所述超聲檢測探頭在接觸方向上的位移，通過比較所述實際摩擦力與所述參考摩擦力，調整所述超聲檢測探頭姿態傾角。

4.一種人體自動超聲掃描機器人，其特征在于，包括權利要求1-3任一項所述的控制器，還包括機器人本體、末端夾持器、力傳感器、關節角度傳感器，其中，

所述末端夾持器設置于所述機器人本體的機械臂上，用于夾持所述超聲檢測探頭；

所述力傳感器設置于所述機器人本體上，所述力傳感器的輸入端連接所述機械臂，輸出端連接所述力位混合控制器，用于測量掃描時所述超聲檢測探頭與人體表面皮膚之間的接觸力和摩擦力，反饋給所述力位混合控制器；

所述關節角度傳感器設置于所述機器人本體上，所述關節角度傳感器的輸入端連接所述機械臂，輸出端連接所述姿態矯正器的輸入端，用于采集所述機器人本體的實際關節空間坐標；

所述控制器，根據所述力傳感器、所述關節角度傳感器的測量結果，控制機械臂所持所述超聲檢測探頭保持接觸力恒定、摩擦力一定、姿態適應皮膚外輪廓運動，其中：所述前饋控制器用于控制所述超聲檢測探頭沿著預定方向運動；所述位置控制器用于控制所述機器人本體的機械臂運動；所述力位混合控制器用于控制所述超聲檢測探頭和人體皮膚表面接觸力恒定以及摩擦力在一定范圍內；所述姿態矯正器用于矯正所述超聲檢測探頭姿態使其更符合皮膚外輪廓。