[发明专利]面向变速曲率受限机器人的无碰撞覆盖路径规划方法

权利要求书

1.一种面向变速曲率受限机器人的无碰撞覆盖路径规划方法，其特征在于包括以下步骤：

第一步，构建面向变速曲率约束机器人的覆盖路径规划系统；覆盖路径规划系统由服务端、客户端和机器人终端组成；

客户端是一台移动终端、PC机或者机器人；客户端上有激光雷达、双目相机等探测传感器，客户端通过探测传感器获取目标区域环境信息，将目标区域环境信息抽象成一个地图G；G中值为0的元素代表障碍物，值为1的元素代表待覆盖网格；客户端将地图G和用户指定的速度采样间隔Inter发送给服务端；

服务端是一台服务器或PC机，其上安装有覆盖路径规划系统；覆盖路径规划系统与客户端和机器人终端相连；它由区域分解模块、风险场构建模块、点到点路径规划模块和序列生成模块组成；

区域分解模块与客户端的探测传感器、机器人终端、点到点路径规划模块和序列生成模块相连，接收客户端发送的目标区域地图信息G、用户指定的速度采样间隔Inter和机器人终端发送的机器人参数，根据机器人参数中的任务传感器覆盖半径r₁将目标区域进行分解，得到单元端点集合P，将单元端点集合P、目标区域地图信息G、用户指定的速度采样间隔Inter发送给点到点路径规划模块和序列生成模块；

风险场构建模块与区域分解模块、点到点路径规划模块和序列生成模块相连，接收区域分解模块发送的目标区域地图信息G和机器人参数，根据机器人参数中的机器人最小安全距离参数d_safe，构建出一个表征机器人碰撞风险的势能场PE，将PE发送给点到点路径规划模块和序列生成模块；

点到点路径规划模块与区域分解模块、风险场构建模块、序列生成模块相连，从区域分解模块接收单元端点集合P、网格地图G和用户指定的速度采样间隔Inter，从风险场构建模块接收势能场PE，从序列生成模块接收机器人参数，根据机器人参数中的机器人的前进速度、加速度、最大角速度，计算出机器人前进速度集合PS和单元到单元的无碰撞路径，进而得到一个表征路径代价的二维代价矩阵CM和一个存储候选路径的二维路径矩阵CPATH，并将CM和CPATH发送到序列生成模块；

序列生成模块与机器人终端、区域分解模块、风险场构建模块、点到点路径规划模块相连，从机器人终端接收机器人参数，从区域分解模块接收单元端点集合P、目标区域地图信息G、用户指定的速度采样间隔Inter，从风险场构建模块接收PE，从点到点路径规划模块接收二维代价矩阵CM和二维路径矩阵CPATH，将覆盖路径规划问题建模成一个旅行商问题，生成访问所有覆盖单元的无碰撞覆盖路径PATH，并将覆盖路径PATH发送给机器人终端；

机器人终端是一台真实的机器人，机器人终端装载有：一个雷达传感器，雷达传感器检测宽度为r₂的矩形区域内的障碍物，将检测到的障碍物信息发送到定位模块；一个用于执行覆盖任务的任务传感器，覆盖宽度为r₁的矩形区域；用于实时定位的定位模块；机器人路径跟随模块；其他机器人运行必备的软硬件控制模块，包含机器人固有的操作系统、底层驱动系统、运动控制系统；机器人终端将机器人的前进速度s、加速度a、最大角速度u_max、最小安全距离d_safe和r₁、r₂发送给服务端；

第二步，客户端的探测传感器采集目标区域环境信息，构建出目标环境的网格地图G，并将网格地图G发送给服务端的区域分解模块；令网格地图G中网格行数为A，网格列数为B；标区域环境信息包括目标区域的面积、边界、障碍物的位置；

第三步，服务端的区域分解模块接收客户端发送的网格地图G、用户指定的采样间隔Inter和机器人终端发送的任务传感器覆盖半径r₁，将目标区域进行分解，得到单元端点集合P，然后将单元端点集合P、网格地图G和用户指定的采样间隔Inter发送给点到点路径规划模块和序列生成模块；方法如下：

3.1区域分解模块采用Semi-BCD分解算法，根据网格地图G将目标区域划分为多个矩形单元；每个矩形单元中，位于x轴的边的宽度等于r₁，位于y轴的边与障碍物或者目标区域边界相交；单个矩形单元对应单个覆盖任务，所有矩形单元构成初始的覆盖任务集合C，C＝{c₁，...，c_n，...，c_N}，N为覆盖任务总数，N为正整数，c_n代表C中第n个覆盖任务；

3.2设定c_n中线与其上下两条边的两个交点，为c_n的上端点p_n，1和下端点p_n，2；p_n，1的坐标为(px，pu)，p_n，2的坐标为(px，pd)，其中px表示c_n中线的x轴坐标，pu表示c_n中线上方端点的y轴坐标，pd表示中线下方端点的y轴坐标；C中的N个单元，对应生成2N个端点{p_1，1，p_1，2，...，p_n，1，p_n，2，...，p_N，1，p_N，2}，其中p_n，1，p_n，2代表C中第n个单元的上下两个端点；为了描述方便，将p_n，1和下端点p_n，2进行简化，得到单元端点集合P＝{p₁，...，p_i，...，p_2N}，1≤i≤2N，p_i是{p_1，1，p_1，2，...，p_n，1，p_n，2，...，p_N，1，P_N，2}中的第i个端点；区域分解模块将单元端点集合P、网格地图G和用户指定的采样间隔Inter发送给点到点路径规划模块和序列生成模块；

第四步，服务端的风险场构建模块接收区域分解模块发送的目标区域网格地图G和机器人最小安全距离参数d_safe，构建出一个表征碰撞风险的风险势能场PE，将PE发送给点到点路径规划模块和序列生成模块；构建碰撞风险势能场PE方法是：

4.1风险场构建模块根据网格地图G构建风险势能场PE，PE是一个包含A×B个元素的二维矩阵，矩阵内的每个元素的值在0到1之间；矩阵元素值为1代表机器人在该元素处一定会发生碰撞风险，值为0代表机器人在该元素处是安全的；PE中元素值越大，代表机器人在该元素处与障碍物发生碰撞的可能性越大；

4.2风险场构建模块将PE发送给点对点路径规划模块和序列生成模块；

第五步，服务端的点到点路径规划模块接收区域分解模块发送的单元端点集合P，计算与P对应的代价矩阵CM和路径矩阵CPATH，方法是：

5.1初始化与P对应的代价矩阵CM和路径矩阵CPATH；代价矩阵CM是一个维度为2N×2N的二维矩阵，该矩阵里任意元素cm_d，e代表P中第d个端点p_d和第e个端点p_e之间的无碰撞路径的时间代价，d＝1，...，2N，e＝1，...，2N；路径矩阵CPATH是一个维度为2N×2N的二维矩阵，该矩阵里任意元素cpath_d，e代表p_d和p_e之间无碰撞路径的航迹点集合，满足cpath_d，e＝{(x_f，y_f，θ_f)，f＝1，...，F}，F为CPATH中航迹点的个数，为正整数，其中(x_f，y_f，θ_f)表示机器人在p_d和p_e之间无碰撞路径的第f个航迹点，x_f，y_f为机器人在第f个航迹点处的x/y轴坐标，θ_f为机器人在第f个航迹点处的朝向；初始化代价矩阵CM的所有元素为0，初始化CPATH中所有元素为空集，初始化行序号i为1，初始化列序号j为1；

5.2点到点路径规划模块接收区域分解模块发送的P、网格地图G和速度采样间隔Inter，接收序列生成模块发送的机器人的前进速度、加速度、最大角速度等机器人参数，计算出从p_d出发到达p_e的无碰撞路径best_path，及best_path的耗时T_min，令cpath_d，e＝best_path，令cm_d，e＝T_min；方法如下：

5.2.1点到点路径规划模块从区域分解模块接收P，根据p_d和p_e相对位置，得到机器人在p_d处的位姿信息(x_d，y_d，θ_d)和p_e处的位姿信息(x_e，y_e，θ_e)，x_d，y_d为机器人在p_d点处的x/y轴坐标，θ_d为机器人在p_d点处的朝向，x_e，y_e为机器人在p_e点处的x/y轴坐标，θ_e为机器人在p_e点处的朝向；

5.2.2点到点路径规划模块接收区域分解模块发送的机器人采样间隔Inter，序列生成模块发送的机器人加速度a、前进速度s和最大角速度u_max，a∈[a_min，a_max]，a_min≥0，a_max＞0，a_min＜a_max，其中a_min和a_max分别代表机器人最小和最大前进加速度；s∈[s_min，s_max]，s_min≥0，s_max＞0，s_min＜s_max，其中s_min和s_max分别代表机器人最小和最大前进速度；对机器人前进速度进行等间隔采样，得到前进速度集合S＝{s₁，...，s_k，...，s_K}，K是速度采样点个数，为正整数，其中s_k是S中的第k个速度采样点，s₁＝s_min，s_K＝s_max；S中任意两个速度构成一个速度对，S中的K个速度对应K²个速度对，这些速度对构成了前进速度对集合PS＝{(s_k1，s_k2)，s_k1，s_k2∈S，k1，k2＝1，..，K}；

5.2.3点到点路径规划模块接收风险场构建模块发送的风险势能场PE，计算出p_d和p_e之间的无碰撞路径best_path及该路径的耗时T_min；方法如下：

5.2.3.1初始化最小路径耗时T_min为一个正数TA，初始化最优路径best_path为空集，令序列号k1＝1，k2＝1；

5.2.3.2计算与速度对(s_k1，s_k2)条件下对应的无碰撞变速Dubins路径集合DP；DP中共有Dubins路径中的包含RSL、RSR、LSR、LSL、LRL、RLR六条路径，其中R代表右转弧线段，L代表左转弧线段，S代表直线段；DP＝{dp_r，r＝1，...，6}，dp_r为DP中的第r条路径，dp_r＝{(x_f，y_f，θ_f)，f＝1，...，F}，F为dp_r中导航点个数，F为正整数，(x_f，y_f，θ_f)为dp_r中的第f个导航点，x_f，y_f，θ_f分别表示机器人在该点处的x/y轴坐标和朝向；

5.2.3.3计算DP中各条路径对应的耗时集合DT，DT＝{dt_r，r＝1，...，6}，其中dt_r代表DT中的第r个元素，其值为路径dp_r的耗时；

5.2.3.4在DT中搜索值最小的元素DT_min，DT_min＝min(DT)；令DT_min为DT中第w个元素，1≤w≤6；如果DT_min＜T_min，令T_min＝DT_min，令最优路径best_path为DP中耗时最少的无碰撞路径dp_w，转5.2.3.5；否则，T_min保持原值，转5.2.3.5；

5.2.3.5如果k2≠B，令k2＝k2+1，转5.2.3.2；如果k2＝B，且k1≠A-1，令k2＝1，k1＝k1+1，转5.2.3.2；否则，说明已经得到p_d和p_e之间耗时最小的无碰撞路径best_path，令cpath_d，e＝best_path，cm_d，e＝T_min，转5.3；

5.3如果d＝2N且e＝2N，说明已经计算得到与P对应的代价矩阵CM和CPATH，转5.4；如果d＝2N，e≠2N，令d＝1，e＝e+1，转第5.2步；如果d≠2N，e≤2N，令d＝d+1，转第5.2步；

5.4点到点路径规划模块将CM和CPATH发送给序列生成模块，转第六步；

第六步，服务端的序列生成模块接收区域分解模块发送的单元端点集合P、点到点路径规划模块发送的代价矩阵CM和CPATH和机器人终端发送的机器人参数，构建覆盖路径规划数学模型，得到访问P中所有端点的代价最小的序列Tour，进而得到覆盖路径PATH；方法如下：

6.1初始化Tour为空集；

6.2序列生成模块接收点到点路径规划子模块发送的CM，构建覆盖路径规划模型，如公式(6)-(10)所示：

Min(∑cm_i，j*z_i，j)，i＝1，2，...，2N，j＝1，2，...，2N，i≠j    (6)

∑z_i，j＝2N    (7)

z_{2×n1+1，2×n1+2}+z_{2×n1+2，2×n1+1}＝1，n1＝1，...，N    (10)

其中自变量z_i，j表示机器人顺序访问端点p_i和p_j的布尔值；如果z_i，j＝1，表示机器人在访问端点p_i之后，再访问端点p_j；如果z_i，j＝0，，表示机器人在访问端点p_i之后，不会访问端点p_j；公式(6)表示求解访问P中所有端点的代价最小的回路，公式(7)表示Tour中包含2N条边，公式(8)表示机器人只能进入同一个端点一次，公式(9)表示机器人只能离开同一个端点一次，公式(10)表示机器人一定会顺序访问同一个矩形单元的两个端点；

6.2对公式(6)-(10)进行求解，得到访问P中所有端点的代价最小的序列Tour＝{t₁，...，t_n，...，t_2N}，其中t_n是Tour中的第n个端点；

6.3根据端点序列Tour和CPATH生成覆盖路径PATH；

6.4序列生成模块将覆盖路径PATH发送到机器人终端，转第七步；

第七步，机器人终端接收序列生子模块输出的覆盖路径PATH，基于机器人终端配置的路径跟随模块、定位模块和机器人运行软硬件控制模块，按照覆盖路径PATH一边运动一边执行任务，实现对目标区域的完全覆盖。