主权项 |
一种基于距离与视差信息的水下机器人避障方法,包括水下机器人、探测声呐、前向红外摄像头、侧向红外摄像头以及尾部红外摄像头,在水下机器人上安装探测声呐,在水下机器人的前端对称安装两个前向红外摄像头,在水下机器人顶部两侧位置分别安装侧向红外摄像头,在水下机器人尾部对称安装两个尾部红外摄像头,其特征在于,所述避障方法包括以下步骤:(1)根据水下机器人与目标点的位置,以及海图数据库信息离线规划出水下机器人的全局参考轨迹,设定如下代价函数使得水下机器人在无障碍情况下能够移动到目标点<maths num="0001" id="cmaths0001"><math><![CDATA[<mrow><mi>E</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mi>ω</mi><msup><mrow><mo>|</mo><mo>|</mo><mi>q</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>-</mo><msub><mi>q</mi><mn>0</mn></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>|</mo><mo>|</mo></mrow><mn>2</mn></msup><mo>+</mo><mi>υ</mi><msup><mrow><mo>|</mo><mo>|</mo><mover><mi>q</mi><mo>.</mo></mover><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>-</mo><msub><mover><mi>q</mi><mo>.</mo></mover><mn>0</mn></msub><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>|</mo><mo>|</mo></mrow><mn>2</mn></msup><mo>,</mo></mrow>]]></math><img file="FDA0000645311780000011.GIF" wi="927" he="109" /></maths>式中,ω>0与υ>0为比例因子;<img file="FDA0000645311780000012.GIF" wi="534" he="95" />为参考轨迹,q<sub>0</sub>(t)∈R<sup>3</sup>与<img file="FDA0000645311780000013.GIF" wi="233" he="94" />分别表示参考轨迹的位置与速度向量;所述水下机器人视为一个质量为m的质点,质点位置为水下机器人中心点,质点在空间坐标系中有三个自由度,即q(t)=[x(t),y(t),z(t)]<sup>T</sup>;通过反馈线性化,水下机器人的运动学方程可以用二阶积分器模型表示,其中速度与控制输入向量分别表示为<img file="FDA0000645311780000014.GIF" wi="504" he="89" />与u(t)=[u<sub>x</sub>(t),u<sub>y</sub>(t),u<sub>z</sub>(t)]<sup>T</sup>;控制输入约束表示为‑u<sub>xmin</sub>≤u<sub>x</sub>(t)≤u<sub>xmax</sub>、‑u<sub>ymin</sub>≤u<sub>y</sub>(t)≤u<sub>ymax</sub>与‑u<sub>zmin</sub>≤u<sub>z</sub>(t)≤u<sub>zmax</sub>,其中u<sub>xmin</sub>>0、u<sub>xmax</sub>>0、u<sub>ymin</sub>>0、u<sub>ymax</sub>>0、u<sub>zmin</sub>>0与u<sub>zmax</sub>>0为输入饱和常数;(2)探测声呐作为全局障碍测量仪器,监测水下机器人周边远距离环境信息,获取水下机器人与障碍物的相对距离信息;红外摄像头作为局部障碍测量仪器,监测水下机器人周边近距离环境信息,获取近距离障碍物的视角信息;(3)判断是否需要避障,如果需要避障,根据相对距离信息,利用预测控制调整避障策略,构造基于预测控制的优化算法;如果不需要避障则按照原路线行驶;(4)若水下机器人到达目标点,则轨迹规划结束,完成任务;若水下机器人未到达目标点,则重新执行步骤(2)。 |