| 主权项 |
一种基于突加度的障碍物躲避运动规划方法，其特征在于，包括如下步骤：1)设计基于突加度的障碍物躲避运动性能指标，生成二次型优化冗余度解析方案；其中基于突加度的障碍物躲避运动性能指标受约束于基于突加度的雅可比矩阵等式、基于突加度的障碍物躲避不等式、关节角度极限、关节速度极限、关节加速度极限和关节突加度极限，即最小化为<img file="FDA0000945387290000011.GIF" wi="219" he="62" />受约束于基于突加度的雅可比矩阵等式<img file="FDA0000945387290000012.GIF" wi="203" he="69" />基于突加度的障碍物躲避不等式<img file="FDA0000945387290000013.GIF" wi="451" he="71" />关节角度极限θ<sup>‑</sup>≤θ≤θ<sup>+</sup>、关节速度极限<img file="FDA0000945387290000014.GIF" wi="269" he="55" />关节加速度极限<img file="FDA0000945387290000015.GIF" wi="237" he="55" />和关节突加度极限<img file="FDA0000945387290000016.GIF" wi="278" he="63" />其中，<img file="FDA0000945387290000017.GIF" wi="196" he="62" />表示优化性能指标，<img file="FDA0000945387290000018.GIF" wi="40" he="55" />表示机械臂关节突加度，上标T表示矩阵和向量的转置，W为单位矩阵；等式约束<img file="FDA0000945387290000019.GIF" wi="170" he="75" />表示机械臂末端运动轨迹，J<sub>e</sub>表示机械臂末端执行器的雅可比矩阵，<img file="FDA00009453872900000110.GIF" wi="478" he="79" /><img file="FDA00009453872900000126.GIF" wi="45" he="45" />表示机械臂末端执行器突加度向量，<img file="FDA00009453872900000111.GIF" wi="60" he="79" />是J<sub>e</sub>的时间导数，<img file="FDA00009453872900000112.GIF" wi="59" he="79" />是<img file="FDA00009453872900000113.GIF" wi="54" he="78" />的时间导数，θ表示机械臂的关节角度，<img file="FDA00009453872900000114.GIF" wi="43" he="70" />表示机械臂的关节速度，<img file="FDA00009453872900000115.GIF" wi="41" he="71" />表示机械臂的关节加速度；<img file="FDA00009453872900000116.GIF" wi="419" he="71" />表示基于突加度的障碍物躲避不等式约束，J<sub>z</sub>、<img file="FDA00009453872900000117.GIF" wi="59" he="79" />和<img file="FDA00009453872900000118.GIF" wi="58" he="79" />分别定义如下：<img file="FDA00009453872900000119.GIF" wi="358" he="70" /><img file="FDA00009453872900000120.GIF" wi="382" he="71" /><img file="FDA00009453872900000121.GIF" wi="382" he="71" />其中，<img file="FDA00009453872900000122.GIF" wi="702" he="85" />(x<sub>c</sub>,y<sub>c</sub>,z<sub>c</sub>)和(x<sub>o</sub>,y<sub>o</sub>,z<sub>o</sub>)分别是判据点C和障碍物点O相对于机械臂基座的坐标，J<sub>c</sub>(θ)表示判据点C处的雅可比矩阵，<img file="FDA00009453872900000123.GIF" wi="142" he="86" />是J<sub>c</sub>(θ)的时间导数，<img file="FDA00009453872900000124.GIF" wi="141" he="85" />是<img file="FDA00009453872900000125.GIF" wi="139" he="83" />的时间导数，向量与矩阵乘法操作◇定义如下：s◇V＝[s<sub>1</sub>V<sub>1</sub>,s<sub>2</sub>V<sub>2</sub>,…s<sub>l</sub>V<sub>l</sub>]<sup>T</sup>其中，列向量s＝[s<sub>1</sub>,s<sub>2</sub>,…,s<sub>l</sub>]<sup>T</sup>，行向量V<sub>i</sub>表示矩阵V的第i行，<img file="FDA00009453872900000210.GIF" wi="298" he="55" />θ<sup>±</sup>表示关节角度上下限，<img file="FDA0000945387290000021.GIF" wi="67" he="62" />表示关节速度上下限，<img file="FDA0000945387290000022.GIF" wi="66" he="60" />表示关节加速度上下限，<img file="FDA0000945387290000023.GIF" wi="73" he="63" />表示关节突加度上下限；2)将步骤1)中所生成的基于突加度的障碍物躲避二次型优化冗余度解析方案转化为二次规划问题；3)将步骤2)中的二次规划问题运用二次规划求解器进行求解；4)将步骤3)的求解结果传递给下位机控制器驱动机械臂运动。 |
